#include <ESP8266WiFi.h>
#include <ESP8266mDNS.h>
#include <ArduinoOTA.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <LSM303.h>

//Wektory
struct Vector2
{
    double x;
    double y;
};

//Sieć połączeniowa
const char* SSID = "Robonomik";
const char* PASSWORD = "WojewodaA1";

//Połączenie GPS z ESP RX TX
SoftwareSerial gps_uart (0, 16);

SoftwareSerial bt_serial(D5,D6); // RX, TX

//Zmienne dla GPS
double robotLatitude = 0;
double robotLongitude = 0;
double robotHeight = 0;
String GPS_data = "";
String GP_GGA = "";

//Pozycje robota, smartfona - x y
struct Vector2 robotPositionGlobal;
struct Vector2 robotPositionLocal;
struct Vector2 smartfonPositionGlobal;
struct Vector2 smartfonPositionLocal;
struct Vector2 virtualNorth;

//Właściwości wektorów
double vectorRSMagnitude = 0;
double vectorNorthMagnitude = 0;
double vectorRSAngle = 0;
double robotAngle = 0;

//Protokół I2C
int ESC1Address = 6;
int ESC2Address = 7;
unsigned long I2CsendRate = 50;    //Czas w milisekundach pomiędzy wysyłaniem
unsigned long I2Cmotor1 = 0;
unsigned long I2Cmotor2 = 0;

//Kompas
LSM303 compass;

//Smartfon data - sterowanie
String WIFI_data = "1,9479323.47395,9275956.53963,0,1,243,232,";
bool manualControl = true;
bool robotDirectionMotor1 = true;
bool robotDirectionMotor2 = true;
bool robotAlarm = false;
int robotMotor1Val = 0;
int robotMotor2Val = 0;

//Debug
unsigned long currentMillis = 0;
unsigned long prevMillis = 0;

