#include <HardwareSerial.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

//SSD1306 OLED definition
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

//Serial definition
#define S2_RX 16 //Connect here from MAX 232 (PIN 13) via TXS0108E (5v=>3,3V)
#define S2_TX 17 //Not connected - ESP32 only receives data
HardwareSerial wind = Serial2;

boolean flag, flag1;
unsigned short int i=0, i2=0, iy=0, iz=0, comma_pos=0;
char incomingData;
char nmea_sentence[]="$WIMWV,xxx,R,x.x,N,A*xx";
char calculated_checksum [3];
char received_checksum[3];
char wind_angle[] = "---"; 
char wind_speed[] = "--.-";

// Print data on OLED function
void print_data(char *ws, char *wa){ 
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(10, 10);
  display.print("Wind Speed: ");
  display.print(ws);
  display.print(" kn");
  display.setCursor(10, 30);
  display.print("Wind Angle: ");
  display.print(wa);
  display.cp437(true);
  display.write(248);
  display.display();
}

//Nmea 0183 string checksum calculation - It's just a byte to byte XOR between nmea string chars
int nmea_checksum(char *nmea_string)
{
  int code = 0;
  for (int x = 1; x < strlen(nmea_string); x++)
  {
    code ^= nmea_string[x];
  }
  return code;
}

void setup()
{
//Serial init 
Serial.begin(9600);
wind.begin(4800, SERIAL_8N1, S2_RX, S2_TX);

//display init
if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
  Serial.println(F("SSD1306 allocation failed"));
  for(;;);
  }
display.setTextColor(WHITE);
display.clearDisplay();
display.display();

Serial.println("Ready to get data from NASA Clipper V2");

delay(2000);
}

void loop()
{
  if (wind.available()>0){
    incomingData = wind.read();

    if (incomingData=='$'){
      flag = false;
      i=0;
      nmea_sentence[0]='$';
    }

    else if (incomingData=='*'){
      flag= true;
      i++;
      nmea_sentence[i] = '\0';
      itoa((nmea_checksum(nmea_sentence)), calculated_checksum, 16);
      // convert_hex_upper(calculated_checksum);
      
      for (int x=0; x<=strlen(calculated_checksum);x++){
        if (calculated_checksum[x] >= 'a' && calculated_checksum[x] <= 'f'){
          calculated_checksum[x] -= 32;
        }
      }
      if (strlen(calculated_checksum) == 1){
      calculated_checksum[1]=calculated_checksum[0];
      calculated_checksum[0]='0';
      }
      calculated_checksum[2]='\0';   
    }

    else if (!flag){
      i++;
      nmea_sentence[i] = incomingData;
    }

    else if (flag && i2==0){
      received_checksum[0] = incomingData;
      i2 = 1;
    } 
    
    else if (flag && i2==1){
      received_checksum[1] = incomingData;
      received_checksum[2]='\0';

      if (strcmp(received_checksum, calculated_checksum) == 0){
        for (unsigned short int ix=0; ix<=strlen(nmea_sentence); ix++){
          if (nmea_sentence[ix] == ','){
            comma_pos++;
            flag1=!flag1;
          }
          else if (comma_pos==1 && flag1){
            wind_angle[iy]=nmea_sentence[ix];
            iy++;
          }
          else if (comma_pos==3 && flag1){
            wind_speed[iz]=nmea_sentence[ix];
            iz++;
          }
        }
        wind_angle[iy]='\0';
        wind_speed[iz]='\0';
        iy=0;
        iz=0;
        flag1=false;
        comma_pos=0;
        print_data(wind_speed, wind_angle);
      
      }
      else {
        Serial.println("CHECKSUM ERROR!!!!!!");
      }
      
      i2 = 0;
      flag = false;
    }
  }
  else {
    char *wind_angle="---";
    char *wind_speed="--.-";
    print_data(wind_speed, wind_angle);
  }
     
}