#include <WiFi.h>
#include <WiFiClientSecure.h>
#include <esp_wifi.h>

#include <WebSocketsServer.h>

const char* ssid = "Baitboat";//AP ssid
const char* password = "Password";//AP password

WebSocketsServer webSocket = WebSocketsServer(81);

const byte numCharsPS3 = 40;
char receivedCharsPS3[numCharsPS3];   // Lijst om ontvangen Seriële data in op te slaan

boolean verbinding;
boolean newDataPS3 = false;

TaskHandle_t Task1;
uint8_t espClient;

void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length) {
    espClient = num;
    switch(type) {
        case WStype_DISCONNECTED:
            verbinding = false;
            Serial.printf("[%u] Verbinding verbroken!\n", num);
            digitalWrite(2, LOW);
            break;
        case WStype_CONNECTED:
            {
                verbinding = true;
                IPAddress ip = webSocket.remoteIP(num);
                Serial.printf("Connectie van %d.%d.%d.%d\n", ip[0], ip[1], ip[2], ip[3]);
                webSocket.sendTXT(num, "Geconnecteerd!");
                digitalWrite(2, HIGH);
            }
            break;
        case WStype_TEXT:
            Serial.printf("[%u] Ontvangen tekst: %s\n", num, payload);
            break;
    }
}

void setup() {
  Serial.begin(115200);
  Serial.println("Hoofd-serial started!");
  Serial2.begin(115200, SERIAL_8N1, 16, 17);
  Serial.println("Serial1 started!");
  pinMode(2, OUTPUT);

  Serial.println("Ontwikkelen van kern 0 ...");
  xTaskCreatePinnedToCore(
                    Task1code,   /* Task function. */
                    "Task1",     /* name of task. */
                    10000,       /* Stack size of task */
                    NULL,        /* parameter of the task */
                    1,           /* priority of the task */
                    &Task1,      /* Task handle to keep track of created task */
                    0);
  Serial.println("Kern 0 succesvol ontwikkeld!");
}

//Code in kern 0
void Task1code( void * pvParameters ){
  Serial.println("Opstarten van WebSocket-server ...");
  WiFi.mode( WIFI_AP );
  delay(200);
  esp_wifi_set_protocol( WIFI_IF_AP, WIFI_PROTOCOL_LR );
  delay(200);
  WiFi.softAP(ssid, password);
  delay(200);
  Serial.println( WiFi.softAPIP() );
  delay(500);

  webSocket.begin();
  webSocket.onEvent(webSocketEvent);
  Serial.println("WebSocket-server succesvol gestart!");//for debug
  for(;;){
    webSocket.loop();
    vTaskDelay(1000 / 10);
  }
}

void loop() {
    serialDataPS3();
    shownewDataPS3();
}

//Functies voor zo snel/efficient mogelijke Seriële communicatie. Bron: arduino.cc
void serialDataPS3() {
    static byte ndx = 0;
    char endMarker = '\n';
    char rc;
    while (Serial2.available() > 0 && newDataPS3 == false) {
        rc = Serial2.read();
        if (rc != endMarker) {
            receivedCharsPS3[ndx] = rc;
            ndx++;
            if (ndx >= numCharsPS3) {
                ndx = numCharsPS3 - 1;
            }
        }
        else {
            receivedCharsPS3[ndx] = '\0'; // terminate the string
            ndx = 0;
            newDataPS3 = true;
        }
    }
}

void shownewDataPS3() {
    if (newDataPS3 == true) {
        PS3Data();
        newDataPS3 = false;
    }
}

void PS3Data(){
  String text = receivedCharsPS3;
  if (text.substring(0,4) == "PS3>") {
    text.remove(0,4);
    /*if (text.substring(0,7) == "MotorL>"){
      text.remove(0,7);
      Serial.println(text);
      webSocket.sendTXT(espClient, text);
    }
    else if (text.substring(0,7) == "MotorR>"){
      text.remove(0,7);
      Serial.println(text);
    }
    else {
      Serial.println(text);
    }*/
    webSocket.sendTXT(espClient, text);
  }
  else {
    Serial.println("Geen match!");
    Serial.println(text.substring(0,4));
  }
}