#include <Arduino.h>

#ifdef WITH_DEBUGGING_ON
#include <GDBStub.h> // debugging support via GDBStub over UART
#endif

#include "PCA9685.h"
extern "C" { 
  #include "user_interface.h" 
}
// OTA & WEB
#include "ota.h"
#include "wifi_credentials.h"
#include "httpserver.h"

// BOARD
#define ESP12_LED 2
#define NODEMCU_LED 16

// PWM
#include "pwm.h"
os_timer_t timer1;
uint8_t timer_flag = 0;
Treppe stairs(13);

// WIFI
const char* ssid = STASSID;
const char* password = STAPSK;


void timerCallback(void *pArg)
{
  *((int *) pArg) += 1;
  stairs.task();
}

// HTTP
void handleNotFound();
HTTPServer httpServer(80, "/");

uint32_t _t=0;
#define SP_US(_str,_a) Serial.print(_str); Serial.print(" took: "); Serial.print(_a); Serial.println("us")
#define TIMEIF_US(_f, _l, _str) _t=micros(); _f; _t=micros()-_t; if(_t > _l) { SP_US(_str, _t); }
// ===============================================


void handleNotFound() {
  String message = "File Not Found\n\n";
  message += "URI: ";
  message += httpServer.uri();
  message += "\nMethod: ";
  message += (httpServer.method() == HTTP_GET) ? "GET" : "POST";
  message += "\nArguments: ";
  message += httpServer.args();
  message += "\n";

  for (uint8_t i = 0; i < httpServer.args(); i++) {
    message += " " + httpServer.argName(i) + ": " + httpServer.arg(i) + "\n";
  }

  httpServer.send(404, "text/plain", message);
}

void setup() {
#ifdef WITH_DEBUGGING_ON
  Serial.begin(460800);
  gdbstub_init();
#else
  Serial.begin(115200);
#endif

  Serial.println(F("Booting ...."));

  pinMode(NODEMCU_LED, OUTPUT);
  pinMode(ESP12_LED, OUTPUT);

  Wire.begin();                       // Wire must be started first
  Wire.setClock(1000000);              // Supported baud rates are 100kHz, 400kHz, and 1000kHz

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);

  while (WiFi.waitForConnectResult() != WL_CONNECTED) {
    Serial.println("Connection Failed! Rebooting...");
    delay(5000);
    ESP.restart();
  }

  Serial.println("");
  Serial.print("Connected to ");
  Serial.println(ssid);
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());

  ota_setup();

  httpServer.start();
  httpServer.onNotFound(handleNotFound);
  Serial.println("HTTP server started !");

  stairs.setup();
  Serial.println("PCA9685 connected !");

  os_timer_setfn(&timer1, timerCallback, &timer_flag);
  os_timer_arm(&timer1, 20, true);

  stairs.setState(1);
  stairs.setDirection(1);
}

void loop() {
  if(millis() > 25000 && stairs.getState() == 1 && stairs.getDirection() == 1) stairs.setState(0);
  if(millis() > 45000 && stairs.getDirection() == 1){
    stairs.setState(1);
    stairs.setDirection(0);
  }
  TIMEIF_US(ArduinoOTA.handle(), 1000, "OTA");
  TIMEIF_US(httpServer.handleClient(), 1000, "HTTP");
}