//#include <Arduino.h>
#ifdef ESP32
  #include <WiFi.h>
#else
  #include <ESP8266WiFi.h>
#endif
  //#include <ESP8266WiFi.h>
#include <WiFiClient.h>
  //#include <Servo.h>
#include "minikame.h"
#include <ArduinoOTA.h>
#include <TelnetStream.h>

// Pin Definitions
const byte PWR_ROPE = 33;

// Wifi Access Point configuration (default)
const char* DEFAULT_AP_SSID = CFG_DEFAULT_AP_SSID;
const char* DEFAULT_AP_PASS = CFG_DEFAULT_AP_PASS;

// OTA configuration
const char* OTA_HOST = CFG_OTA_HOST;
const char* OTA_PASS = CFG_OTA_PASS;
const char* OTA_MD5H = CFG_OTA_MD5H;

// temp (till NVS is implemented)
const char* ssid = CFG_SSID;
const char* pass = CFG_PASS;

void parseData(String data);

MiniKame robot;
WiFiServer server(80);
bool running = 0;
bool calibrating = 0;
String input;
WiFiClient *pClient = 0;
IPAddress IP;

void initPower()
{
    // set up pin to hold Ryoga driver gate high
    //  in order to keep power on
    
    pinMode(PWR_ROPE, OUTPUT);
    digitalWrite(PWR_ROPE, HIGH);
} // end initPower


void initWifi()
{
    // try and connect to a known wifi network
    Serial.print(F("Attempting connection to "));
    Serial.print(ssid); Serial.print(F("... "));
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, pass);

    // if connection to known wifi failed...
    if (WiFi.waitForConnectResult() != WL_CONNECTED)
    {
        Serial.println(F("FAILED"));
        
        // ...create own AP using default setup
        Serial.print(F("Creating default AP: ")); 
        Serial.println(DEFAULT_AP_SSID);
        WiFi.mode(WIFI_AP);
        WiFi.softAP(DEFAULT_AP_SSID, DEFAULT_AP_PASS);
        IP = WiFi.softAPIP();
    } 
    else 
    {
        Serial.println(F("OK"));
        IP = WiFi.localIP();
    }

    Serial.print(F("Robot on IP address: "));
    Serial.println(IP);

    Serial.print(F("Switching to wireless debugging in 3 seconds"));
    TelnetStream.begin();
    for (int i=0; i < 3; i++)
    {
        Serial.print(".");
        delay(1000);
    }
    Serial.println(F("\n\n\rBye bye!"));
} // end initWifi

// ----------------------- OTA handler functions ------------------------

void OTA_onStart()
{
    TelnetStream.print("Receiving new ");
    if (ArduinoOTA.getCommand() == U_FLASH)
    {
      TelnetStream.println("program...");
    }
    else // U_SPIFFS
    {
      TelnetStream.println("file data...");
      // NOTE: when updating SPIFFS, unmount SPIFFS here using SPIFFS.end()
    }
} // end OTA_onStart

void OTA_onEnd()
{
    TelnetStream.println("\n\r.\n\r.\n\r.\n\rI'm different...\n\r.\n\r.\n\r.");
} // end OTA_onEnd

void OTA_onProgress(unsigned int progress, unsigned int total)
{
    // Note: \r keeps this all on one line (ideally stop other text outputs)
    TelnetStream.printf("Progress: %u%%\r", progress / (total / 100) );
} // end OTA_onProgress

void OTA_onError(ota_error_t error)
{
    TelnetStream.printf("Error[%u]: ", error);
    if      (error == OTA_AUTH_ERROR)     TelnetStream.println("Auth Failed");
    else if (error == OTA_BEGIN_ERROR)    TelnetStream.println("Begin Failed");
    else if (error == OTA_CONNECT_ERROR)  TelnetStream.println("Connect Failed");
    else if (error == OTA_RECEIVE_ERROR)  TelnetStream.println("Receive Failed");
    else if (error == OTA_END_ERROR)      TelnetStream.println("End Failed");
} // end OTA_onError

