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

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

WiFiServer server(23);
WiFiClient serverClient;

BluetoothSerial SerialBT;
DummyStream SerialDummy;

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

namespace Main
{

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

unsigned long g_Time1000;


void init()
{
    Serial.begin(115200);
    while(!Serial);
    Serial.println("BOOTED!");

    Serial1.begin(115200);
    Serial2.begin(115200);

    WiFi.setHostname("ESP32-BAFANG");
    if(!WiFi.begin(g_WIFI_SSID, g_WIFI_Passphrase))
        Serial.println("WiFi config error!");
    else {
        WiFi.setAutoConnect(true);
    }

    SerialBT.begin("ESP32-BAFANG");

    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);
    })
    .onEnd([]() {
        Serial.println("\nEnd");
    })
    .onProgress([](unsigned int progress, unsigned int total) {
        Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
    })
    .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");
    });

    ArduinoOTA.begin();

    server.begin();
    server.setNoDelay(true);


}

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

    if(num == BTSERIAL && SerialBT.hasClient())
        return &SerialBT;
    else if(num == TCPSERIAL && serverClient)
        return &serverClient;
    else if(num == HWSERIAL)
        return &Serial;

    return &SerialDummy;
}

void loop()
{
    if(g_Debug[g_CurrentChannel])
    {
        while(Serial1.available())
        {
                channel()->write(Serial1.read());
        }

        while(Serial2.available())
        {
            channel()->write(Serial2.read());
        }
    }

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

    if((millis() - g_Time1000) > 1000)
    {
        g_Time1000 = millis();
    }
}

}

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

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