#include <QMC5883LCompass.h>
#include <LiquidCrystal_I2C.h>
#include <SPI.h>
#include <SD.h>

/*  SD card
 ** MOSI - pin 11
 ** MISO - pin 12
 ** CLK - pin 13
 ** CS - pin 10
*/

#define pi              3.14159
//***  Define pin  *********************
#define button_left_pin     2   //  Pin left button
#define button_right_pin    3   //  Pin right button
#define switch_lock_pin     4   //  Pin lock switch
#define buzzer_pin          5   //  Pin buzzer
//***  Define pin  *********************
/*#define serial_gps_tx       0   // Arduino TX pin
#define serial_gps_rx       1   // Arduino RX pin
#define button_left_pin     2   //  Pin left button
#define button_right_pin    3   //  Pin right button
#define switch_lock_pin     6   //  Pin lock switch
#define buzzer_pin          7   //  Pin buzzer
*/
//**************************************
#define button_delay        100
#define indicator_bar       7
#define indicator_res       6

//***  Calibration value  **************
//int compassMinX            = -945;   //  Compass X min
//int compassMaxX            = 1183;   //  Compass X max
//int compassMinY            = -2043;  //  Compass Y min
//int compassMaxY            = 643;    //  Compass Y max
//**************************************

struct button {
  bool down;
  bool hold;
  unsigned long time;
  };
struct gps {
  float latitude;
  float longitude;
  };

byte arrow[] = {
  B00100,
  B01110,
  B11111,
  B11111,
  B11111,
  B11111,
  B11011,
  B10001
  };
byte arrow_small_left[] = {
  B00000,
  B00000,
  B11110,
  B11000,
  B10100,
  B10010,
  B00001,
  B00000
  };
byte arrow_small_right[] = {
  B00000,
  B00000,
  B01111,
  B00011,
  B00101,
  B01001,
  B10000,
  B00000
  };
byte arrow_left[] = {
  B00000,
  B00100,
  B01000,
  B11111,
  B01000,
  B00100,
  B00000,
  B00000
  };
byte arrow_right[] = {
  B00000,
  B00100,
  B00010,
  B11111,
  B00010,
  B00100,
  B00000,
  B00000
  };

LiquidCrystal_I2C lcd(0x27, 16, 2);
QMC5883LCompass compass;
File file;

// $GPRMC,152452.00,A,5025.49865,N,01548.46721,E,1.593,258.09,070223,,,A*6D

bool statusOK = false;
gps gps_actual, gps_set;
gps waypoints_array[5];// = { 
//  {50.348617, 15.912439},
//  {50.425124, 15.813763},
//  {50.402867, 15.876595}
//  };
button button_left;
button button_right;

int dir, dist;
int compass_data;
byte waypoint = 0;
bool switch_lock_state, switch_lock_old = digitalRead(switch_lock_pin) ? 0 : 1;


