// #include <Arduino.h>

const uint8_t btnPin = 2;
const uint8_t ledPin = 13;
const uint8_t ledYell = 4;
bool ledState = false;
volatile int number = 0;


// GPS distance/ Read gps.txt file

float RADIUS = 6371.01;

// # Kiev
float lat1 = 50.45;
float lon1 = 30.523;
// # Rokitno
float lat2 = 49.69;
float lon2 = 30.47;

// double Convert(double degree)
// {
//     double pi = 3.14159265359;
//     return (degree * (pi / 180));
// }

float convertToRadian(float degree)
{
    float pi = 3.14159265359;
    return (degree * (pi / 180));
}



// === Крутейшая реализация Printf // З ESP32 не працює! !!!
  FILE f_out;
  int sput(char c, __attribute__((unused)) FILE* f) {return !Serial.write(c);}




void foo(char* t, char* tt){
  for(uint8_t i = 0; i<9; i++){
    printf("%d  %x  %x\n", i, *(t+i), *(tt+i));   
  }
  printf("strlen t = %d, strlen tt = %d\n", strlen(t), strlen(tt));
}


void setup() {
  Serial.begin(9600);
  
  fdev_setup_stream(&f_out, sput, nullptr, _FDEV_SETUP_WRITE); // cf https://www.nongnu.org/avr-libc/user-manual/group__avr__stdio.html#gaf41f158c022cbb6203ccd87d27301226
  stdout = &f_out;

  pinMode(ledPin, OUTPUT);
  pinMode(ledYell, OUTPUT);
  pinMode(btnPin, INPUT_PULLUP);

 

// GPS distance

// print(f"lat1 rad {lat1}") //lat1 rad 0.8805186076311393
// print(f"lon1 rad {lon1}") // lon1 rad 0.5327268475862292
// print(f"lat2 rad {lat2}") // lat2 rad 0.8672541053159823
// print(f"lon2 rad {lon2}") // lon2 rad 0.5318018230826722

  Serial.println(lat1);  // 50.45
  Serial.println(lon1);  // 30.523
  Serial.println(lat2); // 49.69
  Serial.println(lon2); //49.69
  
  float lat1_rad = convertToRadian(lat1);
  // printf("lat1 dgr %d = %d rad", lat1, lat1_rad);
  printf("lat1_rad = ");
  Serial.println(lat1_rad, 7); // 0.8805187

  float lon1_rad = convertToRadian(lon1);
  printf("lon1_rad = ");
  Serial.println(lon1_rad, 7); // 0.5327269

  float lat2_rad = convertToRadian(lat2);
  printf("lat1_rad = ");
  Serial.println(lat2_rad, 7); // 0.8672541

  float lon2_rad = convertToRadian(lon2);
  printf("lon2_rad = ");
  Serial.println(lon2_rad, 7); // 0.5318018
 

  // float sin(float x);  // C++ only
  // long double sin(long double x);  // C++ only

  //mySinus = math.sin(lat1)      // Python 0.7710692066831263
  float mySinuslat1 = sin(lat1_rad);  // 0.7710692
  Serial.println(mySinuslat1, 7);

  // float acos (float x)
  // float distance =  RADIUS * math.acos(math.sin(lat1) * math.sin(lat2) + math.cos(lat1) * math.cos(lat2) * math.cos(lon1 - lon2)) // Python
  float distance =  RADIUS * acos(sin(lat1_rad) * sin(lat2_rad) + cos(lat1_rad) * cos(lat2_rad) * cos(lon1_rad - lon2_rad)); // C++
  Serial.print("distance Kiev - Rokitno = ");  // Python 84.59288492001667
  Serial.println(distance, 4); //C++ = 84.5961

}








void loop() {
  ledState = !ledState;
  Serial.println(number);
  digitalWrite(ledPin, ledState);
  delay(1000);
  number ++;
}