/**
 * @file main.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief The main file.
 * 
 * Sets up Network stuff, PS3-Controller, Menu and Lcd
 * Handles Controller Input
 * 
 * @version 0.1
 * @date 2022-02-15
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include <Arduino.h>

#include "config.h"

#include <iostream>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <SPI.h>

#include "Version.h"

#include "driveModi/driveManager.h"
#include "OutputBuf/outputBuf.h"
#include "LcdWrapper.h"
#include "moveControl.h"
#include "network.h"
#include "debugMqtt.h"
#include "battery.h"
#include "debugTimes.h"
#include "controlPad.h"

#include "menu.h"
#include "menuAction.h"
#include "SpecialMenus/driveModi/ManualDrive/menuManualDrive.h"
#include "SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h"
#include "SpecialMenus/driveModi/Autopilot/menuAutopilot.h"
#include "SpecialMenus/driveModi/TestMode/menuTestMode.h"
#include "SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h"
#include "SpecialMenus/Systeminformation/menuSysteminformation.h"
#include "SpecialMenus/PID/menuPidSettings.h"
#include "SpecialMenus/Route/menuRoute.h"
#include "SpecialMenus/GPS/menuGPS.h"

MoveControl moveController;
DriveManager* driveManager;
Menu* main_m;
LiquidCrystal_I2C* lcd;
LcdWrapper* lcdWrapper;
OutputBuf* outputBuf;
DebugMqtt* debugMqtt = nullptr;
Battery* mainBattery;
SPIClass* spiPort;
ControlPad* controlPad;

bool wifiIsActive;

void i2cScanner(void);
void makeMenu(void);
void restart(void);
void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len); 
void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status);
void lcdWrapperCallback (const char data[][DISPLAY_WRAPPER_ROWS], uint8_t lines, uint8_t rows);


void setup() {
    Serial.begin(115200);
    // DebugTimes::setConsolOutput(true);
    DebugTimes setupTime;
    char wifiIndicator = 'X';

    WiFi.mode(WIFI_MODE_STA);
    Serial.print("MAC Address: ");
    Serial.println(WiFi.macAddress());

    spiPort = new SPIClass(HSPI);
    spiPort->begin(UBLOX_GNSS_SPI_SCK, UBLOX_GNSS_SPI_CIPO, UBLOX_GNSS_SPI_COPI, UBLOX_GNSS_SPI_CS);

    mainBattery = new Battery(35);
    controlPad = new ControlPad();

    Wire.begin(21, 19);
    Wire.setClock(400000);
    i2cScanner();
    lcd = new LiquidCrystal_I2C(0x3F,16,2);
    lcd->init();
    lcd->clear();
    lcd->noBacklight();

    Network::setIps();
    Network::setupMQTT();
    wifiIsActive = Network::connectWifi();
    if (wifiIsActive) {
        Network::initMQTT();
        if (Network::checkMQTT()) {
            DebugMqtt::init(Network::getMqttClient(), Loglevel::debug);
            std::cout << "Activate additional output via MQTT..." << std::endl;
            debugMqtt = new DebugMqtt("Console");
            outputBuf = new OutputBuf(debugMqtt);
        } else
            outputBuf = new OutputBuf();
        wifiIndicator = '-';
    } else
        outputBuf = new OutputBuf();
    std::cout.rdbuf(outputBuf);

    Network::connectEspNow(receiveCallback, sendCallback);

    std::cout << "Welcome to Kleiax-Rover" << std::endl;
    std::cout << "Project verion: " << VERSION << std::endl;
    std::cout << "Build timestamp: " << BUILD_TIMESTAMP << std::endl;
    std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl;

    driveManager = new DriveManager(&moveController, spiPort, controlPad->getControlPadDataPtr(), wifiIsActive);

    outputBuf->activateMqtt(true);

    lcd->backlight();
    lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
    lcd->setCursor(0, 1);
    lcd->printf("WiFi channel %u", Network::getCurrentChannel());
    lcdWrapper = new LcdWrapper(lcd);
    lcdWrapper->setCallback(lcdWrapperCallback);

    makeMenu();

    setupTime.stopConsol("Setup");
}

void loop() {
    if (wifiIsActive) {
        DebugTimes wifiTime;
        Network::checkWiFi();
        #ifdef MQTT
        bool err = Network::checkMQTT();
        if (!err)
            std::cout << "loop(): checkMQTT returns false" << std::endl;
        #endif //MQTT
        wifiTime.stopConsol("WiFi-Time", 10);
    }
    driveManager->loop();
    controlPad->loop();
    main_m->update();
    lcdWrapper->loop();
    mainBattery->loop();

    if (mainBattery->isNewValue()) {
        static uint8_t batteryLowCounter = 0;
        if (mainBattery->isBatteryLow(10))
            batteryLowCounter++;
        else
            batteryLowCounter = 0;

        if (batteryLowCounter >= 10) {        
            moveController.emergencyStop();
            uint16_t voltage = (uint16_t) (mainBattery->getBatteryVoltage() * 100);
            lcd->setCursor(0, 0);
            lcd->printf("Low Battery: %u", voltage);
            lcd->setCursor(0, 1);
            lcd->print("Please turn off.");
            
            while (true);
        }
    }
}

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

    byte error, address;
    int nDevices;
    std::cout << "Scanning..." << std::endl;
    nDevices = 0;
    for(address = 1; address < 127; address++ ) {
        Wire.beginTransmission(address);
        error = Wire.endTransmission();
        if (error == 0) {
            std::cout << "I2C device found at address 0x";
            if (address<16)
                std::cout << "0";
            std::cout << std::hex << (int) address << std::dec << std::endl;
            // std::cout << (int) address << std::endl;
            nDevices++;
        }
        else if (error==4) {
            std::cout << "Unknow error at address 0x";
            if (address<16)
                std::cout << "0";
            std::cout << std::hex << (int) address << std::dec << std::endl;
        }    
    }
    if (nDevices == 0)
        std::cout << "No I2C devices found\n" << std::endl;
    else
        std::cout << "done\n"  << std::endl;
}

void makeMenu() {
    auto dummy = []() {
        std::cout << "Dummy in Action" <<std::endl;
    };

    // Create Menu
    main_m = new Menu();
    Menu* mode_m = new Menu();
    Menu* pid_m = new Menu();
    MenuIntInput* pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0)));
    MenuIntInput* pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1)));
    MenuManualControl* man_m = new MenuManualControl(driveManager);
    MenuCaptureRoute* cap_m = new MenuCaptureRoute(driveManager);
    MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
    MenuTestMode* testM_m = new MenuTestMode(driveManager);
    MenuCalibrateCompass* comp_m = new MenuCalibrateCompass(driveManager);
    MenuGPS* gps_m = new MenuGPS(driveManager);
    MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery);
    MenuRoute* rout_m = new MenuRoute(driveManager->getNavigation()->getRoute());

    // Set Menus on LCD and set update time
    main_m->setLcd(lcdWrapper);
    mode_m->setLcd(lcdWrapper);
    pid_m->setLcd(lcdWrapper);
    pidl_m->setLcd(lcdWrapper);
    pidr_m->setLcd(lcdWrapper);
    man_m->setLcd(lcdWrapper);
    cap_m->setLcd(lcdWrapper);
    auto_m->setLcd(lcdWrapper);
    auto_m->setUpdateDelay(1000);
    testM_m->setLcd(lcdWrapper);
    comp_m->setLcd(lcdWrapper);
    gps_m->setLcd(lcdWrapper);
    gps_m->setUpdateDelay(1000);
    sys_m->setLcd(lcdWrapper);
    sys_m->setUpdateDelay(1500);
    rout_m->setLcd(lcdWrapper);

    // Entry for the main menu
    main_m->addEntry(new MenuAction("Mode", mode_m));
    main_m->addEntry(new MenuAction("GPS", gps_m));
    main_m->addEntry(new MenuAction("Route", rout_m));
    main_m->addEntry(new MenuAction("PID", pid_m));
    main_m->addEntry(new MenuAction("Systeminfo", sys_m));
    main_m->addEntry(new MenuAction("Restart", restart));

    // Entry for the mode Menu
    mode_m->addEntry(new MenuAction("Manual Control", man_m));
    mode_m->addEntry(new MenuAction("Capture Route", cap_m));
    mode_m->addEntry(new MenuAction("Autopilot", auto_m));
    mode_m->addEntry(new MenuAction("Gauge Compass", comp_m));
    mode_m->addEntry(new MenuAction("Test Mode", testM_m));
    mode_m->addEntry(new MenuAction("Consol Control", dummy));

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

    // Other config
    pidl_m->setMinMax(0, UINT8_MAX);
    pidl_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(0)->GetKp());
    pidl_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(0)->GetKi());
    pidl_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(0)->GetKd());

    pidr_m->setMinMax(0, UINT8_MAX);
    pidr_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(1)->GetKp());
    pidr_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(1)->GetKi());
    pidr_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(1)->GetKd());

    controlPad->setMenuControl(main_m);
}

void restart(void) {
    lcdWrapper->clear();
    lcdWrapper->setCursor(0, 0);
    lcdWrapper->print("Rebooting ...");
    ESP.restart();
}

void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len) {
    if (len != sizeof(ControlPadInput))
        return;
    controlPad->insertData(incomingData);
}

void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status) {
    if (status != ESP_NOW_SEND_SUCCESS)
        std::cout << "sendCallback - Delivery Fail" << std::endl;
}

void lcdWrapperCallback (const char data[][DISPLAY_WRAPPER_ROWS], uint8_t lines, uint8_t rows) {
    if (!controlPad->isControlPadConnected())
        return;

    esp_err_t result = esp_now_send(Network::getBroadcastAddress(), (uint8_t *) data, lines * rows);
   
    if (result != ESP_OK)
        Serial.println("Error sending the data");
}