/**
 * @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 "networkConfig.h"
#include "debugMqtt.h"
#include "battery.h"
#include "sensorData.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/SensorData/menuSensorData.h"
#include "SpecialMenus/CalibrateBattery/menuCalibrateBattery.h"

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

constexpr uint16_t displayUpdateDelay = 500;

void i2cScanner();
void makeMenu();
void restart();
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[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows);
NetworkAddresses setIPs();

void setup()
{
    Serial.begin(Settings::baudRate);
    // DebugTimes::setConsolOutput(true);
    DebugTimes setupTime;

    spiPort = new SPIClass(HSPI);
    spiPort->begin(PinNumbers::spiSck, PinNumbers::spiCipo, PinNumbers::spiCopi, PinNumbers::gnssSpiCs);

    mainBattery = new Battery(PinNumbers::battery);
    controlPad = new ControlPad();

    Wire.begin(PinNumbers::sda, PinNumbers::scl);
    Wire.setClock(Settings::i2cSpeed);
    i2cScanner();
    lcd = new LiquidCrystal_I2C(0x3F, 16, 2);
    lcd->init();
    lcd->clear();
    lcd->noBacklight();

    network = new Network(static_cast<const char *>(NetworkConfig::ssid),
                          static_cast<const char *>(NetworkConfig::password),
                          setIPs());
    network->activateEspNow(receiveCallback, sendCallback);
    if (NetworkConfig::mqtt)
    {
        if (network->activateMqtt(static_cast<const char *>(MqttConfig::user),
                                  static_cast<const char *>(MqttConfig::password)))
        {
            DebugMqtt::init(network->getMqttClient(), Loglevel::debug);
            debugMqtt = new DebugMqtt("Console");
            outputBuf = new OutputBuf(debugMqtt);
            outputBuf->activateMqtt(true);
        }
        else
        {
            outputBuf = new OutputBuf();
        }
    }
    else
    {
        outputBuf = new OutputBuf();
    }
    std::cout.rdbuf(outputBuf);

    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;

    sensorData = new SensorData();
    sensorData->enableGnss(spiPort, PinNumbers::gnssSpiCs);
    sensorData->enableRealCompass();
    sensorData->enableNtrip(static_cast<const char *>(NtripConfig::host),
                            NtripConfig::port,
                            static_cast<const char *>(NtripConfig::mountPoint),
                            static_cast<const char *>(NtripConfig::user),
                            static_cast<const char *>(NtripConfig::password));
    sensorData->enableGyroscope();
    driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr());

    char wifiIndicator = 'X';
    if (network->isWifiConnected())
    {
        wifiIndicator = '-';
    }
    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()
{
    network->loop();
    sensorData->loop();
    driveManager->loop();
    controlPad->loop();
    main_m->update();
    lcdWrapper->loop();
    mainBattery->loop();

    // new Value ervery 0.5s
    if (mainBattery->isNewValue())
    {
        static uint8_t batteryLowCounter = 0;
        static constexpr uint8_t minVoltage = 10;
        if (mainBattery->isBatteryLow(minVoltage))
        {
            batteryLowCounter++;
        }
        else
        {
            batteryLowCounter = 0;
        }

        if (batteryLowCounter >= 10)
        {
            moveController.emergencyStop();
            const auto voltage = static_cast<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()
{
    constexpr uint8_t checkForLength = 16;
    constexpr uint8_t maxAdresses = UINT8_MAX / 2;
    std::cout << "\nI2C Scanner" << std::endl;

    byte error = 0;
    byte address = 0;
    int nDevices = 0;
    std::cout << "Scanning..." << std::endl;
    for (address = 1; address < maxAdresses; address++)
    {
        Wire.beginTransmission(address);
        error = Wire.endTransmission();
        if (error == 0)
        {
            std::cout << "I2C device found at address 0x";
            if (address < checkForLength)
            {
                std::cout << "0";
            }
            std::cout << std::hex << static_cast<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 < checkForLength)
            {
                std::cout << "0";
            }
            std::cout << std::hex << static_cast<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();
    main_m->setLcd(lcdWrapper);
    auto *mode_m = new Menu();
    auto *set_m = new Menu();
    auto *pid_m = new Menu();
    auto *pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0)));
    auto *pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1)));
    auto *speed_m = new MenuIntInput(2, new MenuSpeed(driveManager->getDrivingSpeedsRef()));
    auto *man_m = new MenuManualControl(driveManager);
    auto *cap_m = new MenuCaptureRoute(driveManager);
    auto *auto_m = new MenuAutopilot(driveManager);
    auto *testM_m = new MenuTestMode(driveManager);
    auto *comp_m = new MenuCalibrateCompass(driveManager);
    auto *sys_m = new MenuSysteminformation(mainBattery);
    auto *sen_m = new MenuSensorData(sensorData);
    // TODO: Wie bekommt jeder die dumme Route?
    auto *rout_m = new MenuRoute(new Route());
    auto *bat_m = new MenuCalibrateBattery(mainBattery);

    auto_m->setUpdateDelay(displayUpdateDelay);
    sys_m->setUpdateDelay(displayUpdateDelay);
    sen_m->setUpdateDelay(displayUpdateDelay);

    // Entry for the main menu
    main_m->addEntry(new MenuAction("Mode", mode_m));
    main_m->addEntry(new MenuAction("Sensor", sen_m));
    main_m->addEntry(new MenuAction("Route", rout_m));
    main_m->addEntry(new MenuAction("Settings", set_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 setting menu
    set_m->addEntry(new MenuAction("PID", pid_m));
    set_m->addEntry(new MenuAction("Speed", speed_m));
    set_m->addEntry(new MenuAction("Distance", dummy));
    set_m->addEntry(new MenuAction("WiFi", dummy));
    set_m->addEntry(new MenuAction("Battery", bat_m));

    // 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", static_cast<uint8_t>(moveController.getPID(0)->GetKp()));
    pidl_m->setEntry(1, "I-Part", static_cast<uint8_t>(moveController.getPID(0)->GetKi()));
    pidl_m->setEntry(2, "D-Part", static_cast<uint8_t>(moveController.getPID(0)->GetKd()));

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

    speed_m->setMinMax(5, UINT8_MAX);
    speed_m->setEntry(0, "x m/s e-1", driveManager->getDrivingSpeedsRef().x * 10);
    speed_m->setEntry(1, "z rad/s e-1", driveManager->getDrivingSpeedsRef().rot * 10);

    man_m->addSensorMenu(sen_m);

    controlPad->setMenuControl(main_m);
}

void restart()
{
    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[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows)
{
    if (!controlPad->isControlPadConnected())
    {
        return;
    }

    const esp_err_t result = esp_now_send(network->getBroadcastAddress(), reinterpret_cast<const uint8_t *>(data), lines * rows);

    if (result != ESP_OK)
    {
        Serial.println("Error sending the data");
    }
}

NetworkAddresses setIPs()
{
    NetworkAddresses adresses;
    adresses.localIP.fromString(static_cast<const char *>(NetworkConfig::ip));
    adresses.subnet.fromString(static_cast<const char *>(NetworkConfig::subnet));
    adresses.gateway.fromString(static_cast<const char *>(NetworkConfig::gateway));
    adresses.dnsServer.fromString(static_cast<const char *>(NetworkConfig::dns));
    if (NetworkConfig::mqtt)
    {
        adresses.mqttServer.fromString(static_cast<const char *>(MqttConfig::server));
        adresses.mqttPort = MqttConfig::port;
    }

    return adresses;
}