#include <NewSoftSerial.h>
#include <TinyGPS.h>
#include <Servo.h>
#include <math.h>
#include <EEPROM.h>

/* This sample code demonstrates the normal use of a TinyGPS object.
   It requires the use of NewSoftSerial, and assumes that you have a
   4800-baud serial GPS device hooked up on pins 2(rx) and 3(tx).
*/
TinyGPS gps;
NewSoftSerial nss(4,13);
Servo servo;


//long targetLat =  3028369; //elyk
//long targetLon = -8151467;


//float targetLat =  29.898436; //frt
//float targetLon = -81.312795;

float targetLat =  30.283151; //dog park
float targetLon = -81.406214;

int targetDistance = 10000;//distance to target in meters

float targets[][2] =
{
  {30.283151, -81.406214},
  {30.28369, -81.51467}
};

char* hints[2] =
{
  "Riley"
  "Andreas's Work",
};

void gpsdump(TinyGPS &gps);
bool feedgps();
bool open = false;
int offPin = 12;
NewSoftSerial lcdSerialPort(5, 6);
int currentWaypoint;
void setup()
{
  currentWaypoint = (int)getCurrentWaypoint();
  setBaudRate(115200);

  clearScreen();

  // Set a dim display
  setBackgroundBrightness(5);
  servo.attach(9);
  pinMode(offPin, OUTPUT);
  digitalWrite(offPin, LOW);

  Serial.begin(115200);
  nss.begin(4800);

  Serial.print("Testing TinyGPS library v. "); Serial.println(TinyGPS::library_version());
  Serial.println("by Mikal Hart");
  Serial.println();
  Serial.print("Sizeof(gpsobject) = "); Serial.println(sizeof(TinyGPS));
  Serial.println();

  print("Hello. Acquiring signal. Please wait");
}

void loop()
{
  lockBox();
  bool newdata = false;
  int not_found = 0;

  while(!newdata && not_found  < 12)
  {
    Serial.print("newdata: ");
    Serial.println(newdata);
    long start = millis();
    while(millis()-start<5000)
    {
      if(feedgps())
        newdata = true;
    }
    print(".");
    not_found++;
  }


  if (newdata)
  {
    clearScreen();
    Serial.println("Acquired Data");
    Serial.println("-------------");
    gpsdump(gps);
    Serial.println("-------------");
    Serial.println();
  }
  else
  {
    if(not_found == 12) //60 seconds
    {
      clearScreen();
      print("No signal.");
      setX(0);
      setY(50);
      //servo.write(1000);
      unlockBox();
      print("Powering down. Bye");
      delay(10000);
      digitalWrite(offPin, HIGH);
    }
  }
}

void gpsdump(TinyGPS &gps)
{
  char buffer[12];
  char *str;
  float lat, lon;
  unsigned long age, date, time, chars;
  int year;
  float d;
  byte month, day, hour, minute, second, hundredths;
  unsigned short sentences, failed;

  gps.f_get_position(&lat, &lon, &age);
  d = distance_between(lat, lon, targetLat, targetLon);
  if(d<targetDistance || open == true)
  {
    accessGranted();
  }
  else
  {
    lcdSerialPort.print(round(d/1000));
    lcdSerialPort.print("km");
    delay(5000);
    setX(15);
    setY(15);
    print("Powering down. Bye.");
    delay(5000);
    digitalWrite(offPin, HIGH);
  }
  Serial.print(d);
  Serial.print("Lat/Long(10^-5 deg): "); Serial.print(lat); Serial.print(", "); Serial.println(lon);
  Serial.print(" Fix age: "); Serial.print(age); Serial.println("ms.");

  feedgps(); // If we don't feed the gps during this long routine, we may drop characters and get checksum errors
}

void accessGranted()
{
  print("Access Granted!");
  unlockBox();
  print("Powering Off. Bye");
  delay(5000);
  digitalWrite(offPin, HIGH);
}

void lockBox()
{
   servo.write(90);
}
void unlockBox()
{
   servo.write(185);
}

bool feedgps()
{
  while (nss.available())
  {
    if (gps.encode(nss.read()))
      return true;
  }
  return false;
}

