#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
// https://learn.adafruit.com/adafruit-gfx-graphics-library/rotating-the-display
// https://javl.github.io/image2cpp/
// https://toptechboy.com/lesson-24-understanding-gps-nmea-sentences/
//#include <avr/pgmspace.h>
#include <SoftwareSerial.h>

#define GPSPIN1_PIN 4
#define GPSPIN2_PIN 3

SoftwareSerial gps(GPSPIN1_PIN, GPSPIN2_PIN);
char dato=' ';


#define BUTTON1_PIN 7
#define BUTTON2_PIN 6
#define LED_PIN 13

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define VIEW_WIDTH    128
#define VIEW_HEIGHT   64
#define BRUJULA_WIDTH    22
#define BRUJULA_HEIGHT   22
#define scaleX 50
#define scaleY 40

#define DSCREEN 0x3C //Dirección de comunicacion: 0x3D para 128x64, 0x3C para 128x32

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

float origin_lat = 37.862133;
float origin_lon = -6.825711;  

const unsigned char PROGMEM brujula[] = {
	0x00, 0x00, 0x40, 0x01, 0xc1, 0x90, 0x07, 0xe3, 0x60, 0x03, 0xf6, 0xd0, 0x03, 0xfb, 0xa8, 0x07, 
	0xf0, 0xe0, 0x07, 0xe0, 0x10, 0x07, 0xf0, 0x00, 0x05, 0xe0, 0x00, 0x02, 0xf1, 0xc0, 0x07, 0xd7, 
	0x80, 0x06, 0xfe, 0x00, 0x04, 0x7a, 0x00, 0x0e, 0x71, 0x00, 0x0c, 0x77, 0x80, 0x02, 0x7f, 0x80, 
	0x00, 0x3b, 0x00, 0x00, 0x77, 0xe0, 0x07, 0xe3, 0xc0, 0x0f, 0xc0, 0x00, 0x19, 0x80, 0x00, 0x18, 
	0x00, 0x00
};

const char* const PROGMEM trackPlaces[] = {
  "los marines", "fuente heridos", "cortelazor", "valdelarco", "galaroza", "la nava", "cortegana"
};

