#include <Arduino.h>
#include <WiFi.h>
#include <ArduinoOTA.h>
#include <BluetoothSerial.h>
#include "DummyStream.h"

#include "bafang.h"
#include "commands.h"
#include "utils.h"
#include "main.h"

#define DISPLAY_RX 18
#define DISPLAY_TX 19
#define MOTOR_RX 16
#define MOTOR_TX 17

#define THROTTLE_GPIO 32
#define BRAKE_GPIO 25

WiFiServer server(23);
WiFiClient serverClient;

BluetoothSerial SerialBT;
DummyStream SerialDummy;

const char g_WIFI_SSID[] = "BotoX";
const char g_WIFI_Passphrase[] = "";

namespace Main
{

int g_CurrentChannel = CHANNEL_HWSERIAL;
int g_Debug[NUM_CHANNELS];
char g_SerialBuffer[NUM_CHANNELS][255];
int g_SerialBufferPos[NUM_CHANNELS];

void init()
{
    setCpuFrequencyMhz(80);

    Serial.begin(115200);
    Serial.println("BOOTED!");

    pinMode(THROTTLE_GPIO, INPUT);
    pinMode(BRAKE_GPIO, INPUT);

    Serial1.begin(1200, SERIAL_8N1, DISPLAY_RX, DISPLAY_TX); // Display
    Serial2.begin(1200, SERIAL_8N1, MOTOR_RX, MOTOR_TX); // Motor

    Bafang::DisplaySerial = Serial1;
    Bafang::MotorSerial = Serial2;

    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()
        channel()->println("Start updating " + type);
    })
    .onEnd([]() {
        channel()->println("\nEnd");
    })
    .onProgress([](unsigned int progress, unsigned int total) {
        channel()->printf("Progress: %u%%\r", (progress / (total / 100)));
    })
    .onError([](ota_error_t error) {
        channel()->printf("Error[%u]: ", error);
        if (error == OTA_AUTH_ERROR) channel()->println("Auth Failed");
        else if (error == OTA_BEGIN_ERROR) channel()->println("Begin Failed");
        else if (error == OTA_CONNECT_ERROR) channel()->println("Connect Failed");
        else if (error == OTA_RECEIVE_ERROR) channel()->println("Receive Failed");
        else if (error == OTA_END_ERROR) channel()->println("End Failed");
    });
}

Stream* channel(int num)
{
    if(num == -1)
        num = g_CurrentChannel;

    if(num == CHANNEL_BTSERIAL && SerialBT.hasClient())
        return &SerialBT;
    else if(num == CHANNEL_TCPSERIAL && serverClient)
        return &serverClient;
    else if(num == CHANNEL_HWSERIAL)
        return &Serial;

    return &SerialDummy;
}

void loop()
{
    Bafang::tick();

    if(WiFi.isConnected())
    {
        ArduinoOTA.handle();

        if(server.hasClient())
        {
            if(serverClient) // disconnect current client if any
                serverClient.stop();
            serverClient = server.available();
        }
        if(!serverClient)
            serverClient.stop();
    }

    for(int i = 0; i < NUM_CHANNELS; i++)
    {
        while(channel(i)->available())
        {
            g_CurrentChannel = i;
            int c = channel(i)->read();
            if(c == '\r' || c == '\n' || g_SerialBufferPos[i] == sizeof(*g_SerialBuffer))
            {
                g_SerialBuffer[i][g_SerialBufferPos[i]] = 0;

                if(g_SerialBufferPos[i])
                    Commands::parseLine(g_SerialBuffer[i]);

                g_SerialBufferPos[i] = 0;
                continue;
            }
            g_SerialBuffer[i][g_SerialBufferPos[i]] = c;
            ++g_SerialBufferPos[i];
        }
    }

    SecretFunction();
}

void SecretFunction()
{
    static bool s_bThrottle = false;
    static unsigned long s_firstThrottleTime = 0;
    static int s_throttleCount = 0;
    static bool s_bTriggered = false;

    bool bBrake = !digitalRead(BRAKE_GPIO);
    uint16_t throttleVal = analogRead(THROTTLE_GPIO);

    if(bBrake)
    {
        if(throttleVal > 1100 && !s_bThrottle)
        {
            s_bThrottle = true;
            if(!s_throttleCount)
                s_firstThrottleTime = millis();
            s_throttleCount++;
        }
        else if(throttleVal < 1000 && s_bThrottle)
        {
            s_bThrottle = false;
        }

        if(!s_bTriggered && s_throttleCount && millis() - s_firstThrottleTime > 3000)
        {
            s_bTriggered = true;
            SecretAction(s_throttleCount);
        }
    }
    else
    {
        if(!s_bTriggered && s_throttleCount)
            SecretAction(s_throttleCount);
        s_bTriggered = false;
        s_firstThrottleTime = 0;
        s_throttleCount = 0;
    }
}

