

/* MAIN UNIT / BRAIN 

    This code has been written explicitely for the ESP32S3. 

    Summary:
    - Goal: interacts with the user and manages all subsystems like navigation, ..
    - Output: UI based on HTML, Javascript, CSS, p5.js via Wifi that as stored as seperate index.html file via LittleFS
    - Input: Object info from AI module via UART (after invoking it)
    - Input: Commands from user via the UI coming in via websockets (Wifi)

    Tasks:
    - after startup it checks all subsystems by sending `STATUS` message
    - if every subsystem has send `READY` back then continue
    - send `GEOFENCE_<coordinate>_<radius>` to the navigation module
    - default mode of operation (MOP) is `TEST` (which means manual control)

    In loop:
    - it will ask for status update from the NAVIGATION each minute 
    - it will wait for a command from the user; this can be:
        - mode of operation (STEALTH, GUIDE, GAME, TEST)
        - `STOP` (stop everything)
        - `HOME` (come to the home/ starting position)

    In case of:
    - STEALTH: send `WAYPOINT_<coordinate>_<radius>` to the navigation module
    - GUIDE: send 1st position of the route to the navigation module via `WAYPOINT_<coordinate>_<radius>`
    - GAME:
        - invoke the object recognizition module
        - decide left, center or right of the camera view point
        - send either `NAVLEFT_`, `NAVRIGHT_` to the navigation module
    - STOP: send `STOP_` to the navigation module
    - HOME: send `HOME_` to the navigation module

    NAVIGATION UNIT API
    - in: `STATUS_` -> out: `RDY_` (before the start), `OK_` or `NOK_` (during)
    - in: `NAVLEFT_`, `NAVRIGHT_` --> out: `ACK_` (free navigation during game)
    - in: `WAYPOINT_<coordinate>` --> out: `ACK_` (coordinates of a waypoint)
    - in: `SPEED_<number>` --> out: `ACK_` (speed between 0 and 11; 0 no speed; 10 max speed)
    - in: `GEOFENCE_<coordinate>_<radius>` --> out: `ACK_` (coordinate of circle with radius in meters)
    - in: `STOP_` --> out: `ACK_` (stops everything)
    - in: `HOME_` --> out: `ACK_` (goes to the HOME position)

    It connects to a Grove AI Vision module v2 internally but has a grove cable that connects to the NAVIGATION unit via I2C
*/

/**** Wifi & webserver ****/
#include <WiFi.h>
#include <ESPAsyncWebServer.h>
#include <AsyncTCP.h>
#include "LittleFS.h" // little filesystem that is installed on the ESP32S3 to store the index.html file

const char* ssid = "RD3000"; // is the name of the Wifi accesspoint which user can choose via device 
const char* password = "fabacademy"; // password needed ... it is in the code because it's just to prevent that anybody can just use the UI

AsyncWebServer server(80); // use webserver on port 80
AsyncWebSocket ws("/ws"); // path to recognize calls via websockets

unsigned long lastCleanup = 0;
const unsigned long cleanupInterval = 1000; // every second wifi client connections are cleaned / can't be changed during execution as it is constant
unsigned int nrWifiClients = 0;


/**** AI vision (including variables etc) ****/
#include <Seeed_Arduino_SSCMA.h> // required to interact with the Vision AI module
SSCMA AI; // initialize interaction with AI module
HardwareSerial atSerial(1); // uses internal serial connection with the AI vision module

unsigned long lastInvoke = 0; // to pace calling the AI engine
const unsigned long invokeInterval = 1000; // every 1000msec = 1 second / can't be changed as it is constant
String boxDescription = "";

/**** NAVIGATION UNIT ****/
#include <Wire.h> // to communicate with the navigation unit via I2C
#define I2C_NAV_ADDRESS 0x10 // the address of this NAVIGATION UNIT

unsigned long lastRequestNav = 0; // to pace calls to this component
const unsigned long requestNavInterval = 2000; // every 10 seconds