const int PROGMEM Itrack[] = {
140, 4, 142, 6, 148, 4, 149, 5, 158, 2, 161, 2, 162, 1, 168, 1, 170, 0, 
173, 1, 171, 4, 170, 4, 168, 7, 168, 8, 164, 12, 163, 14, 160, 19, 157, 23, 
155, 23, 153, 22, 149, 26, 140, 29, 136, 32, 132, 34, 132, 34, 128, 38, 128, 39, 
129, 45, 130, 52, 132, 52, 134, 51, 136, 52, 140, 56, 142, 57, 142, 59, 140, 60, 
138, 66, 139, 70, 139, 74, 140, 76, 139, 80, 139, 84, 141, 86, 137, 95, 136, 99, 
133, 102, 132, 102, 131, 105, 130, 103, 130, 105, 128, 107, 128, 111, 131, 114, 136, 117, 
139, 115, 141, 109, 143, 106, 142, 105, 144, 105, 145, 104, 146, 105, 148, 101, 147, 99, 
150, 93, 149, 92, 151, 85, 155, 84, 158, 81, 158, 80, 161, 75, 163, 74, 164, 75, 
165, 75, 167, 72, 170, 74, 171, 73, 178, 69, 178, 64, 180, 63, 180, 60, 183, 55, 
185, 54, 186, 54, 189, 51, 197, 48, 196, 52, 201, 48, 203, 48, 206, 43, 207, 45, 
208, 45, 211, 41, 213, 35, 218, 33, 218, 33, 219, 35, 224, 35, 225, 34, 223, 29, 
223, 27, 222, 26, 222, 25, 222, 25, 223, 25, 225, 27, 230, 29, 234, 31, 237, 36, 
245, 39, 247, 39, 249, 41, 253, 40, 253, 41, 254, 42, 255, 41, 257, 42, 263, 39, 
265, 40, 267, 37, 270, 38, 273, 36, 277, 36, 277, 38, 279, 40, 279, 41, 283, 39, 
284, 40, 284, 38, 286, 38, 287, 41, 291, 44, 294, 48, 294, 47, 296, 50, 298, 48, 
301, 49, 303, 46, 307, 45, 311, 45, 314, 48, 317, 49, 320, 47, 326, 51, 331, 49, 
335, 50, 340, 52, 341, 54, 343, 53, 345, 55, 345, 56, 347, 55, 351, 60, 355, 62, 
356, 61, 356, 62, 363, 68, 366, 68, 366, 71, 367, 71, 366, 75, 362, 78, 361, 76, 
359, 78, 359, 79, 358, 79, 357, 79, 352, 87, 351, 87, 348, 90, 343, 93, 337, 92, 
335, 89, 332, 88, 327, 89, 324, 91, 323, 90, 320, 92, 315, 92, 310, 93, 299, 102, 
298, 105, 296, 106, 292, 111, 286, 112, 283, 115, 281, 101, 283, 99, 282, 94, 281, 94, 
281, 90, 280, 91, 279, 89, 275, 91, 274, 94, 270, 97, 268, 98, 268, 99, 265, 99, 
264, 98, 263, 99, 261, 98, 255, 100, 251, 100, 250, 99, 248, 100, 243, 99, 242, 96, 
240, 96, 240, 93, 239, 92, 229, 92, 232, 97, 235, 100, 236, 103, 236, 105, 233, 109, 
233, 113, 235, 116, 236, 115, 238, 116, 239, 115, 241, 118, 245, 119, 245, 126, 245, 127, 
248, 133, 251, 132, 254, 132, 255, 137, 258, 140, 258, 144, 264, 143, 269, 145, 270, 146, 
269, 149, 270, 151, 270, 152, 272, 154, 275, 155, 275, 158, 277, 162, 274, 165, 273, 171, 
268, 172, 266, 171, 264, 172, 262, 171, 262, 173, 262, 174, 258, 171, 254, 171, 255, 171, 
253, 174, 251, 173, 250, 172, 249, 171, 244, 178, 242, 183, 243, 184, 242, 189, 238, 190, 
238, 191, 234, 193, 232, 191, 230, 192, 229, 191, 226, 194, 223, 192, 218, 193, 216, 195, 
213, 196, 211, 195, 206, 197, 205, 197, 204, 198, 200, 197, 199, 195, 198, 195, 197, 196, 
197, 195, 195, 193, 192, 188, 192, 177, 192, 176, 192, 174, 189, 172, 186, 169, 184, 169, 
184, 168, 182, 168, 182, 166, 175, 162, 174, 159, 172, 158, 170, 158, 169, 154, 170, 153, 
168, 149, 167, 148, 168, 147, 167, 145, 166, 144, 166, 145, 160, 145, 157, 143, 156, 143, 
156, 145, 155, 146, 155, 149, 150, 154, 149, 154, 145, 160, 146, 162, 145, 165, 147, 167, 
149, 170, 151, 171, 150, 170, 151, 173, 149, 176, 148, 180, 149, 184, 148, 189, 149, 191, 
147, 196, 145, 196, 142, 200, 140, 200, 139, 204, 139, 208, 137, 210, 136, 213, 134, 213, 
128, 218, 127, 221, 123, 223, 120, 222, 119, 223, 118, 222, 114, 224, 113, 223, 114, 224, 
113, 225, 114, 224, 113, 223, 102, 231, 98, 233, 96, 228, 94, 227, 89, 221, 89, 219, 
87, 218, 83, 212, 83, 210, 81, 210, 79, 209, 77, 205, 77, 203, 75, 200, 76, 199, 
75, 196, 70, 192, 67, 191, 65, 189, 64, 189, 62, 183, 60, 183, 56, 179, 53, 180, 
50, 178, 49, 176, 44, 175, 44, 173, 42, 172, 39, 169, 37, 167, 35, 165, 35, 162, 
32, 159, 31, 159, 32, 157, 30, 154, 29, 151, 29, 150, 29, 151, 27, 143, 27, 136, 
26, 134, 26, 131, 27, 125, 29, 125, 30, 113, 30, 109, 28, 106, 25, 106, 23, 101, 
24, 98, 21, 100, 19, 99, 15, 102, 10, 100, 9, 105, 8, 104, 8, 105, 8, 104, 
9, 105, 10, 103, 11, 106, 11, 106, 12, 106, 11, 106, 12, 108, 14, 107, 12, 110, 
9, 106, 6, 105, 7, 104, 5, 103, 6, 103, 5, 102, 5, 103, 5, 99, 3, 98, 
4, 98, 5, 96, 3, 91, 3, 89, 2, 86, 3, 83, 1, 80, 2, 74, 0, 71, 
4, 68, 5, 64, 17, 58, 17, 55, 15, 53, 15, 50, 16, 48, 15, 47, 16, 45, 
23, 46, 25, 44, 28, 40, 29, 40, 32, 37, 34, 38, 44, 32, 46, 27, 49, 25, 
50, 22, 53, 20, 53, 18, 52, 21, 53, 23, 55, 23, 55, 25, 58, 24, 60, 26, 
59, 27, 62, 31, 61, 34, 59, 34, 59, 40, 55, 44, 56, 48, 53, 55, 56, 54, 
60, 56, 63, 55, 64, 56, 66, 57, 65, 53, 67, 47, 65, 54, 66, 57, 70, 56, 
74, 54, 73, 53, 74, 53, 76, 45, 79, 44, 81, 42, 89, 37, 94, 37, 94, 36, 
96, 37, 100, 36, 104, 37, 106, 35, 110, 34, 114, 34, 114, 25, 117, 27, 121, 25, 
125, 17, 133, 11, 136, 5, 139, 3, 140, 3};

