#include <Arduino.h>
#include <Ps3Controller.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <time.h>

#include "config.h"

#include "hardware/motorControl.h"
#include "hardware/speedometer.h"
#include "driveModi/autopilot.h"
#include "driveModi/captureRoute.h"
#include "driveModi/manualControl.h"
#include "driveModi/consolControl.h"
#include "moveControl.h"
#include "route.h"
#include "debugMqtt.h"

void callbackControllerAction();
void callbackControllerConnect();
void controllerPrintBattery();

void reconnectMqtt();

MotorControl left_motor;
MotorControl right_motor;
Speedometer speedometer_left;
Speedometer speedometer_right;
MoveControl moveController;
Route route;
DebugMqtt debugger("main");

ManualControl manualControl;
CaptureRoute captureRoute;
Autopilot autopilot;
ConsolControl consolControl;

IPAddress local_IP(WLAN_IP);
IPAddress gateway(WLAN_GATEWAY);
IPAddress subnet(WLAN_SUBNETMASK);
IPAddress mqtt_server(MQTT_SERVER);

WiFiClient wifi_client;
PubSubClient mqtt_client(wifi_client);

int controller_battery = -1;
enum DriveMode {autopilot_e, captureRoute_e, manualControl_e, consolControl_e};
DriveMode driveMode = manualControl_e;


void setup() {
    Serial.begin(115200);

    // Configures static IP address
    if (!WiFi.config(local_IP, gateway, subnet)) {
        Serial.println("STA Failed to configure");
    }

    // Connect to Wi-Fi network with SSID and password
    Serial.print("Connecting to ");
    Serial.println(WLAN_SSID);
    WiFi.begin(WLAN_SSID, WLAN_PASSWORD);
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }

    // Print local IP address and start web server
    Serial.println("");
    Serial.println("WiFi connected.");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());

    mqtt_client.setServer(mqtt_server, MQTT_PORT);
    reconnectMqtt();

    configTime(3600, 3600, "192.168.1.2");

    DebugMqtt::init(&mqtt_client, Loglevel::debug);
    DebugMqtt::initRealMillis();

    Ps3.attach(callbackControllerAction);
    Ps3.attachOnConnect(callbackControllerConnect);
    // Ps3.attachOnDisconnect(callbackControllerDisconnect);
    Serial.println("\nReady to connect a PS3 Controller... \n");
    Ps3.begin();

    // Init hardware
    left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12, "left");
    right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22, "right");

    speedometer_left.init(M_ENCODE_1A, M_ENCODE_1B, "left");
    speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, "right");

    moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);

    // Init driveModi
    manualControl.init(&moveController, &left_motor, &right_motor);
    captureRoute.init(&route, &moveController, &left_motor, &right_motor);
    autopilot.init(&route, &moveController);
    consolControl.init(&moveController);
}

void loop() {
    if (!mqtt_client.connected())
        reconnectMqtt();
    mqtt_client.loop();

    left_motor.runMotorControl();
    right_motor.runMotorControl();
    speedometer_left.runSpeedometer();
    speedometer_right.runSpeedometer();
    route.runRoute();

    switch (driveMode) {
        case manualControl_e:
            manualControl.runManualControl();
            break;

        case captureRoute_e:
            captureRoute.runCaptureRoute();
            break;

        case autopilot_e:
            autopilot.runAutopilot();
            break;

        case consolControl_e:
            consolControl.run();
            break;

        default:
            break;
    }
}

void reconnectMqtt() {
    // Loop until reconnection
    while (!mqtt_client.connected()) {
        Serial.print("Attempting MQTT connection...");
        // Create a random client ID
        String clientId = "ESP32Rover-";
        clientId += String(random(0xffff), HEX);
        // Attempt to connect
        if (mqtt_client.connect(clientId.c_str())) {
            Serial.println("connected");
            // Once connected, publish an announcement...
            mqtt_client.publish("Rover/Info", "Connected to Mqtt-Broker");
        } else {
            Serial.print("failed, rc=");
            Serial.print(mqtt_client.state());
            Serial.println(" try again in 5 seconds");
            // Wait 5 seconds before retrying
            delay(5000);
        }
    }
}

void callbackControllerAction() {
    if (Ps3.event.button_down.ps) {
        switch (driveMode) {
            case manualControl_e:
            driveMode = DriveMode::captureRoute_e;
            debugger.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
            break;

        case captureRoute_e:
            driveMode = DriveMode::autopilot_e;
            debugger.sendMsg(Loglevel::info, "New driveMode = Autopilot");
            break;

        case autopilot_e:
            driveMode = DriveMode::consolControl_e;
            debugger.sendMsg(Loglevel::info, "New driveMode = ConsolControl");
            break;

        case consolControl_e:
            driveMode = DriveMode::manualControl_e;
            debugger.sendMsg(Loglevel::info, "New driveMode = ManualControl");
            break;

        default:
            break;
        }
    }
}

void callbackControllerConnect() {
    Serial.println("Controller connected to ESP32");
    debugger.sendMsg(Loglevel::info, "Controller connected to ESP32");
}

void controllerPrintBattery() {
    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 )      Serial.println("charging");
    else if( controller_battery == ps3_status_battery_full )     Serial.println("FULL");
    else if( controller_battery == ps3_status_battery_high )     Serial.println("HIGH");
    else if( controller_battery == ps3_status_battery_low)       Serial.println("LOW");
    else if( controller_battery == ps3_status_battery_dying )    Serial.println("DYING");
    else if( controller_battery == ps3_status_battery_shutdown ) Serial.println("SHUTDOWN");
    else Serial.println("UNDEFINED");
}