/*** GENERAL/ OTHER ****/
#define LED_BUILTIN 21   // Onboard yellow LED on GPIO21

unsigned long lastStatusUpdate = 0; // to pace system updates
const unsigned long statusUpdateInterval = 1000; // every second

String currentMode = "";


void blinkLed(int dutyCycle, unsigned long  mSeconds) {

    unsigned long  lastUpdate;
    lastUpdate = millis();
    
     while ((millis() - lastUpdate) < mSeconds) {
        digitalWrite(LED_BUILTIN, HIGH);  // turn LED on
        delay(dutyCycle);
        digitalWrite(LED_BUILTIN, LOW);   // turn LED off
        delay(dutyCycle);
    }
}

void setup() {
    Serial.begin(115200); // to show log to the computer via USB
    delay(5000); // just wait a bit for the connection to come up

    if (!LittleFS.begin()) { // check that the little filesystem works
        Serial.println("LittleFS Failed");
        while(1) {blinkLed(300, 1000);}
    }

    pinMode(LED_BUILTIN, OUTPUT);

    WiFi.mode(WIFI_AP); // initialize as wifi access point
    WiFi.softAPConfig(IPAddress(192,168,4,1), IPAddress(192,168,4,1), IPAddress(255,255,255,0));
    WiFi.softAP(ssid, password, 6, false, 4);   // channel 1, no hidden, max 4 clients

    Serial.print("AP IP: ");
    Serial.println(WiFi.softAPIP());

    ws.onEvent(onWsEvent);
    server.addHandler(&ws);
    server.serveStatic("/", LittleFS, "/").setDefaultFile("index.html");
    server.begin();

    ws.enable(true);   // makes sure it's active
    Serial.print("Server ready - connect to Wifi network: ");
    Serial.println(ssid );

    // Grove AI init
    atSerial.setRxBufferSize(32 * 1024);
    atSerial.begin(921600, SERIAL_8N1, 44, 43);
    while(!atSerial) { delay(20);} // wait for the connection
    AI.begin(&atSerial); // start communicating with the AI module via atSerial
    delay(1000);

    Wire.begin(); // join i2c bus (address optional for master) AS MASTER!!

}

// FUNCTION: asks the AI vision module to activate it's model and returns any hits
bool hasFoundObject () {

    if (!AI.invoke(1, false, true)) { // get the image as well; we don't send it right away

        if (AI.boxes().size() > 0) {
            auto best = AI.boxes()[0];
            for (int i = 1; i < AI.boxes().size(); i++) {
                if (AI.boxes()[i].score > best.score) best = AI.boxes()[i];
            }
            boxDescription = String(best.target) + "," +
                  String(best.score) + "," +
                  String(best.x) + "," +
                  String(best.y) + "," +
                  String(best.w) + "," +
                  String(best.h);

            Serial.print("Found object in image: ");
            Serial.println(boxDescription);

            return true;
        }
        return false; // nothing found
    }
    return false; // nothing found
}

// FUNCTION: that checks whether it's time to activate the vision model again
void findObject() {

    // AI inference timing
    if ((millis() - lastInvoke) > invokeInterval) { // lets invoke AI again
        lastInvoke = millis();
        if(hasFoundObject()) {
            // do something here
            ws.textAll(boxDescription);  // send it to the user interface via websocket
        }
    }
}

// FUNCTION: general function to spit out info
void LogSystemStatus() {

    if ((millis() - lastStatusUpdate) > statusUpdateInterval) {
        lastStatusUpdate = millis();
        Serial.print("Number of connected Wifi clients: ");
        Serial.println(nrWifiClients);
        //Serial.print("Last box data: ");
        //Serial.println(boxDescription);
        Serial.print("Current mode: ");
        Serial.println(currentMode);
    }
}