void OTA_handlerTask(void *parameter)
{
    // to avoid error: 'E (4725) FreeRTOS: FreeRTOS Task "OTA_HANDLE" 
    //  should not return, Aborting now!'
    while (true)    
    {
        ArduinoOTA.handle();
        delay(5500);  
        // Note: error above 5.5 to 10.5+ seconds... so guessing 10 secs max
    }
} // end OTA_handlerTask

// ----------------- end OTA handler functions ---------------------


void initOTA()
{
    // setup a unique hostname for OTA
    const int MAXLEN = 40;
    char fullhostname[MAXLEN];
    uint8_t MAC[6];

    WiFi.macAddress(MAC);
    snprintf
    (
        fullhostname, MAXLEN, "%s-%02x%02x%02x", 
        OTA_HOST, MAC[3], MAC[4], MAC[5]
    );
    TelnetStream.print("OTA Hostname is: ");
    TelnetStream.println(fullhostname);
    ArduinoOTA.setHostname(fullhostname);
  
    // Port defaults to 3232
    // ArduinoOTA.setPort(3232);  
    //  (Use 8266 port if you are working in Sloeber 
    //   IDE, it is fixed there and not adjustable)

    // No authentication by default
    // ArduinoOTA.setPassword(OTA_PASS);
  
    // Password can be set with its MD5 value as well
    // MD5(OTA_PASS) = "4e229053c865b858b3f9a77274de18b7"
    // ArduinoOTA.setPasswordHash(OTA_MD5H);

    // setup handler functions
    ArduinoOTA
        .onStart(OTA_onStart)
        .onEnd(OTA_onEnd)
        .onProgress(OTA_onProgress)
        .onError(OTA_onError);

    ArduinoOTA.begin();
    TelnetStream.println("OTA Initialised");

    // create a task (thread) for RTOS to handle checking for OTA updates
    TelnetStream.println("Creating RTOS task for handling OTA updates");
    xTaskCreate
    (
        OTA_handlerTask,    /* Task function */
        "OTA_HANDLE",       /* String with name of task */
        10000,              /* Stack size in bytes */
        NULL,               /* Parameter passed as input of the task */
        1,                  /* Priority of the task */
        NULL                /* Task handle */
    );               
} // end initOTA()


void initControl()
{
    // start Wifi based control interface
    TelnetStream.println("Starting wifi control interface");
    server.begin();

    // start other control interface
    // ...BLE?...
    
} // end initControl


void setup() 
{
    // start debugging output
    Serial.begin(115200);
      //while (!Serial);  // DEBUG - TODO remove later! (may not be needed)
    Serial.println(F("\nHello there! :)\n"));
    
    initPower();
    initWifi();
    initOTA();
    initControl();
    robot.init();   
} // end setup

#ifndef WOKWI_SIMULATOR
void loop() 
{
    WiFiClient client = server.available();
    pClient = &client;  // hack
    
    if (!client) 
    {
        // repeat advertise IP address on serial connection
        // until client connects      
        Serial.print(F("Robot on IP address: "));
        Serial.println(IP);
        
        delay(1000);
    }
    
    while (client.connected()) 
    {
        if (running)
        {
                //Serial.println("running");
            
            if (client.available()) 
            {
                while(client.available())
                {
                    input = client.readStringUntil('+');
                }
                parseData(input);
            }
            else 
            {
                Serial.println("Keep 'Moving'...");
                
                parseData(input);
            }

            // think above two parseData(input); can move here
        }
        else
        {
                //Serial.println("Normal mode");
            
            if (client.available()) 
            {
                while(client.available()) 
                {
                  input = client.readStringUntil('+');
                }
                parseData(input);
            }
            else
            {
                if (!calibrating)
                {
                    robot.home();

                    // think this is full speed repeat home'ing the servos
                }
            }
        }
    } // end clientConnected
} //end loop
#else
//                      --- WOKWI ONLY VERSION OF LOOP ---
void loop() 
{   
    if (running)
    {
            //Serial.println("running");
        
        if (Serial.available()) 
        {
            while(Serial.available())
            {
                input = Serial.readStringUntil('+');
            }
            parseData(input);
        }
        else 
        {
            Serial.println("Keep 'Moving'...");
            
            parseData(input);
        }

        // think above two parseData(input); can move here
    }
    else
    {
            //Serial.println("Normal mode");
        
        if (Serial.available()) 
        {
            while(Serial.available()) 
            {
              input = Serial.readStringUntil('+');
            }
            parseData(input);
        }
        else
        {
            if (!calibrating)
            {
                robot.home();
                Serial.print(".");

                // think this is full speed repeat home'ing the servos
            }
        }
    }

} //end loop
//                  --- END WOKWI ONLY VERSION OF LOOP ---
#endif