//***  SETUP  ******************************************************************************************************************
void setup() {
  Serial.begin(9600);
  pinMode(button_left_pin,  INPUT_PULLUP);
  pinMode(button_right_pin, INPUT_PULLUP);
  pinMode(switch_lock_pin,  INPUT_PULLUP);
  pinMode(buzzer_pin,       OUTPUT);
  pinMode(A0, INPUT); //test compass

  compass.init();
//  compass.setSmoothing(10,true);
  compass.setCalibration(366, 1783, -2427, -756, -4806, -4412);

  lcd.begin(16,2);
  lcd.init();
  lcd.backlight();

  lcd.createChar(1, arrow);
  lcd.createChar(2, arrow_small_left);
  lcd.createChar(3, arrow_small_right);
  lcd.createChar(4, arrow_left);
  lcd.createChar(5, arrow_right);

  if (!SD.begin(10)) error(1);
  fileGen();
  fileRead();
  if (waypoint == 0) error(2);
}
//***  LOOP  ******************************************************************************************************************
void loop() {
  gpsRead();

  if (statusOK) {
    switch_lock_state = digitalRead(switch_lock_pin);
    if (switch_lock_state != switch_lock_old) {
      if (!switch_lock_state) {     // set -> run
        gps_set = waypoints_array[waypoint];
        Serial.print("Actual: ");
        Serial.print(gps_actual.latitude, 7);
        Serial.print("N, ");
        Serial.print(gps_actual.longitude, 7);
        Serial.println("E");
        Serial.print("Load:   ");
        Serial.print(gps_set.latitude, 7);
        Serial.print("N, ");
        Serial.print(gps_set.longitude, 7);
        Serial.println("E");
      }
      lcd.clear();
    }
    if (!switch_lock_state) {     // Run mode
      compass_data = map(analogRead(A0), 0, 1023, 0, 359);
  //    compass_data = readCompass();
      dir = compassCourse(compass_data, course(gps_actual.latitude, gps_actual.longitude, gps_set.latitude, gps_set.longitude));
      dist = distance(gps_actual.latitude, gps_actual.longitude, gps_set.latitude, gps_set.longitude);
      displayRun(dir, dist);
    } else {                      // Set mode
      waypoint_select();
    }
    switch_lock_old = switch_lock_state;
  } else {
    lcd.setCursor(0, 0);
    lcd.print("Cekam na signal ");
    lcd.setCursor(0, 1);
    lcd.print("      GPS       ");
  }
}
//*****************************************************************************************************************************
void error(int nr) {
  lcd.clear();
  switch (nr) {
    case 1: lcd.print("    SD error    "); break;
    case 2: lcd.print("Wrong data on SD"); break;
  }
  while (1);
}
void waypoint_select() {
  keyDetect();
  if (button_left.down && waypoint > 0) {
    button_left.down = false;
    waypoint--;
  }
  if (button_right.down && waypoint < 20) {
    button_right.down = false;
    waypoint++;
  }

  char line[17];
  lcd.setCursor(0, 0);
  lcd.print("   Vyber bod    ");
  waypoint == 0 ? sprintf(line, "  Zpatky domu   ") : 
                  sprintf(line, "       %2d        ", waypoint);
  lcd.setCursor(0, 1);
  lcd.print(line);
}
//===============================================================================
void keyDetect() {
  bool button_left_state = (digitalRead(button_left_pin) == 0 ? 1 : 0);
  bool button_right_state = (digitalRead(button_right_pin) == 0 ? 1 : 0);
  unsigned long time = millis();

  if (button_left_state && button_right_state) {
    button_left.down = false;
    button_right.down = false;
    button_left.hold = true;
    button_right.hold = true;
  } else {
    //---  Left button  -----------------------------------
    if (time > (button_left.time + button_delay)) {
      if (button_left.hold && !button_left_state) {
        button_left.hold = false;
        button_left.down = false;
      }
      if (button_left_state && !button_left.hold) {
        button_left.down = true;
        button_left.hold = true;
        button_left.time = millis();
      }
    }
    //---  Right button  ----------------------------------
    if (time > (button_right.time + button_delay)) {
      if (button_right.hold && !button_right_state) {
        button_right.hold = false;
        button_right.down = false;
      }
      if (button_right_state && !button_right.hold) {
        button_right.down = true;
        button_right.hold = true;
        button_right.time = millis();
      }
    }
  }  
}
//===============================================================================
void displayRun(int m, int dist) {
  char line_0[17];    // +1 zakoncovaci byte /0
  char line_1[17];
  for (int i = 0; i < 16; i++) line_0[i] = (char)32;
  
  if (m < 0) m > -indicator_bar ? line_0[(indicator_bar + 1) - m] = 3 : line_0[indicator_bar * 2 + 1] = 5;
  if (m > 0) m <  indicator_bar ? line_0[indicator_bar - m]       = 2 : line_0[0]  = 4;
  if (m == 0) { line_0[indicator_bar] = 1; line_0[indicator_bar + 1] = 1;}

  lcd.setCursor(0, 0);
  lcd.print(line_0);

  sprintf(line_1, "Vzdalenost:%4dm", dist);

  lcd.setCursor(0, 1);
  lcd.print(line_1);
}
//===  Read compass azimuth  ==================================================================================================
int readCompass() {
  compass.read();
  int azimuth = compass.getAzimuth(); // Calculated from X and Y value 
  if (azimuth < 0) azimuth += 360;
  return azimuth;
}
//===  Unit transfers  ========================================
float deg2rad(float n) {
  return n * pi / 180;
}
float rad2deg(float n) {
  return n / (pi / 180);
}
//===  Distance calculation  ========================================
int distance(float latitude1, float longitude1, float latitude2, float longitude2) {
  int r = 6371;   // Polomer zemekoule
  float width_diff = deg2rad(latitude1 - latitude2) / 2;
  float length_diff = deg2rad(longitude1 - longitude2) / 2;
  float a = sin(width_diff) * sin(width_diff) +
            sin(length_diff) * sin(length_diff) *
            cos(deg2rad(latitude1)) * cos(deg2rad(latitude2)); 
  return r * (2 * atan2(sqrt(a), sqrt(1 - a)) * 1000);  // Vzdalenost v metrech
}
//===  Course calculation  ========================================
int course(float latitude1, float longitude1, float latitude2, float longitude2) {
  latitude1  = deg2rad(latitude1);
  longitude1 = deg2rad(longitude1);
  latitude2  = deg2rad(latitude2);
  longitude2 = deg2rad(longitude2);
  float angle = atan2(sin(longitude2 - longitude1) * cos(latitude2),
            (cos(latitude1) * sin(latitude2)) - (sin(latitude1) * cos(latitude2) * cos(longitude2 - longitude1)));
  if (angle < 0) angle += 2 * pi;
  angle = rad2deg(angle);
  angle += 0.5;
  if (angle > 360) angle -= 360;
  return int(angle);
}
//===  Course indicator  =============================================================
int compassCourse(int courseC, int courseG) {
  int ret = courseC - courseG;
  if (ret < -179) ret += 360;
  if (ret > 179)  ret -= 360;
  ret /= indicator_res;
  if (ret <= -indicator_bar) return -indicator_bar;
  if (ret >= indicator_bar) return indicator_bar;
  return ret;
}
//====================================================================================
void serial_decode(String data, gps &gps_data, bool &status) {
  String data_array[15];
  int index = 0;

  for (int i = 0; i < data.length(); i++) {
    if (data[i] == ',') {
      index++;
    } else {
      data_array[index] += data[i];
    }
  }

  if (data_array[0] == "$GPRMC") {
      data_array[2] == "A" ? status = true : status = false;
      gps_data.latitude  = data_array[3].toFloat() / 100;
      gps_data.longitude = data_array[5].toFloat() / 100;
  }
}
//====================================================================================
void gpsRead() {
  String readData = "";
  if (Serial.available()) {
    readData = Serial.readString();
    serial_decode(readData, gps_actual, statusOK);
  }
}
//====================================================================================
//====================================================
void fileGen() {
  file = SD.open("gps.txt", FILE_WRITE);
  if (file) {
    file.println("50.848617, 15.012439");
    file.println("50.725124, 15.113763");
    file.println("50.602867, 15.276595");
    file.println("50.548617, 15.312439");
    file.println("50.425124, 15.413763");
//    file.println("50.302867, 15.576595");
//    file.println("50.248617, 15.612439");
//    file.println("50.125124, 15.713763");
//    file.println("50.025124, 15.813763");
//    file.println("50.902867, 15.976595");
    file.close();
    Serial.println("done.");
  } else {
    Serial.println("error create gps.txt");
  }
}
//====================================================
//====================================================
void fileRead() {
  file = SD.open("gps.txt");
  if (file) {
    if (file.available()) {
      int i = 0; 
      while (1) {  
        if (!fileReadFloat(waypoints_array[i].latitude, waypoints_array[i].longitude)) {
          Serial.print(i);
          Serial.println(" - error");
          break;
        }
        i++;
        if (i >= 50) break;
      }
      waypoint = i;
      for (int i = 0; i < 10; i++) {
        Serial.print(waypoints_array[i].latitude, 6);
        Serial.print(", ");
        Serial.println(waypoints_array[i].longitude, 6);
      }
    }
    file.close();
  } else {
    Serial.println("error opening test.txt");
  }
}
//====================================================
bool fileReadFloat(float &latitude, float &longitude) {
  String read_data = "";
  if (file.available()) {
    read_data = file.readStringUntil(',');
    latitude = read_data.toFloat();
  } else {
    return 0;
  }
  if (file.available()) {
    read_data = file.readStringUntil('\n');
    longitude = read_data.toFloat();
  } else {
    return 0;
  }
  return 1;
}
//====================================================