/*------------------------------------------------*/
// подключение библиотек для работы с OLED
#define SSD1306_128_64
#include <Adafruit_GFX.h>
//#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <math.h>
/*------------------------------------------------*/
#define OLED_RESET 7
//Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);
Adafruit_SSD1306 display(128, 64); // создаём объект "дисплей"
double angle = 0;
int count = -1;
int mode = 0;
int time = 0;
int cursor_location = 0;
int index = 0;
int k = 1;
int level = 5;
String number;
float MY_COORDS[2] {55.91879, 37.75404};
float TARGET_COORDS[2] {54.61590, 39.73647};
String symbols[2] = {"+", "-"};
byte nums[10] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
int coords[36][2];
int16_t coord[3] = {60, 30, 30};
/*------------------------------------------------*/
void setup() {
  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); 
  display.setTextSize(2); // установка размера шрифта
  display.setTextColor(WHITE); // инициализация дисплея по интерфейсу I2C, адрес 0x3C
  display.clearDisplay(); // очистка дисплея
  display.display(); // и её выводим на экран
  Serial.begin(115200);
  for (int i = 0; i < 360; i += 10)
  {
    count += 1;
    angle += 10;
    int x = coord[0] + cos(angle * 0.017) * coord[2];
    int y = coord[1] - sin(angle * 0.017) * coord[2];

    coords[count][0] = x;
    coords[count][1] = y;
  }
}
/*------------------------------------------------*/
int calc_angle(float llat1, float llong1, float llat2, float llong2)
{
  float lat1 = llat1 * 3.14 / 180;
  float lat2 = llat2 * 3.14 /180;
  float long1 = llong1 * 3.14 / 180;
  float long2 = llong2 * 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 x = (cl1 *  sl2) - (sl1 * cl2 * cdelta);
  float y = sdelta * cl2;
  float z = (atan(-y / x) * 180) / 3.14;
 
  if (x < 0)
  {
    z = z + 180;
  }

  float z2 = (int)(z + 180) % 360 - 180;
  z2 = - (z2 * 3.14) / 180;
  float anglerad2 = z2 - ((2 * 3.14) * floor((z2/(2 * 3.14))));
  float angledeg = (anglerad2 * 180) / 3.14;
  //Serial.println((360 - angledeg + 90) / 360);
  return (int)(((360 - angledeg + 90) - 360 * ((int)(360 - angledeg + 90) / 360)) / 10);
}
void mode1(bool draw = false)
{
  display.setCursor(0, 5);
  display.print("at ");
  display.setCursor(0, 30);
  display.print("lt ");
  
  //if (digitalRead(2) == LOW || draw)
  //{
    if (cursor_location == 0)
    {
      
      display.setTextColor(BLACK);
      display.setCursor(28 + cursor_location, level);
      display.print("+");
      if (index == 1)
      {
        index = 0;
      }
      else
      {
        index = 1;
      }
      display.setTextColor(WHITE);
      display.setCursor(28 + cursor_location, level);
      display.print(symbols[index]);
       
    }
    if(cursor_location != 0 || draw)
    {
      display.setTextColor(BLACK);
      display.setCursor(28 + cursor_location, level);
      display.print(nums[index]);
      if (index == 9)
      {
        index = 0;
      }
      else
      {
        index += 1;
      }
      display.setTextColor(WHITE);
      display.setCursor(28 + cursor_location, level);
      display.print(nums[index]); 
    }
    display.display();
  //}
  display.display();
}
void mode0()
{
  display.clearDisplay();
  //display.drawLine(coord[0], coord[1], coords[count][0], coords[count][1], BLACK);
  count += k;
  display.drawCircle(coord[0], coord[1], coord[2], WHITE);
  /*
  if (count == 36 && mode == 0)
  {
    count = 0;
  }
  if (count == -1 && mode == 1)
  {
    count = 35;
  }*/
  int ind = calc_angle(MY_COORDS[0], MY_COORDS[1], TARGET_COORDS[0], TARGET_COORDS[1]) - 1;
  display.drawLine(coord[0], coord[1], coords[ind][0], coords[ind][1], WHITE);
  //Serial.println(coords[0][0]);
  display.display();
}
void loop() {
  //TARGET_COORDS[1] += 1;
  if (mode == 0)
  {
    mode0();
  }
  
  if (digitalRead(3) == LOW)
  {
    if(mode == 1)
    {
      if (cursor_location == 0)
        {
          number += symbols[index];
        }
        else
        {
          number += String(nums[index]);
        }
      index = 0;
      cursor_location += 12;
      if (cursor_location == 36)
      {
        number += ".";
        display.drawLine(62, 13 + level, 62, 12 + level, WHITE);
        display.drawLine(63, 12 + level, 63, 13 + level, WHITE);
      }
      if (cursor_location == 96)
      {
        
        
        if (level == 5)
        {
          TARGET_COORDS[0] = number.toFloat();
          cursor_location = 0;
          level = 30;
          mode1();
        }
        else
        {
          TARGET_COORDS[1] = number.toFloat();
          display.clearDisplay();
          Serial.println(TARGET_COORDS[0], 5);
          Serial.println(TARGET_COORDS[1], 5);
          mode = 0;
          k = 1;
          level = 5;
        }
        number = "";
        
      }
      else
      {
        
        mode1(true);
      }
      
      delay(100);
    }
    
    //Serial.println(index);
  }
  if (digitalRead(2) == LOW)
  {
        
        if (mode == 1)
        {
          mode1();
          delay(100);
        }
        time += 1;
        if (time == 10)
        {
          if (mode == 0)
          {
            mode = 1;
            level = 5;
            cursor_location = 0;
            display.clearDisplay();
            k = -1;
          }
          else
          {
            mode = 0;
            k = 1;
          }
        }
        
        
  }
  else
  {
        time = 0;
  }
  
  //delay(500);
  
}
/*------------------------------------------------*/