void cleanupWifClients() {
   // Wifi cleanup
    // Cleanup only every 500ms instead of every loop cycle
    // it looks for old lost connections and cleans them
    if ((millis() - lastCleanup) > cleanupInterval) {
        ws.cleanupClients();
        lastCleanup = millis();
    }
}


void updConnectedClients() {
    nrWifiClients = WiFi.softAPgetStationNum(); // update the general variable
}

bool sendCmd2NavUnit(String msg) {
    // placeholder to send command to NAV UNIT
    Serial.print("sending command to RP2040: ");
    Serial.println(msg);

    lastRequestNav = millis(); // so we don't ask for an update in the mean time

    Wire.beginTransmission(I2C_NAV_ADDRESS);  // Transmit to device number 44 (0x2C)
    // Arduino's library is meant to send bytes not Strings 
    // Therefore I'll copy the String into a buffer of type uinit8_t = unsigned byte of 8 bits

    Wire.write((const uint8_t*)msg.c_str(), msg.length()); // send this buffer
    byte error = Wire.endTransmission(); // Stop transmitting
    if (error == 0) {
        Serial.println("command was send");
        return true;
    } else {
        Serial.print("command was not send/ error: ");
        Serial.println(error);
        return false;
    }
}


void ask4UpdateNavUnit() {

    String response = "";

    if ((millis() - lastRequestNav) > requestNavInterval) { // lets ask the NAV unit for an update again
        Serial.println("asking for info from RP2040");
        lastRequestNav = millis();

        // https://docs.arduino.cc/language-reference/en/functions/communication/wire/requestFrom/
        // output is number of bytes received
        int count = Wire.requestFrom(I2C_NAV_ADDRESS, 4);    // request 20 bytes from peripheral device; might not get it
        while (Wire.available()) { // peripheral may send less than requested but we don't mind
           char c = Wire.read(); // receive a byte as character
           response += c;
        }
        Serial.print("NAV response, bytes= ");
        Serial.print(count);
        Serial.print(": ");
        Serial.println(response);
    }
}

void loop() {
/* In the loop:
    - it will ask for status update from the NAVIGATION each minute 
    - it will wait for a command from the user; this can be:
        - mode of operation (STEALTH, GUIDE, GAME, TEST)
        - `STOP` (stop everything)
        - `HOME` (come to the home/ starting position)
    The request from the user are handled async via websockets
*/

    updConnectedClients();
    cleanupWifClients();
   // findObject(); // for the moment turned off
    ask4UpdateNavUnit();
    LogSystemStatus();
}

/****************/

// FUNCTION: if user selects the option STEALTH
void switch2StealthMode() {
/* - STEALTH:
    - send `WAYPOINT_<coordinate>_<radius>` to the navigation module
    There's no loop
    This mode continues until user selects something else
*/

    if (sendCmd2NavUnit("GEOFENCE_<coordinate>_<radius>_")) {
        ws.textAll("ACK_"); // send info back to the client
        currentMode = "STEALTH";
    } else
        ws.textAll("NOK_");
}

void switch2GameMode() {
/* - GAME:
        - invoke the object recognizition module
        - decide left, center or right of the camera view point
        - send either `NAVLEFT_`, `NAVRIGHT_` to the navigation module
    Of course this is a loop until the user selects something else
*/

    if (sendCmd2NavUnit("RC_")) {
        ws.textAll("ACK_"); // send info back to the client
        currentMode = "GAME";
    }
    else
        ws.textAll("NOK_");
}

void switch2GuideMode() {
/* - GUIDE:
    send 1st position of the route to the navigation module via `WAYPOINT_<coordinate>_<radius>`
    There is NO loop 
    This mode continues until user selects something else
*/    


    if (sendCmd2NavUnit("WAYPOINT_<coordinate>_")) {
        ws.textAll("ACK_"); // send info back to the client
        currentMode = "GUIDE";
    }
    else
        ws.textAll("NOK_");
}