void parseData(String data)
{
    int parsedCommand = data.toInt();               // note: returns zero on parse error
    
    // crudely alter the servo offsets
    if (100 <= parsedCommand && parsedCommand <= 903)
    {
        running = 0;                                // DO NOT REPEAT!!
        calibrating = 1;                            // do not return to home position;
        Serial.println("Normal mode");
        
        int trimCommand = parsedCommand % 100;      // third digit gives command
        int index = parsedCommand / 100 - 1;        // first digit gives servo number
        int trim = robot.getTrim(index);
        
        switch (trimCommand)                
        {             // TODO WOKWI: BELOW CODE WILL CRASH, no pClient
            case 0:                                 // n00 = print servo offsets code
                pClient->println("Calibration code is as follows:\n");
                for (int i = 0; i < 9; i++)
                {
                    pClient->print("trim[");
                    pClient->print(i);
                    pClient->print("] = ");
                    pClient->print(robot.getTrim(i));
                    pClient->println(";");
                }     
            break;

            case 1:                                 // n01 = decrease servo n offset by 1
            case 3:                                 // n03 = increase servo n offset by 1
                robot.setTrim(index, trim + trimCommand - 2);
                pClient->print("Servo ");
                pClient->print(index);
                pClient->print(" trim set to: ");
                pClient->println(robot.getTrim(index));
            break;
        }
            Serial.println("Zero");
        robot.zero();                               // move robot to new calibration position
    }
    
    switch (parsedCommand)   
    {
        case 1: // Up
            robot.walk(1, 550);
            running = 1;
        break;

        case 2: // Down
                            // causes robot to repeatedly do 'nothing', if used after a direction
                            // command. Otherwise it just does 'nothing' the once)
        break;

        case 3: // Left
            robot.turnL(1, 550);
            running = 1;
        break;

        case 4: // Right
            robot.turnR(1, 550);
            running = 1;
        break;

        case 5: // STOP
            running = 0;    // stops the last command repeating. If no further input occurs, 
                            // robot returns to home stance, once current movement animation
                            // reaches the end of its cycle
            Serial.println("Normal mode");
        break;

        case 6: // heart
            robot.pushUp(2, 2000);
        break;

        case 7: // fire
            robot.upDown(4, 250);
        break;

        case 8: // skull
            robot.jump();
        break;

        case 9: // cross
            robot.hello();
        break;

        case 10: // punch
            robot.frontBack(4, 200);
        break;

        case 11: // mask
            robot.dance(2, 1000);
        break;

        case 12: // calibration position / finish calibrating
            robot.zero();
            running = 1;
            calibrating = 0;
        break;

        case 13:  // trying a moon walk to the left
            robot.moonwalkL(2, 1000); // do it twice taking 1 second each time
        break;

        case 14:  // trying "run" whatever that may be
            robot.run(4, 200);  // 4x 200ms each (using "punch" as a comparable movement to define params right... maybe?)
        break;

        case 99: // shutdown power
            Serial.println("Received shutdown command... ignoring... maybe...");
            // TODO kill connections 
            // TODO save settings?
            // TODO any other cleaning?
            digitalWrite(PWR_ROPE, LOW);
        break;

        default:
            if (!calibrating)
            {
                    Serial.println("Home");
                robot.home();
            }
        break;
    } // end switch 
} // end parseData