#include <Arduino.h>
#include <Ps3Controller.h>
#include <iostream>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

#include "config.h"


#include "driveModi/Modi/ManualControl/manualControl.h"
#include "driveModi/driveManager.h"
#include "moveControl.h"
#include "network.h"

#include "menu.h"
#include "menuAction.h"
#include "SpecialMenus/PID/menuPidSettings.h"
#include "SpecialMenus/driveModi/ManualDrive/menuManualDrive.h"
#include "SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h"
#include "SpecialMenus/driveModi/Autopilot/menuAutopilot.h"
#include "SpecialMenus/GPS/menuGPS.h"

MoveControl moveController;
DriveManager driveManager(&moveController);
Menu* main_m;
LiquidCrystal_I2C* lcd;

// TODO: Platzhalter, muss irgendwann weg
void dummy(){std::cout << "Dummy in Action" <<std::endl;}


void callbackControllerAction();
void callbackControllerConnect();
void controllerPrintBattery();
void i2cScanner();
void restart();


void setup() {
    Serial.begin(115200);
    std::cout << "Welcome to Kleiax-Rover" << std::endl;
    std::cout << "All actions from the main programm run on Core -> " << xPortGetCoreID() << std::endl;

    Wire.begin(21, 19);
    // i2cScanner();
    lcd = new LiquidCrystal_I2C(0x3F,16,2);
    lcd->init();
    lcd->backlight();
    lcd->print("- Kleiax-Rover -");
    lcd->setCursor(0, 1);
    lcd->print("Press PS-Button");

    Network::setIps();
    Network::connectWifi();
    Network::setupMQTT();

    Ps3.attach(callbackControllerAction);
    Ps3.attachOnConnect(callbackControllerConnect);
    std::cout << "\nReady to connect a PS3 Controller... \n";
    Ps3.begin(CONTROLLER_MAC);

    // Create Menu
    main_m = new Menu();
    Menu* mode_m = new Menu();
    Menu* pid_m = new Menu();
    MenuPidSettings* pidl_m = new MenuPidSettings(moveController.getPID(0));
    MenuPidSettings* pidr_m = new MenuPidSettings(moveController.getPID(1));
    MenuManualControl* man_m = new MenuManualControl(&driveManager);
    MenuCaptureRoute* cap_m = new MenuCaptureRoute(&driveManager);
    MenuAutopilot* auto_m = new MenuAutopilot(&driveManager);
    MenuGPS* gps_m = new MenuGPS(driveManager.getNavigation()->getGPS());

    // Set Menus on LCD
    main_m->setLcd(lcd);
    mode_m->setLcd(lcd);
    pid_m->setLcd(lcd);
    pidl_m->setLcd(lcd);
    pidr_m->setLcd(lcd);
    man_m->setLcd(lcd);
    cap_m->setLcd(lcd);
    auto_m->setLcd(lcd);
    gps_m->setLcd(lcd);


    // Entry for the main menu
    MenuAction* mode_e = new MenuAction("Mode", mode_m);
    MenuAction* gps_e = new MenuAction("GPS", gps_m);
    MenuAction* pid_e = new MenuAction("PID", pid_m);
    MenuAction* restart_e = new MenuAction("Restart", restart);
    main_m->addEntry(mode_e);
    main_m->addEntry(gps_e);
    main_m->addEntry(pid_e);
    main_m->addEntry(restart_e);

    // Entry for the mode Menu
    MenuAction* autopilot_e = new MenuAction("Autopilot", auto_m);
    MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m);
    MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
    MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
    MenuAction* testMode_e = new MenuAction("Test Mode", dummy);
    mode_m->addEntry(autopilot_e);
    mode_m->addEntry(captureRoute_e);
    mode_m->addEntry(consolControl_e);
    mode_m->addEntry(manualControl_e);
    mode_m->addEntry(testMode_e);

    // Entry for the PID Menu
    MenuAction* pidl_e = new MenuAction("Left", pidl_m);
    MenuAction* pidr_e = new MenuAction("Right", pidr_m);
    pid_m->addEntry(pidl_e);
    pid_m->addEntry(pidr_e);

}

void loop() {
    Network::checkMQTT();
    driveManager.loop();
}

bool isDelayReached(bool reset = false) {
    static uint32_t lastMillis = 0;
    if (reset)
        lastMillis = millis();
    static const uint16_t delay = 300;
    bool res = false;

    if (millis() - lastMillis > delay)
        res = true;    
    return res;
}

void callbackControllerAction() {
    // Menu controlling
    if (isDelayReached()) {
        if (Ps3.data.button.up)
            main_m->up();
        else if (Ps3.data.button.down)
            main_m->down();
        else if (Ps3.data.button.left)
            main_m->left();
        else if (Ps3.data.button.right)
            main_m->right();
        else if (Ps3.data.button.circle)
            main_m->yes();
        else if (Ps3.data.button.cross)
            main_m->no();   
        else if (Ps3.data.button.ps)
            controllerPrintBattery();

        isDelayReached(true);        
    }
    // This have to be stand here because it have to be run on the other Core
    main_m->update();
}

void callbackControllerConnect() {
    std::cout << "Controller connected to ESP32" << std::endl;
    std::cout << "All actions from the Controller run on Core -> " << xPortGetCoreID() << std::endl;

    // TODO: Display is not functional without this seconde init, first init is in setup()
    lcd->init();
    main_m->printMenu();
}

void controllerPrintBattery() {
    static uint8_t controller_battery = -1;
    if( controller_battery != Ps3.data.status.battery ){
        controller_battery = Ps3.data.status.battery;
    }

    Serial.print("The controller battery is ");
    if( controller_battery == ps3_status_battery_charging )      printf("charging\n");
    else if( controller_battery == ps3_status_battery_full )     printf("FULL\n");
    else if( controller_battery == ps3_status_battery_high )     printf("HIGH\n");
    else if( controller_battery == ps3_status_battery_low)       printf("LOW\n");
    else if( controller_battery == ps3_status_battery_dying )    printf("DYING\n");
    else if( controller_battery == ps3_status_battery_shutdown ) printf("SHUTDOWN\n");
    else printf("UNDEFINED\n");
}

void i2cScanner() {
    std::cout << "\nI2C Scanner" << std::endl;


    byte error, address;
    int nDevices;
    Serial.println("Scanning...");
    nDevices = 0;
    for(address = 1; address < 127; address++ ) {
        Wire.beginTransmission(address);
        error = Wire.endTransmission();
        if (error == 0) {
            Serial.print("I2C device found at address 0x");
            if (address<16)
                Serial.print("0");
            Serial.println(address,HEX);
            nDevices++;
        }
        else if (error==4) {
            Serial.print("Unknow error at address 0x");
            if (address<16)
                Serial.print("0");
            Serial.println(address,HEX);
        }    
    }
    if (nDevices == 0)
        Serial.println("No I2C devices found\n");
    else
        Serial.println("done\n");
}

void restart() {
    ESP.restart();
}