#include <sstream>
// WiFi configuration. Sorry, no station mode... for this version.
#include <wiif_psk.h>
// Self signed certificates
#include "cert.h"
#include "private_key.h"


#include <SPIFFS.h>
#include <FS.h>
// We will use wifi
#include <WiFi.h>
// Includes for the server
#include <HTTPSServer.hpp>
#include <SSLCert.hpp>
#include <HTTPRequest.hpp>
#include <HTTPResponse.hpp>
#include <WebsocketHandler.hpp>
#include <ESPmDNS.h>

#include <camera.h>

//50 PWM Hz for servos, 16bit resolution
#define pwmFreq 50
#define pwmRes 16
//motor and PWM channel config
//min and max duty cycle
#define motorAPin 12
#define motorA 2
#define dcMinA 4070
//center: 4820
//center: 4470
#define dcMaxA 5570
#define motorBPin 13
#define motorB 3
#define dcMinB 4000
#define dcMaxB 5600
#define motorCPin 15
#define motorC 4
#define dcMinC 4000
#define dcMaxC 5600
#define motorDPin 14
#define motorD 5
#define dcMinD 4062
//center: 4812
#define dcMaxD 5562

//camera setup
Camera camera;

TimerHandle_t watchdog;
int watchdogTimerId = 1;
 //300 ms to stop motors
 #define wdtInterval 300

// The HTTPS Server comes in a separate namespace. For easier use, include it here.
using namespace httpsserver;

// Create an SSL certificate object from the files included above ...
SSLCert cert = SSLCert(
  example_crt_DER, example_crt_DER_len,
  example_key_DER, example_key_DER_len
);
// ... and create a server based on this certificate.
// The constructor has some optional parameters like the TCP port that should be used
// and the max client count. For simplicity, we use a fixed amount of clients that is bound
// to the max client count.
HTTPSServer secureServer = HTTPSServer(&cert, 443, 1);

// Declare some handler functions for the various URLs on the server
// The signature is always the same for those functions. They get two parameters,
// which are pointers to the request data (read request body, headers, ...) and
// to the response data (write response, set status code, ...)
void handleRoot(HTTPRequest * req, HTTPResponse * res);
void handle404(HTTPRequest * req, HTTPResponse * res);
void fileServer(HTTPRequest * req, HTTPResponse * res);
// As websockets are more complex, they need a custom class that is derived from WebsocketHandler
class wsHandler : public WebsocketHandler {
public:
  // This method is called by the webserver to instantiate a new handler for each
  // client that connects to the websocket endpoint
  static WebsocketHandler* create();

  // This method is called when a message arrives
  void onMessage(WebsocketInputStreambuf * input);

  // Handler function on connection close
  void onClose();
};

//timeout handler
void watchdogTimeout(TimerHandle_t xTimer) {
    ledcWrite(motorA,0);
    ledcWrite(motorB,0);
    ledcWrite(motorC,0);
    ledcWrite(motorD,0);
    xTimerReset(watchdog, 10);
}


// Simple array to store the active clients:
wsHandler* wsClient = nullptr;

/**
 * @brief init the hardware, start the servers
 * 
 */