double deg2rad(double deg)
{
    //Zmiana stopni na radiany
    double rad = deg * PI / 180;
    return rad;
}
double rad2deg(double rad)
{
    //Zmiana radiany na stopnie
    double deg = rad * 180 / PI;
    return deg;
}
void WiFi_settings()
{
    //Ustawienia WiFi
    WiFi.mode(WIFI_STA);
    WiFi.begin(SSID, PASSWORD);

    //Czekanie na połączenie
    if (WiFi.waitForConnectResult() != WL_CONNECTED)
    {
        Serial.println("Nieudane połączenie do sieci! Restartowanie...");
        ESP.restart();
    }
    else if (WiFi.waitForConnectResult() == WL_CONNECTED)
    {
        Serial.print("Połączono do sieci: ");
        Serial.println(WiFi.SSID());
        Serial.print("Adres IP: ");
        Serial.println(WiFi.localIP());
    }
}
void OTA_updater()
{
    //Ustawianie nazwy ESP8266 w sieci
    ArduinoOTA.setHostname("ESP8266-Przewodnik");

    //Rebootowanie ESP po aktualizacji
    ArduinoOTA.setRebootOnSuccess(true);

    //Ustawienia ESP na start aktualizacji
    ArduinoOTA.onStart([](){
        Serial.println("Wgrywanie...");
        // Tutaj ustawić wszystko nieruchomo żeby łazik podczas aktualizacji nie zaczął jechać bez kontroli
    });

    //Ustawienia ESP po zakończeniu aktualizacji
    ArduinoOTA.onEnd([](){
        Serial.println("Wgrano program!");
    });

    //Ustawienia ESP gdy nastapi błąd podczas aktualizacji
    ArduinoOTA.onError([](ota_error_t error){
        Serial.print("Podczas aktualizacji wystąpił błąd: ");
        if(error == OTA_AUTH_ERROR)
        {
            Serial.println("AUTH_ERROR");
        }
        else if (error == OTA_BEGIN_ERROR)
        {
            Serial.println("BEGIN_FAILED");
        }
        else if (error == OTA_CONNECT_ERROR)
        {
            Serial.println("CONNECT_ERROR");
        }
        else if (error == OTA_RECEIVE_ERROR)
        {
            Serial.println("RECEIVE_ERROR");
        }
        else if (error == OTA_END_ERROR)
        {
            Serial.println("END_ERROR");
        }
    });

    //Inicjalizacja OTA
    ArduinoOTA.begin();
    Serial.println("Gotowy!");
}
void geo_to_cartesian(double lati, double longi, double hght, double *x, double *y)
{
  //Średnie promienie ziemi a i b
  double equatorialRadius = 6378137.0;
  double polarRadius = 6356752.314245;

  //Zmiana latitude i longitude z stopni na radiany
  lati = deg2rad(lati);
  longi = deg2rad(longi);

  //Przeliczanie z geo na XYZ
  double eToSquare = 1 - (pow(polarRadius, 2) / pow(equatorialRadius, 2));
  double Nlati = equatorialRadius / sqrt(1 - eToSquare * pow(sin(lati), 2));

  //Przypisywanie wartości
  *x = (Nlati + hght) * cos(lati) * cos(longi);
  *y = (Nlati + hght) * cos(lati) * sin(longi);
  //Opcjonalne Z
  //double z = ((pow(polarRadius, 2) / pow(equatorialRadius, 2)) * Nlati + hght) * sin(lati);
}
void GPS_get_postion(double *setLati, double *setLongi, double *setHght)
{
    if(gps_uart.available())
    {
        GPS_data = gps_uart.readStringUntil('\n');

        if(GPS_data.indexOf("$GPGGA") > -1)
        {
            //Wyznaczanie końca lini
            GP_GGA = GPS_data.substring(GPS_data.indexOf("$GPGGA"), GPS_data.indexOf("*") + 3);

            int colon_array[15];
            for (int i = 1; i < 15; i++)
            {
                colon_array[i] = GPS_data.indexOf(',', colon_array[i - 1] + 1);
            }

            //Ustawienia latitude
            if (GP_GGA.substring(colon_array[2], colon_array[3]) != ",")
            {
                *setLati = (GP_GGA.substring(colon_array[2] + 1, colon_array[3])).toDouble() / 100;
            }
            else
            {
                *setLati = 0;
            }

            //Ustawianie longitude
            if (GP_GGA.substring(colon_array[4], colon_array[5]) != ",")
            {
                *setLongi = (GP_GGA.substring(colon_array[4] + 1, colon_array[5])).toDouble() / 100;
            }
            else
            {
                *setLongi = 0;
            }

            //Ustawianie Height
            if (GP_GGA.substring(colon_array[9], colon_array[10]) != ",")
            {
                *setHght = (GP_GGA.substring(colon_array[9] + 1, colon_array[10])).toDouble();
            }
            else
            {
                *setHght = 0;
            }
        }
        // Serial.print("Latitude: ");
        // Serial.println(*setLati, 7);
        // Serial.print("Longitude: ");
        // Serial.println(*setLongi, 7);
        // Serial.print("Height: ");
        // Serial.println(*setHght, 7);
        // Serial.println("");
    }
}
void get_local_postion()
{
    robotPositionLocal.x = 0;
    robotPositionLocal.y = 0;
    smartfonPositionLocal.x = robotPositionGlobal.x - smartfonPositionGlobal.x;
    smartfonPositionLocal.y = robotPositionGlobal.y - smartfonPositionGlobal.y;
}
void get_vectorRS()
{
    //Ustalanie pozycji wirtualnej północy
    virtualNorth.x = 0;
    virtualNorth.y = 3;

    //Wyznaczanie długości wektora
    vectorRSMagnitude = sqrt(pow((smartfonPositionLocal.x - robotPositionLocal.x), 2) + pow((smartfonPositionLocal.y - robotPositionLocal.y), 2));
    vectorNorthMagnitude = sqrt(pow((virtualNorth.x - robotPositionLocal.x), 2) + pow((virtualNorth.y - robotPositionLocal.y), 2));

    //Obliczanie kątu wektora RS z wektorem North
    double vectorRSdot = (smartfonPositionLocal.x * virtualNorth.x) + (smartfonPositionLocal.y * virtualNorth.y);
    if(smartfonPositionLocal.x >= 0)
    {
        vectorRSAngle = rad2deg(acos(vectorRSdot / (vectorRSMagnitude * vectorNorthMagnitude)));
    }
    else
    {
        vectorRSAngle = -rad2deg(acos(vectorRSdot / (vectorRSMagnitude * vectorNorthMagnitude)));
    }
}
double read_robot_angle()
{
    compass.read();
    double angle = compass.heading();
    return angle;
}
void get_interface_info(String interface_data, bool *manualControl, double *smartfonX, double *smartfonY, bool *moto1Dir, bool *motor2Dir, int *motor1Value, int *motor2Value, bool *alarm)
{
    //GPS czy sterowanie ręczne
    if(interface_data.substring(0, 1) == "1")
    {
        *manualControl = true;
    }
    else if(interface_data.substring(0, 1) == "0")
    {
        *manualControl = false;
    }
    else
    {
        *alarm = true;
    }
    //Siatka karezjańska
    //X
    *smartfonX = interface_data.substring(2, 15).toDouble();
    //Y
    *smartfonY = interface_data.substring(16, 29).toDouble();

    //Kierunek motor1 1 - przod, 0 - tył
    if(interface_data.substring(30, 31) == "1")
    {
        *moto1Dir = true;
    }
    else if(interface_data.substring(30, 31) == "0")
    {
        *moto1Dir = false;
    }
    else
    {
        *alarm = true;
    }
    //Kierunek motor2 1 - przod, 0 - tył
    if(interface_data.substring(32, 33) == "1")
    {
        *motor2Dir = true;
    }
    else if(interface_data.substring(32, 33) == "0")
    {
        *motor2Dir = false;
    }
    else
    {
        *alarm = true;
    }

    //Motor1 Power
    if(interface_data.substring(34, 37).toInt() >= 0 && interface_data.substring(34, 37).toInt() <= 255)
    {
        *motor1Value = interface_data.substring(34, 37).toInt();
    }
    else
    {
        *alarm = true;
    }

    //Motor2 Power
    if(interface_data.substring(38, 41).toInt() >= 0 && interface_data.substring(38, 41).toInt() <= 255)
    {
        *motor2Value = interface_data.substring(38, 51).toInt();
    }
    else
    {
        *alarm = true;
    }
}
void I2C_send(int address, int value)
{
    //Wysyłanie wiadomości
    Wire.beginTransmission(address);
    Wire.write(value);
    Wire.endTransmission(address);
}
void motor_send(int motorAdr, int value, bool direction, unsigned long *prevTime)
{
    if(currentMillis - *prevTime >= I2CsendRate)
    {
        I2C_send(motorAdr, direction);
        I2C_send(motorAdr, value);
        *prevTime = currentMillis;
    }
}

