#include "TroykaButton.h"
// создаём объект для работы с кнопкой
TroykaButton button(A2);
// библиотека для работы с дисплеем
#include <QuadDisplay2.h>
// создаём объект класса QuadDisplay и передаём номер пина CS
QuadDisplay qd(10);
// библиотека для работы с GPS устройством
#include <TroykaGPS.h>
// создаём объект класса GPS и передаём в него объект Serial1 
GPS gps(Serial1);
// задаём размер массива для времени, даты, широты и долготы
#define MAX_SIZE_MASS 16
// макросы
#define SPEED         0
#define DISTANCE      1
// переменые для хранения текущей скорости и пройденного расстояния
float speed;
float distance = 0;
// переменные для хранения координат
float latitudeLast;
float longitudeLast;
// режим отображение: текущей скорости / пройденного расстояния 
bool state = SPEED;
// флаг первого считывания данных с GPS-модуля
bool firstStart = true;
 
void setup()
{ 
  // открываем последовательный порт для мониторинга действий в программе
  // для ревизии v1 модуля GPS:
//  Serial.begin(115200); 
  // для ревизии v2 модуля GPS:
  Serial.begin(9600);
//  Serial.print("Serial init OK\r\n");
  // открываем Serial-соединение с GPS-модулем
  // для ревизии v1 модуля GPS: 
//  Serial1.begin(115200); 
  // для ревизии v2 модуля GPS: 
  Serial1.begin(9600);
  // инициализация дисплея
  qd.begin();
  // инициализация кнопки
  button.begin();
}
 
void loop()
{
  // переменные для хранения текущих координат
  float latitude;
  float longitude;
  // считывание данных с кнопки
  button.read();
  // если был клик
  if (button.justPressed()) {
    // меняем отображение скорости на расстояния или наоборот
    state = !state;
    // в зависимости от текущего состояния спидометра
    if (!state) {
      qd.displayDigits(QD_S, QD_P, QD_E, QD_d);
    } else {
      qd.displayDigits(QD_d, QD_I, QD_S, QD_t);
    }
    delay(1000);
  }
    // если пришли данные с GPS-модуля
  if (gps.available()) {
    // считываем данные и парсим
    gps.readParsing();
    // считываем состояние GPS-модуля
    switch(gps.getState()) {
      // всё OK
      case GPS_OK:
        // считываем с GPS-модуля текушие координаты и скорость
        speed = gps.getSpeedKm();
        latitude = gps.getLatitudeBase10();
        longitude = gps.getLongitudeBase10();
        // если данные с GPS-модуля пришли впервые
        if (firstStart) {
          // сохраняем текущие значения координат
          latitudeLast = latitude;
          longitudeLast = longitude;
          // первый запуск был совершен
          firstStart = false;
        }
        distance = distance + latlng2distance(latitudeLast, longitudeLast, latitude, longitude);
        // сохраняем текущие значения координат
        latitudeLast = latitude;
        longitudeLast = longitude;
  /*      Serial.print(speed);
        Serial.print("\t");
        Serial.print(latitude, 6);
        Serial.print("\t");
        Serial.print(longitude, 6);
        Serial.print("\t"); 
        Serial.println(distance);*/
        // в зависимости от режима работы спидометра
        if(!state) {
          // выводим текущую скорость
          qd.displayFloat(speed, 1);
        } else {
          // выводим текущее расстояние
          qd.displayFloat(distance, 1);
        }
        break;
      // ошибка данных
      case GPS_ERROR_DATA:
        Serial.println("GPS error data");
        break;
      // нет соединение со спутниками
      case GPS_ERROR_SAT:
        Serial.println("GPS no connect to satellites");
        qd.displayDigits(QD_E, QD_r, QD_r, QD_NONE);
        break;
    }
  }
}
 
float latlng2distance(float lat1, float long1, float lat2, float long2) {
    //радиус Земли
    unsigned long R = 6372795;
    //перевод коордитат в радианы
    lat1 *= 3.14 / 180;
    lat2 *= 3.14 / 180;
    long1 *= 3.14 / 180;
    long2 *= 3.14 / 180;
    //вычисление косинусов и синусов широт и разницы долгот
    float cl1 = cos(lat1);
    float cl2 = cos(lat2);
    float sl1 = sin(lat1);
    float sl2 = sin(lat2);
    float delta = long2 - long1;
    float cdelta = cos(delta);
    float sdelta = sin(delta);
    //вычисления длины большого круга
    float y = sqrt(pow(cl2 * sdelta, 2) + pow(cl1 * sl2 - sl1 * cl2 * cdelta, 2));
    float x = sl1 * sl2 + cl1 * cl2 * cdelta;
    float ad = atan2(y, x);
    float dist = ad * R; //расстояние между двумя координатами в метрах
    return dist;
}