/**
 * @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 <Ps3Controller.h>
#include <iostream>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Ping.h>
#include <BluetoothSerial.h>
#include <esp_bt_main.h>

#include "driveModi/driveManager.h"
#include "OutputBuf/outputBuf.h"
#include "moveControl.h"
#include "network.h"
#include "debugMqtt.h"
#include "battery.h"
#include "debugTimes.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/PID/menuPidSettings.h"
#include "SpecialMenus/GPS/menuGPS.h"
#include "SpecialMenus/Akku/menuAkku.h"

MoveControl moveController;
DriveManager* driveManager;
Menu* main_m;
LiquidCrystal_I2C* lcd;
OutputBuf* outputBuf;
DebugMqtt* debugMqtt = nullptr;
Battery* mainBattery;
Ps3Controller* gamepad;
BluetoothSerial SerialBT;

bool wifiIsActive;
bool disconnectPs3 = false;


void configureController(void);
void callbackControllerAction(void);
void callbackControllerConnect(void);
void callbackControllerDisconnect(void);
void i2cScanner(void);
void makeMenu(void);


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

    mainBattery = new Battery(35, 692000, 218500);

    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();
    driveManager = new DriveManager(&moveController, wifiIsActive);
    if (wifiIsActive) {
        
        // const char* remoteHost = "www.google.com";
        // std::cout << "Pinging host: " << remoteHost << std::endl;
        // if (Ping.ping(remoteHost))
        //     std::cout << "Ping successful..." << std::endl;
        // else
        //     std::cout << "Ping failed. :/" << std::endl;

        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);

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

    gamepad = new Ps3Controller;
    configureController();

    outputBuf->activateMqtt(false);

    lcd->backlight();
    lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
    lcd->setCursor(0, 1);
    lcd->print("Press PS-Button");

    makeMenu();

    setupTime.stopConsol("Setup");
}

void loop() {
    if (wifiIsActive) {
        DebugTimes wifiTime;
        Network::checkWiFi();
        Network::checkMQTT();
        wifiTime.stopConsol("WiFi-Time", 5);
    }
    driveManager->loop();

    DebugTimes mainBatteryTime;
    mainBattery->loop();
    mainBatteryTime.stopConsol("MainBatteryTime", 5);

    if (disconnectPs3) {
        DebugTimes disconnectPS3Time;
        std::cout << "Disconnect PS3 Controller." << std::endl;
        gamepad->end();
        esp_bluedroid_disable();
        esp_bluedroid_deinit();
        configureController();
        disconnectPs3 = false;
        disconnectPS3Time.stopConsol("Ps3 Disconnect");
    }
}

void configureController() {
    
    gamepad->attach(callbackControllerAction);
    gamepad->attachOnConnect(callbackControllerConnect);
    gamepad->attachOnDisconnect(callbackControllerDisconnect);
    std::cout << "\nReady to connect a PS3 Controller... \n";

    gamepad->begin(CONTROLLER_MAC);
}

void callbackControllerAction() {
    auto isDelayReached = [](bool reset = false) -> bool {
        static uint32_t lastMillis = 0;
        if (reset)
            lastMillis = millis();
        static const uint16_t delay = 100;
        bool res = false;

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

    // Menu controlling
    if (isDelayReached()) {
        if (gamepad->data.button.up)
            main_m->up();
        else if (gamepad->data.button.down)
            main_m->down();
        else if (gamepad->data.button.left)
            main_m->left();
        else if (gamepad->data.button.right)
            main_m->right();
        else if (gamepad->data.button.circle)
            main_m->yes();
        else if (gamepad->data.button.cross)
            main_m->no();
        else if (gamepad->data.button.select)
            debugMqtt->sendMsg(Loglevel::info, "Here we are: callbackController..");

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

void callbackControllerConnect() {
    static bool firstConnect = true;

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

    // Display is not functional without this seconde init, first init is in setup(),
    // because after the setup the other core control the display.
    if (firstConnect) {
        lcd->init();
        main_m->printMenu();
        firstConnect = false;
    }
}

void callbackControllerDisconnect() {
    std::cout << "Controller disconnected..." << std::endl;
    // gamepad->end();
}

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;
    };

    auto restart = [&]() {
        lcd->clear();
        lcd->setCursor(0, 0);
        lcd->print("Rebooting ...");
        ESP.restart();
    };

    auto disconnectController = [&]() {
        disconnectPs3 = true;
    };

    // 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);
    MenuTestMode* test_m = new MenuTestMode(driveManager);
    MenuGPS* gps_m = new MenuGPS(driveManager);
    MenuAkku* akku_m = new MenuAkku(mainBattery, gamepad);

    // 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);
    test_m->setLcd(lcd);
    gps_m->setLcd(lcd);
    akku_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);
    MenuAction* akku_e = new MenuAction("Battery", akku_m);
    MenuAction* contr_e = new MenuAction("Remove PS3", disconnectController);
    main_m->addEntry(mode_e);
    main_m->addEntry(gps_e);
    main_m->addEntry(pid_e);
    main_m->addEntry(akku_e);
    main_m->addEntry(contr_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", test_m);
    mode_m->addEntry(autopilot_e);
    mode_m->addEntry(captureRoute_e);
    mode_m->addEntry(manualControl_e);
    mode_m->addEntry(consolControl_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);
}