void SecretAction(int code)
{
    if(g_Debug[g_CurrentChannel])
        channel()->printf("SecretAction: %d\n", code);

    switch(code)
    {
        case 3:
        case 4: {
            motorProfile(1);
        } break;
        case 5: {
            toggleBluetooth();
        } break;
        case 6: {
            toggleWiFi();
        } break;
    }
}

void toggleBluetooth(int mode)
{
    if(mode == -1)
        mode = !btStarted();

    if(mode == 1)
    {
        if(btStarted())
        {
            channel()->println("Bluetooth already enabled.");
            return;
        }

        channel()->println("Enabling bluetooth...");
        SerialBT.begin("ESP32-BAFANG");
    }
    else if(mode == 0)
    {
        if(!btStarted())
        {
            channel()->println("Bluetooth already disabled.");
            return;
        }

        channel()->println("Disabling bluetooth...");
        SerialBT.end();
    }
}

void toggleWiFi(int mode)
{
    if(mode == -1)
        mode = !(WiFi.getMode() == WIFI_MODE_NULL);

    if(mode == 1)
    {
        if(WiFi.getMode() != WIFI_MODE_NULL)
        {
            channel()->println("WiFi already enabled.");
            return;
        }

        channel()->println("Enabling WiFi...");
        WiFi.begin(g_WIFI_SSID, g_WIFI_Passphrase);
        WiFi.setHostname("ESP32-BAFANG");
        ArduinoOTA.begin();
        server.begin();
        server.setNoDelay(true);
    }
    else if(mode == 0)
    {
        if(WiFi.getMode() == WIFI_MODE_NULL)
        {
            channel()->println("WiFi already disabled.");
            return;
        }

        channel()->println("Disabling WiFi...");
        ArduinoOTA.end();
        server.end();
        WiFi.disconnect(true, true);
    }
}

void motorProfile(int profile)
{
    switch(profile)
    {
        case 0: { // Unlimited
            const uint8_t basic[] = {0x16,0x52,0x18,0x26,0x1E,0x01,0x14,0x19,0x1E,0x23,0x28,0x2D,0x32,0x37,0x64,0x01,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x3A,0x01,0xFF};
            const uint8_t pedal[] = {0x16,0x53,0x0B,0x03,0xFF,0x28,0x0A,0x04,0x04,0xFF,0x0A,0x08,0x00,0x50,0xFB};
            const uint8_t throttle[] = {0x16,0x54,0x06,0x0B,0x2A,0x01,0x09,0x28,0x0F,0xD0};

            Bafang::QueueMotor(basic, sizeof(basic), Bafang::PrintCallback, (void *)g_CurrentChannel, 500);
            Bafang::QueueMotor(pedal, sizeof(pedal), Bafang::PrintCallback, (void *)g_CurrentChannel, 500);
            Bafang::QueueMotor(throttle, sizeof(throttle), Bafang::PrintCallback, (void *)g_CurrentChannel, 500);
        } break;
        case 1: { // Limited 10A 26km/h
            const uint8_t basic[] = {0x16,0x52,0x18,0x26,0x0A,0x01,0x14,0x19,0x1E,0x23,0x28,0x2D,0x32,0x37,0x64,0x01,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x3A,0x01,0xEB};
            const uint8_t pedal[] = {0x16,0x53,0x0B,0x03,0xFF,0x1A,0x0A,0x04,0x04,0xFF,0x0A,0x08,0x00,0x50,0xED};
            const uint8_t throttle[] = {0x16,0x54,0x06,0x0B,0x2A,0x01,0x09,0x1A,0x0F,0xC2};

            Bafang::QueueMotor(basic, sizeof(basic), Bafang::PrintCallback, (void *)g_CurrentChannel, 500);
            Bafang::QueueMotor(pedal, sizeof(pedal), Bafang::PrintCallback, (void *)g_CurrentChannel, 500);
            Bafang::QueueMotor(throttle, sizeof(throttle), Bafang::PrintCallback, (void *)g_CurrentChannel, 500);
        } break;
    }
}

}

void setup()
{
    Main::init();
}

void loop()
{
    Main::loop();
}