#include "FlashConfig.h"
#include "OSCRemote.h"
#include "ps4remote.h"
#include "Vbat.h"
#include "WebOTA.h"
#include "body.h"
#include "settings.h"
#include <ArduinoOTA.h>
#include <WiFi.h>
#include <memory>

Body body = Body::instance();
FlashConfig config;
std::unique_ptr<IRemote> remote;
Vbat vbat;
WebOTA webOTA(80);

void servoTest() {
    for (uint8_t legnum = 0; legnum < body.legs.size(); legnum++) {
        body.legs[legnum].setPos(Leg::Hipp, 0);
        body.legs[legnum].setPos(Leg::Knee, 0);
    }
}

void connectWiFi() {
    Serial.printf("Connecting to %s\n", config.data().wifiSSID.data());
    WiFi.begin(config.data().wifiSSID.data(), config.data().wifiPass.data());
    WiFi.setHostname(hostname);
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }
    Serial.println("");
    Serial.println("WiFi connected");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());
}

void testRemoteCallback(const IRemote::Output& o) {
    for (size_t i = 0; i < 6; ++i) {
        body.legs[i].setPos(Leg::Hipp, double(o.attitude.roll) * pi / 2 );
        body.legs[i].setPos(Leg::Knee, double(o.attitude.pitch) * pi / 2 );
        // Serial.printf("CB: %f, %f\n", o.attitude.roll, o.attitude.pitch);
    }
}

// Just a movement with one leg to show that we have booted
void sweepLeg() {
    body.legs[1].setPos(Leg::Knee, pi / 2.0f);
    sleep(1);
    body.legs[1].setPos(Leg::Knee, 0.0f);
}

void checkConfig() {
    FlashConfig::Data compData;
    strncpy(compData.wifiSSID.data(), wifiSSID, sizeof(compData.wifiSSID));
    strncpy(compData.wifiPass.data(), wifiPass, sizeof(compData.wifiPass));

    if (config.data() != compData) {
        Serial.println("New configuration loaded");
        config.save(compData);
        config.printConfig();
    }
}

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

    body.init();
    config.init();
    delay(250);

    servoTest();
    config.load();

    Serial.println("Current Flash Config:");
    Serial.printf("- WiFi SSID: %s\n", config.data().wifiSSID.data());
    Serial.printf("- WiFi pass: %s\n", config.data().wifiPass.data());
    checkConfig();
    connectWiFi();
    #ifdef OSC_REMOTE
    remote.reset(new OSCRemote(vbat));
    remote->registerCallback(testRemoteCallback);
    #endif
    #ifdef PS4_REMOTE
    remote.reset(new Ps4remote(HOST_MAC));
    #endif

    if (remote)
    {
        remote->init();
        remote->registerCallback(testRemoteCallback);
    }

    sweepLeg();

    // OTA
    ArduinoOTA.setHostname("Hexapod");
    ArduinoOTA.onStart([]() {
        String type;
        if (ArduinoOTA.getCommand() == U_FLASH)
            type = "sketch";
        else // U_SPIFFS
            type = "filesystem";

        // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end()
        Serial.println("Start updating " + type);
    });
    ArduinoOTA.onEnd([]() { Serial.println("\nEnd"); });
    ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) {
        Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
    });
    ArduinoOTA.onError([](ota_error_t error) {
        Serial.printf("Error[%u]: ", error);
        if (error == OTA_AUTH_ERROR)
            Serial.println("Auth Failed");
        else if (error == OTA_BEGIN_ERROR)
            Serial.println("Begin Failed");
        else if (error == OTA_CONNECT_ERROR)
            Serial.println("Connect Failed");
        else if (error == OTA_RECEIVE_ERROR)
            Serial.println("Receive Failed");
        else if (error == OTA_END_ERROR)
            Serial.println("End Failed");
    });

    webOTA.setup();
    ArduinoOTA.begin();
}

void loop() {
    // Serial.print(".");
    // sleep(1);
    if (remote)
        remote->loop();

    body.healthCheck();
    webOTA.loop();
    ArduinoOTA.handle();
}