//#include <Arduino.h>
#ifdef ESP32
  #include <WiFi.h>
#else
  #include <ESP8266WiFi.h>
#endif
#include <WiFiClient.h>
#include "querty.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;
Querty robot;
Querty* Querty::robot = 0;  // if I don't do this, the static member isn't writable!
WiFiServer server(80);
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...");
        //robot.startFirmwareUpdate();
    }
    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();
 
    // testing area
    //robot.testMethod();
    //robot.testAnimation();
} // end setup

#ifndef WOKWI_SIMULATOR_PUBLIC_IoT_GATEWAY
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 (client.available()) 
        {
            input = client.readStringUntil('\n');
            parseData(input);
        }

        robot.handleEverything();
    }
}
#else
//               --- WOKWI PUBLIC IoT GATEWAY ONLY VERSION OF LOOP ---
void loop()
{
    if (Serial.available()) 
    {
        // read and execute command - WARNING! 1 sec time out!
        // use Serial.setTimeout(millisecs) to shorten.
        input = Serial.readStringUntil('\n');
        parseData(input);
    }

    robot.handleEverything();
}
//               --- END WOKWI PUBLIC IoT GATEWAY ONLY VERSION OF LOOP ---
#endif

// App GUI: Probably worth a new version, that requires button holding for repeat
//          actions. These will trigger a standard number send on buttonDown event
//          and a 2 [stop()] action initialiser on buttonUp. Would also need to first
//          send a repeat enabler before action, and a repeat disabler after. Trouble
//          is, with current repeat implementation at least, repeat has to be enabled
//          before an action is initalised. We could ditch dual action buttons and 
//          instead enable repeat on long press but that would be executed after the 
//          action started so action wouldn't repeat. A fix could be to additionally
//          set currentAction.repeat to true with the repeat setter, or maybe have a 
//          paramterised version that optionally does this.
// 
//          Might be fun to animate it the center image if repeat enabled
//          or a default repeat (e.g. walk) is running. Could maybe have different
//          animations for different directional movement (given it has those wheels)
//
// PS3 DS3 Control ideas: 
//  - Left or right: held for rotation
//  - up: for walk
//  - down: for reverse (not yet implemented)
//  - diagonals: omniwalk? (not yet tried)
//  - analogue NSEW: (as above but with shorter period for axis increase)
//  - analogue diagonals: (omniwalk? but with shorter period [T] and increased
//    turn factor for axis increase)
//  - shoulder left or right: held for head rotation
//  - shoulder analogue left or right: held for rotation with reduced period? for
//    increased press amount
//  - select: rotate through LED binary sequence for action (0-15)
//  - start: play selected action | long press (no action): power down?
//  - XOST: ??? power? listen? autonomous mode switch?   
//
//  For analogue, may need action adjustors that just change period/axis value?
//  might therefore need to adjust completion time too?

void parseData(String data)
{
    int parsedCommand = data.toInt();               // note: returns zero on parse error
    
    // TEMP Dirty GPS data handler
    if (parsedCommand == 0)
    {
        // print GPS data
        Serial.println(data);

        // not a command, so don't parse
        return;
    }   // end - Wokwi GPS test hack
    
    // crudely alter the servo offsets
    if (100 <= parsedCommand && parsedCommand <= 903)
    {       
        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 PUB IoT GATEWAY: 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;
        }

        robot.zero();                               // move robot to new calibration position

        return;                                     // don't continue to parse command
    }   // end - handle calibration command
    
    switch (parsedCommand)   
    {
        case 1: // Up
            robot.walk(1, 550);
        break;

        case 2:   
            robot.stop();
        break;

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

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

        case 5: // STOP
            robot.home();
        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
            robot.zero();
        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 15:
            robot.setRepeat(true);
        break;

        case 16:
            robot.setRepeat(false);
        break;

        case 17:
            robot.headLeft();     // TODO consider if needs speed param
        break;

        case 18:
            robot.headRight();    // TODO consider if needs speed param
        break;

        case 19:
            robot.antennaClockwise();       // TODO consider if needs speed param
        break;

        case 20:
            robot.antennaAntiClockwise();   // TODO consider if needs speed param
        break;

        case 21:
            //robot.antenna2Clockwise();      // TODO - probably won't ever be used
        break;

        case 22:
            //robot.antenna2AntiClockwise();  // TODO - probably won't ever be used
        break;

        case 23:
            //robot.utilityLeft();            // TODO - probably won't ever be used
        break;

        case 24:
            //robot.utilityRight();           // TODO - probably won't ever be used
        break;

        case 25:
            robot.disableServos();
        break;

        case 26:
            robot.enableServos();
        break;

        case 27:
            robot.setSimUpdatesEnabled(true);
        break;

        case 28:
            robot.setSimUpdatesEnabled(false);
        break;

        case 29:
            robot.enableGPSProcessing();
        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:
            // on anything other than: 0, 100-903
            robot.home();
        break;
    } // end - handle normal command 
} // end parseData