int xgps, ygps;
int tcount = sizeof(Itrack) / sizeof(Itrack[0]);
int tcountplaces = sizeof(trackPlaces) / sizeof(trackPlaces[0]);
int gcount=0; //214;
int nscreen=2, nbutton1=1, nbutton2=1, ntimer2=0, nmenu=0, nrefresh1=-1;

// Euclidean distance function
int distance(int x1, int y1, int x2, int y2) {
    int r=VIEW_WIDTH/2;
    if (x2>=x1-r and x2<=x1+r){
      if (y2>=y1-r and y2<=y1+r){
        return 0;
      }
    }
    return 9999;
    //return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));
}
void debugcoord(int x, int y){
    Serial.print("{");
    Serial.print(x);
    Serial.print(",");
    Serial.print(y);
    Serial.print("}");
}

int convert_to_local_coordinate_system(float latitude, float longitude, float origin_lat, float origin_lon) {
    // Coordinates of the origin in the local system
    // Range of coordinates in the local system
    Serial.println("");
    Serial.println("getgps convert_to_local_coordinate_system");
    int min_x = 0, max_x = 10000;
    int min_y = 0, max_y = 10000;
    // Calculate differences from the origin
    float delta_lat = latitude - origin_lat;
    float delta_lon = longitude - origin_lon;
    // Scale to convert 0.01 degrees to units in the local system
    //int scale = 100;
    // Calculate linear interpolation with the scale
    xgps = (delta_lon * scaleX / 360.0) * (max_x - min_x) + min_x;
    ygps = (delta_lat * scaleY / 180.0) * (max_y - min_y) + min_y;    
}

void getgps(){  
  float gpslat = 37.867603;
  float gpslon = -6.707511;
  if(gps.available())
  {
    dato=gps.read();
    Serial.print(dato);
  }  
  //convert_to_local_coordinate_system(gpslat, gpslon, origin_lat, origin_lon);  
  // for DEBUG:
  xgps = pgm_read_word(Itrack + gcount);
  ygps = pgm_read_word(Itrack + gcount + 1);  
  //Serial.println("");
  //Serial.println("getgps DEBUG");
  //debugcoord(xgps, ygps);
}

