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

#include "config.h"
#include "hardware/motorControl.h"
#include "moveControl.h"
#include "hardware/speedometer.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;
DebugMqtt debugger("main");

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;

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("Kleiax2", "Punica-699");
    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();

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

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

    left_motor.runMotorControl();
    right_motor.runMotorControl();
    speedometer_left.runSpeedometer();
    speedometer_right.runSpeedometer();
}

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.start) {
        
    }
}

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