void setup() {
  delay(500);

  // For logging
  Serial.begin(115200);
  
  Serial.println("Starting motor control.");
  ledcSetup(motorA, pwmFreq, pwmRes);
  ledcSetup(motorB, pwmFreq, pwmRes);
  ledcSetup(motorC, pwmFreq, pwmRes);
  ledcSetup(motorD, pwmFreq, pwmRes);
  ledcAttachPin(motorAPin, motorA);
  ledcAttachPin(motorBPin, motorB);
  ledcAttachPin(motorCPin, motorC);
  ledcAttachPin(motorDPin, motorD);
  ledcWrite(motorA,map(127, 0, 255, dcMinA, dcMaxA));
  ledcWrite(motorB,map(127, 0, 255, dcMinB, dcMaxB));
  ledcWrite(motorC,map(127, 0, 255, dcMinC, dcMaxC));
  ledcWrite(motorD,map(127, 0, 255, dcMinD, dcMaxD));

  Serial.println("Starting Filesystem..");
  if(!SPIFFS.begin(true)) {
    Serial.println("Unable to open filesystem");
    return;
  } else {
    Serial.println("Filesystem ready");
  }

  Serial.println("Starting camera1...");
  if(camera.init(FRAMESIZE_UXGA, 25) == false) {
    Serial.println("camera failed!");
  } else {
    camera.setWindow(0, 0, 1600, 1200, 254, 90, 1024, 1024, 1024, 1024, false, false);
    camera.set_ae_level(64);
    delay(500);
  }
  
 // Connect to WiFi
  Serial.println("Setting up WiFi");
  WiFi.begin(WIFI_SSID, WIFI_PSK);
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(500);
  }
  Serial.print("Connected. IP=");
  Serial.println(WiFi.localIP());
  Serial.println("starting mDNS...");
  if(!MDNS.begin("panobot")) {
     Serial.println("mDNS failed!");
  }

  
  // The websocket handler can be linked to the server by using a WebsocketNode:
  // (Note that the standard defines GET as the only allowed method here,
  // so you do not need to pass it explicitly)
  WebsocketNode * camNode = new WebsocketNode("/ws", &wsHandler::create);
  // Adding the node to the server works in the same way as for all other nodes
  secureServer.registerNode(camNode);

  //the web server, handles all the https GET requests
  ResourceNode * defaultHandler = new ResourceNode("", "GET", &fileServer);
  secureServer.setDefaultNode(defaultHandler);

  Serial.println("Starting server...");
  secureServer.start();
  if (secureServer.isRunning()) {
    Serial.print("Server ready. https://");
    Serial.println(WiFi.localIP());
  }
  Serial.println("Starting motor timeouts");
  watchdog = xTimerCreate("wdt", pdMS_TO_TICKS(wdtInterval), pdFALSE, (void *)watchdogTimerId, &watchdogTimeout);
  if(xTimerStart(watchdog,50) != pdPASS) {
    Serial.println("failed to start watchdog timer");
  }

}

/**
 * @brief The main loop
 * 
 */
void loop() {
  //so this is a stoooooopid bugfix.
  //it's basically fake multitasking.
  secureServer.loop();
  secureServer.loop();
  secureServer.loop();
  if (wsClient != nullptr) {
      wsClient->send(camera.run(),camera.getBufferLen());    
  }
  secureServer.loop();
  secureServer.loop();
  secureServer.loop();
  
}

/**
 * @brief websocket connect event handler
 * 
 * @return WebsocketHandler* 
 */
WebsocketHandler * wsHandler::create() {
  Serial.println("Client connected!");
  wsHandler * handler = new wsHandler();
  wsClient = handler;
  return handler;
}

/**
 * @brief websocket disconnect handler
 * 
 */
void wsHandler::onClose() {
  wsClient = nullptr;
}

/**
 * @brief websocket message handler. Controls the motors.
 * 
 * @param inbuf 
 */
void wsHandler::onMessage(WebsocketInputStreambuf * inbuf) {
  // Get the input message
  std::ostringstream ss;
  std::string msg;
  ss << inbuf;
  msg = ss.str();
  xTimerReset(watchdog,10);
  //front left
  ledcWrite(motorA, map(msg[0],0,255,dcMinA,dcMaxA));
  //front right, reversed
  ledcWrite(motorB, map(msg[1],0,255,dcMaxB,dcMinB));
  //back left
  ledcWrite(motorC, map(msg[2],0,255,dcMinC,dcMaxC));
  //back right, reversed
  ledcWrite(motorD, map(msg[3],0,255,dcMaxD,dcMinD));
}

/*
Super insecure SPIFFS file server. It's super slow, too!
*/
void fileServer(HTTPRequest * req, HTTPResponse * res) {
  Serial.println("processing request...");
  Serial.println(req->getRequestString().c_str());
  File file = SPIFFS.open(req->getRequestString().c_str());
  uint8_t *buff = new uint8_t[1024];
  req->discardRequestBody();
  if(!file || file.isDirectory()) {
    file = SPIFFS.open("/index.html");
  }
  res->setStatusCode(200);
  size_t fileLen = file.size();
  while(fileLen) {
    size_t buffSize = fileLen;
    if(buffSize > 1024) {
      buffSize = 1024;
    }
    file.read(buff, buffSize);
    res->write(buff, buffSize);
    fileLen -= buffSize;
  }
  file.close();
  Serial.println("file flushed.");
}