void setup() {
  
  Serial.begin(115200);            
  gps.begin(9600); 

  pinMode(BUTTON1_PIN, INPUT_PULLUP);
  pinMode(BUTTON2_PIN, INPUT_PULLUP);
  pinMode(LED_PIN, OUTPUT);

  if(!display.begin(SSD1306_SWITCHCAPVCC, DSCREEN)) {
    Serial.println(F("error SSD1306"));
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(2,12); 
  display.println("loading...");
  display.println(" dgegps ;-)");
  display.display();
  delay(1000);
  display.clearDisplay();
}

void screen2(){
  nbutton2=digitalRead(BUTTON2_PIN);  
  if(nbutton2==0){
    ntimer2=6;
  }
  delay(100);
  nrefresh1--; // isnt needed, static menu
  
  if (nrefresh1<0){
    // init gps simulation code
    gcount+=2;
    if (gcount>=tcount){
      gcount=0;
    }    
    // end gps simulation code    
    display.clearDisplay();   
    // first coordiantes
    getgps(); 
    // Radius
    int i, x, y, x1, y1, x2, y2, nflag=0;
    int WS=VIEW_WIDTH/2, HS=VIEW_HEIGHT/2;  
    // Filtered coordinates
    //Serial.println("loop!");  
    //Serial.print("gc: ");
    //Serial.print(gcount);
    //debugcoord(xgps, ygps);
    //Serial.println("--end1;");
    x1=-1;
    y1=-1;
    for (i = 0; i < tcount; i += 2) {
      // Extract X and Y coordinates from Itrack array            
      x = pgm_read_word(Itrack + i);
      y = pgm_read_word(Itrack + i + 1);       
      //x2=xgps-x+WS;
      x2=x-xgps+WS;
      y2=ygps-y+HS; // VIEW_HEIGHT-y-y0;
      if (distance(xgps, ygps, x, y) <= VIEW_HEIGHT) {    
        // Print or process the coordinates
        if(x1!=-1){
          //Serial.print(i);
          //debugcoord(x, y);
          //debugcoord(x2, y2);
          display.drawLine(x1, y1, x2, y2, WHITE);
          display.drawPixel(x1+1, y1+1, WHITE);
          //display.drawLine(x1, y1-1, x2, y2-1, WHITE);
          //display.drawLine(x1+1, y1+1, x2+1, y2+1, WHITE);
          //display.drawPixel(x1, y1, WHITE);
          if(nflag==0){          
            display.drawCircle(x1, y1, 2, WHITE);
            display.drawCircle(x2, y2, 3, WHITE);
            nflag=1;
          }
        }      
      }
      x1=x2;
      y1=y2;    
    }    
    display.drawCircle(WS, HS, 8, WHITE);  
    display.drawCircle(WS, HS, 2, WHITE);
    //display.setCursor(1,1);
    //display.print("los marines");
    display.setCursor(114,0);
    display.print("N");
    if(ntimer2>0){
      // 0 pressed, 1unpressed
      ntimer2-=1;
      display.drawBitmap(SCREEN_WIDTH-BRUJULA_WIDTH, 10, brujula, BRUJULA_WIDTH, BRUJULA_HEIGHT, WHITE);
    }
    //display.setCursor(108,52);
    //display.print(gcount/2);
    //
    display.display();  
    //
    nrefresh1=8;
  }
}

void screen1(){
  int x=12, y=1, ystep=9, ymenu=0, R;
  const char* place;
  // Clear the screen on each iteration
  //Serial.print(tcountplaces);  
  nbutton2=digitalRead(BUTTON2_PIN);  
  if(nbutton2==0){
    digitalWrite(LED_PIN, HIGH);
    delay(100);
    nmenu++;
    if (nmenu>=tcountplaces){
      nmenu=0;
    }
    digitalWrite(LED_PIN, LOW);
    nrefresh1=-2;
  }else{
    delay(100);
    nrefresh1--; // isnt needed, static menu
  }
  if (nrefresh1<0){    
    ymenu=nmenu*ystep+y+3;
    display.clearDisplay();  
    for (int i = 0; i < tcountplaces; i++) {
      place = (const char*)pgm_read_word(&(trackPlaces[i]));
      display.setCursor(x,y);
      y+=ystep;
      display.print(place);
    }
    R=1+nrefresh1*-1;
    display.drawCircle(5, ymenu, R, WHITE);
    display.drawCircle(5, ymenu, R-1, WHITE);
    //y+=ystep;
    display.drawBitmap(SCREEN_WIDTH-BRUJULA_WIDTH, 10, brujula, BRUJULA_WIDTH, BRUJULA_HEIGHT, WHITE);
    display.display(); 
    //
    nrefresh1=30; 
  }
}

void loop() {
  nbutton1=digitalRead(BUTTON1_PIN);  
  if(nbutton1==0){ // press
    digitalWrite(LED_PIN, HIGH);
    delay(400);  // Pause for N seconds to visualize
    // change screen
    nscreen+=1;  
    Serial.print(nscreen);
    if(nscreen>2){
      nscreen=1;
    }
    digitalWrite(LED_PIN, LOW);
    nrefresh1=-2;
  }
  //
  if(nscreen==1){
    screen1();
  }else{
    if(nscreen==2){
      screen2();
    }
  }

}