String read_bt(){
    String bt_message = "";
    if (bt_serial.available())
    {
        bt_message = bt_serial.readStringUntil('\n');
    }

    if (bt_message.length() > 0)
    {
        return bt_message;
    }

}
void setup()
{
    //Ustawienia USB-UART
    Serial.begin(115200);
    Serial.println("Inicjalizacja ESP8266!");

    //Połączenie GPS z ESP
    gps_uart.begin(9600);

    bt_serial.begin(9600);

    //I2C Protokół
    Wire.begin();

    //Kompas
    compass.init();
    compass.enableDefault();
    compass.m_min = (LSM303::vector<int16_t>){-2453, -2735, -2865};
    compass.m_max = (LSM303::vector<int16_t>){+3635, +3145, +3089};

    WiFi_settings();
    OTA_updater();
}
void loop()
{
    currentMillis = millis();
    ArduinoOTA.handle();
    get_interface_info(WIFI_data, &manualControl, &smartfonPositionGlobal.x, &smartfonPositionGlobal.y, &robotDirectionMotor1, &robotDirectionMotor2, &robotMotor1Val, &robotMotor2Val, &robotAlarm);
    if(currentMillis - prevMillis >= 500)
    {
        Serial.print("Manual Control: ");
        Serial.println(manualControl);
        Serial.print("Smartfon X global: ");
        Serial.println(smartfonPositionGlobal.x, 6);
        Serial.print("Smartfon Y global: ");
        Serial.println(smartfonPositionGlobal.y, 6);
        Serial.print("Front/Back-ward motor1: ");
        Serial.println(robotDirectionMotor1);
        Serial.print("Front/Back-ward motor2: ");
        Serial.println(robotDirectionMotor2);
        Serial.print("Motor1: ");
        Serial.println(robotMotor1Val);
        Serial.print("Motor2: ");
        Serial.println(robotMotor2Val);
        Serial.println("");
        prevMillis = currentMillis;
    }

    if(manualControl == true)
    {
        motor_send(ESC1Address, robotMotor1Val, robotDirectionMotor1, &I2Cmotor1);
        motor_send(ESC2Address, robotMotor2Val, robotDirectionMotor2, &I2Cmotor2);
    }
    if(robotAlarm)
    {
        I2C_send(ESC1Address, 0);
        I2C_send(ESC2Address, 0);
    }
}