void switch2StopMode() {
/* - STOP:
    send `STOP_` to the navigation module
    There is NO loop 
    This mode continues until user selects something else
*/    
    if (sendCmd2NavUnit("STOP_")) {
        ws.textAll("ACK_"); // send info back to the client
        currentMode = "STOP";
    } else
        ws.textAll("NOK_");
}

void switch2HomeMode() {
/*
    - HOME: send `HOME_` to the navigation module
    There is NO loop 
    This mode continues until user selects something else
*/
    if (sendCmd2NavUnit("HOME_")) {
        ws.textAll("ACK_"); // send info back to the client
        currentMode = "HOME";
    } else
        ws.textAll("NOK_");
}

void switch2RCMode() {
/* RC:
    This is an internal function that will allow the user to control LEFT, RIGHT and SPEED
    There is NO loop 
    This mode continues until user selects something else
*/    

   if (sendCmd2NavUnit("RC_")) {
        ws.textAll("ACK_"); // send info back to the client
        currentMode = "RC";
    }
    else
        ws.textAll("NOK_");
}

void steerRCRight() {
/* 
    Requires: RC mode
    makes the duck go right
    no switch of mode
*/
    if (sendCmd2NavUnit("NAVRIGHT_"))
        ws.textAll("ACK_"); // send info back to the client
    else
        ws.textAll("NOK_");
}

void steerRCLeft() {
/* 
    Requires: RC mode
    makes the duck go left
    no switch of mode
*/
    if (sendCmd2NavUnit("NAVLEFT_"))
        ws.textAll("ACK_"); // send info back to the client
    else
        ws.textAll("NOK_");
}

void adjustSpeed(String msg) {
/* 
    Requires: RC mode
    makes the duck increase of decrease speed
    no switch of mode

    msg will be SPEED_00, SPEED_10, SPEED_30, SPEED_50, SPEED_70 or SPEED_100
*/

    // UI might send SPEED_70 without ending underscore; so just to be sure
    if (!msg.endsWith("_")) {
        msg += "_";
    }
    if (sendCmd2NavUnit(msg)) // just send the msg as-is 
        ws.textAll("ACK_"); // send info back to the client
    else
        ws.textAll("NOK_");
}

// FUNCTION: get command via websocket and calls the appropiate function
void processUserRequest (String msg) {

// User can send a couple of requests
    if (msg == "STEALTH") {
        switch2StealthMode();
    } else 
    if (msg == "GUIDE") {
        switch2GuideMode();
    } else
    if (msg == "GAME") {
        switch2GameMode();
    } else
    if (msg == "STOP") {
        switch2StopMode();
    } else
    if (msg == "HOME") {
        switch2HomeMode();
    }  else
    if (msg == "RC") {
        switch2RCMode();
    } else
    if (msg == "LEFT") {
        steerRCLeft();
    } else
    if (msg == "RIGHT") {
        steerRCRight();
    } else
    // if underscore is part of the message then it means SPEED_xy
    if (msg.startsWith("SPEED_")) { 
        adjustSpeed(msg);
    }
    Serial.print("Request from client: ");
    Serial.println(msg);
}

// // this function is only called when it requested by the front-end
// void sendSnapshot() {
//     if (AI.last_image().length() > 0) {
//         String header = "IMG:";
//         ws.textAll(header + AI.last_image());
//         Serial.println("Snapshot sent (" + String(AI.last_image().length()/1024) + " KB)");
//     } else {
//         Serial.println("No image available for snapshot");
//     }
// }


// Websocket interrupt handler
void onWsEvent(AsyncWebSocket *server, AsyncWebSocketClient *client,
               AwsEventType type, void *arg, uint8_t *data, size_t len) {

    if (type == WS_EVT_CONNECT) {
        Serial.println("Client connected");
    }
    else if (type == WS_EVT_DATA) {
        String msg = String((char*)data).substring(0, len); // convert to a string
        processUserRequest(msg);
    }
}