byte getCurrentWaypoint()
{
  byte temp;
  temp = EEPROM.read(15);
  if(temp==255)
  {
    EEPROM.write(15,0);
    return 0;
  }
  return temp;
}

float distance_between(float flat1, float flon1, float flat2, float  flon2)
{
float dist_calc=0;
float dist_calc2=0;
float diflat=0;
float diflon=0;

//I've to spplit all the calculation in several steps. If i try to do it in a single line the arduino will explode.
diflat=radians(flat2-flat1);
flat1=radians(flat1);
flat2=radians(flat2);
diflon=radians((flon2)-(flon1));

dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
dist_calc2= cos(flat1);
dist_calc2*=cos(flat2);
dist_calc2*=sin(diflon/2.0);
dist_calc2*=sin(diflon/2.0);
dist_calc +=dist_calc2;

dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));

dist_calc*=6371000.0; //Converting to meters
//Serial.println(dist_calc);
return dist_calc;
}

void print(char *data){
  lcdSerialPort.print(data);
}

void clearScreen(){
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x00,BYTE);
}

void demo(){
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x04,BYTE);
}

void toggleSplashScreen(){
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x13,BYTE);
}

void setBackgroundBrightness(byte value){
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x02,BYTE);
  lcdSerialPort.print(value,BYTE);
}

void setBaudRate(long value){
  // Get the internal reference for this baud rate
  char *ref = " ";
  if(value == 4800)
    ref = "1";
  else if(value == 9600)
    ref = "2";
  else if(value == 19200)
    ref = "3";
  else if(value == 38400)
    ref = "4";
  else if(value == 57600)
    ref = "5";
  else if(value == 115200)
    ref = "6";
  else
    return;

  // Since it often rolls back to 115200, try setting it via 115200 first
  lcdSerialPort.begin(115200);
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x07,BYTE);
  lcdSerialPort.print(ref);

  // Now change the serial port to the desired rate, and set it again.
  lcdSerialPort.begin(value);
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x07,BYTE);
  lcdSerialPort.print(ref);
}

void setX(byte value){
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x18,BYTE);
  lcdSerialPort.print(value,BYTE);
}

void setY(byte value){
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x19,BYTE);
  lcdSerialPort.print(value,BYTE);
}

void setPixel(byte state){
  lcdSerialPort.print(0x50,BYTE);
  lcdSerialPort.print(0x40,BYTE);
  lcdSerialPort.print(state,BYTE);
}

void drawLine(byte startX, byte startY, byte endX, byte endY, byte state){
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x0C,BYTE);
  lcdSerialPort.print(startX,BYTE);
  lcdSerialPort.print(startY,BYTE);
  lcdSerialPort.print(endX,BYTE);
  lcdSerialPort.print(endY,BYTE);
  lcdSerialPort.print(state,BYTE);
}

void drawCircle(byte startX, byte startY, byte radius, byte state){
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x03,BYTE);
  lcdSerialPort.print(startX,BYTE);
  lcdSerialPort.print(startY,BYTE);
  lcdSerialPort.print(radius,BYTE);
  lcdSerialPort.print(state,BYTE);
}

void drawBox(byte topLeftX, byte topLeftY, byte bottomRightX, byte bottomRightY, byte state){
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x0F,BYTE);
  lcdSerialPort.print(topLeftX,BYTE);
  lcdSerialPort.print(topLeftY,BYTE);
  lcdSerialPort.print(bottomRightX,BYTE);
  lcdSerialPort.print(bottomRightY,BYTE);
  lcdSerialPort.print(state,BYTE);
}

void eraseBox(byte topLeftX, byte topLeftY, byte bottomRightX, byte bottomRightY, byte state){
  lcdSerialPort.print(0x7C,BYTE);
  lcdSerialPort.print(0x05,BYTE);
  lcdSerialPort.print(topLeftX,BYTE);
  lcdSerialPort.print(topLeftY,BYTE);
  lcdSerialPort.print(bottomRightX,BYTE);
  lcdSerialPort.print(bottomRightY,BYTE);
  lcdSerialPort.print(state,BYTE);
}