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

Servo myservo;  // create servo object to control a servo
int pos = 0;    // variable to store the servo position

TinyGPS gps;
NewSoftSerial nss(8, 2);
LiquidCrystal lcd(12, 11, 10, 4, 5, 6, 7);

const int offPin = 17;

void gpsdump(TinyGPS &gps);
bool feedgps();
//void printFloat(double f, int digits = 2);

long lat, lon;
float flat, flon;
unsigned long age;

int addr = 0; //seals location in eeprom
int lockaddr = 16;
int seals;

//Seal Locations
//like im going to leave this to be read but they are floating point to about 6 digits. And all in the southern hemisphere
float fs_lat = 
float fs_lon = 
float s1_lat1 = 
float s1_lon1 = 
float s1_lat2 = 
float s1_lon2 = 
float s1_lat3 = 
float s1_lon3 = 
float s1_lat4 = 
float s1_lon4 = 
float s1_lat5 = 
float s1_lon5 = 

void setup()
{
  Serial.begin(115200);
  nss.begin(4800);
  lcd.begin(20, 4); //lcd.begin(columns, rows)
  lcd.clear();

  if (EEPROM.read(lockaddr) == 0)
    ServoLock();
   
  Serial.print("Testing TinyGPS library v. "); Serial.println(TinyGPS::library_version());
  Serial.print("Sizeof(gpsobject) = "); Serial.println(sizeof(TinyGPS));
  Serial.println();
  Serial.print("Lock: ");
  Serial.println(EEPROM.read(lockaddr));
  
  pinMode(offPin, OUTPUT);
   
  seals = EEPROM.read(addr);
  Serial.print("Seal: ");
  Serial.println(seals);
  
  lcd.setCursor(8,3);
  lcd.print("Open Seals ");
  lcd.print(seals);
}

void gpsdump(TinyGPS &gps)
{
  feedgps();
    
  gps.get_position(&lat, &lon, &age);

  Serial.print("Lat/Long(10^-5 deg): "); Serial.print(lat); Serial.print(", "); Serial.print(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
  gps.f_get_position(&flat, &flon, &age);

  //comment out the below before using
  lcd.setCursor(0,1);
  lcd.print("Lat: ");
  lcd.print(lat);
  lcd.setCursor(0,2);
  lcd.print("Lon: ");
  lcd.print(lon);
}
  
bool feedgps()
{
  while (nss.available())
  {
    if (gps.encode(nss.read()))
      return true;
  }
  return false;
}

void badsignal() //Signal is bad, notify on lcd then turn off
{
 Serial.println("Bad/Low Signal from GPS");
 lcd.clear();
 lcd.setCursor(0,0);
 lcd.print("Bad or Low Signal");
 lcd.setCursor(0,2);
 lcd.print("Try going outside...");
 delay(10000);
 //turn off
 digitalWrite(offPin, HIGH);
}

float gpsdistance(float LocationX, float LocationY, float DestinationX, float DestinationY)
{
  float DistanceX;
  float DistanceY;
  float Distance;
  
  if (LocationX > DestinationX)
    DistanceX = LocationX - DestinationX;
  else
    DistanceX = DestinationX - LocationX;
    
  if (LocationY > DestinationY)
    DistanceY = LocationY - DestinationY;
  else
    DistanceY = DestinationY - LocationY;
 
  Distance = (DistanceX * DistanceX) + (DistanceY * DistanceY);
  Distance = sqrt(Distance) * (110.56);

  
  return Distance;
}

bool testDistance(float &Dest_lat, float &Dest_long){
  float Dist_Remain;

  Dist_Remain = gpsdistance(flat, flon, Dest_lat, Dest_long);
  
  lcd.setCursor(0,0);
  if (Dist_Remain <= 0.25) //Less then 250m
  {
    lcd.clear(); 
    lcd.print("Seal #");
    lcd.print(seals + 1);
    lcd.print(" Located");
    delay(5000);
    return true;
  }
  else if (Dist_Remain < 1.0){ //Less then 1km
    lcd.print("Distance");
    lcd.setCursor(0,1);
    lcd.print("Remaining ");
    lcd.setCursor(10,0);
    
    Serial.println(Dist_Remain);
    lcd.print(Dist_Remain * 1000); 
    lcd.print("m");
    
    lcd.setCursor(0,2);
    lcd.print("To Next Seal");
  }
  else //More then 1km
  {
    lcd.print("Distance");
    lcd.setCursor(0,1);
    lcd.print("Remaining ");
    lcd.setCursor(10,0);
    
    Serial.println(Dist_Remain);
    lcd.print(Dist_Remain); 
    lcd.print("km"); 
    
    lcd.setCursor(0,2);
    lcd.print("To Next Seal");
  }    
  
  delay(5000);
  digitalWrite(offPin, HIGH);
  return false;

}

void LCD_progress(){
  int coord;
  lcd.setCursor(0,2);
  for (coord = 0; coord < 20; coord+=1){
      lcd.print("#");
      delay(100);
  }
}

void ServoLock(){
    Serial.begin(0);
    nss.begin(0);
    Serial.flush();
    nss.flush();
  
    lcd.clear();
    lcd.setCursor(0,1);
    lcd.print("     Unlocking      ");
    
    myservo.attach(14);  // attaches the servo on pin 9 to the servo object 
    delay(2000);
        
    for(pos = 0; pos < 70; pos += 1)  // goes from 0 degrees to 180 degrees 
    {                                  // in steps of 1 degree 
      myservo.write(pos);              // tell servo to go to position in variable 'pos' 
      delay(15);                       // waits 15ms for the servo to reach the position 
    } 

    //say unlocked open box
    lcd.setCursor(0,2);
    lcd.print("  < Open Box Now >  ");
    delay(20000);
    lcd.clear();  
    
    for(pos = 70; pos>=1; pos-=1)     // goes from 180 degrees to 0 degrees 
    {                                
      myservo.write(pos);              // tell servo to go to position in variable 'pos' 
      delay(15);                       // waits 15ms for the servo to reach the position 
    } 
    delay(1000);
  
    myservo.detach();
    //should turn off here
    digitalWrite(offPin, HIGH);
    Serial.begin(115200);
    nss.begin(4800);
    
}

void loop()
{    
  bool newdata = false;
  float Dist_Remain;
  unsigned long start = millis();
  seals = EEPROM.read(addr);
 
  lcd.clear();
  
  lcd.setCursor(0,0);
  lcd.print("Aquiring Signal...");
  lcd.setCursor(8,3);
  lcd.print("Open Seals ");
  lcd.print(seals);

  // Every 5 seconds we print an update
  while (millis() - start < 5000)
  {
    if (feedgps())
      newdata = true;
  }
  
  if (newdata)
  {
    gps.f_get_position(&flat, &flon, &age);
    if (age == TinyGPS::GPS_INVALID_AGE)
      badsignal();
    else if (age > 5000)
      badsignal();
    else
      Serial.println("Data is current.");
    
    //lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Signal Found...     ");
    gpsdump(gps);
    delay(2000);
    lcd.clear();
       
    //if within 30m of failsafe
    if (gpsdistance(flat, flon, fs_lat, fs_lon) < 0.05){
    //display failsafe active
      delay(500);
      //Serial.println(off);
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("      Failsafe      ");
      lcd.setCursor(0,1);
      lcd.print("      Engaged       ");
      LCD_progress();
      
      delay(3000);
      lcd.clear();
    //unlock box dont update EEPROM
      ServoLock();
    }
    
    switch (seals){
      case 0:
        Serial.println("Seal #1");
        if (testDistance(s1_lat1, s1_lon1) == true){
          EEPROM.write(addr, 1);
          lcd.clear();
          lcd.setCursor(0,0);
          lcd.print("     Releasing      ");
          lcd.setCursor(0,1);
          lcd.print("     First Seal     ");
          LCD_progress();
          delay(5000);
        }
        break;
      case 1:
        Serial.println("Seal #2");
        if (testDistance(s1_lat2, s1_lon2) == true){
          EEPROM.write(addr, 2);
          lcd.clear();
          lcd.setCursor(0,0);
          lcd.print("     Releasing      ");
          lcd.setCursor(0,1);
          lcd.print("    Second Seal     ");
          LCD_progress();
          delay(5000);
        }
        break;
      case 2:
        Serial.println("Seal #3");
        if (testDistance(s1_lat3, s1_lon3) == true){
          EEPROM.write(addr, 3);
          lcd.clear();
          lcd.setCursor(0,0);
          lcd.print("     Releasing      ");
          lcd.setCursor(0,1);
          lcd.print("     Third Seal     ");
          LCD_progress();
          delay(5000);
        }
        break;
      case 3:
        Serial.println("Seal #4");
        if (testDistance(s1_lat4, s1_lon4) == true){
          EEPROM.write(addr, 4);
          lcd.clear();
          lcd.setCursor(0,0);
          lcd.print("     Releasing      ");
          lcd.setCursor(0,1);
          lcd.print("    Fourth Seal     ");
          LCD_progress();
          delay(5000);
        }
        break;
      case 4:
        Serial.println("Seal #5");
        if (testDistance(s1_lat5, s1_lon5) == true){
          EEPROM.write(addr, 5);
          lcd.clear();
          lcd.setCursor(0,0);
          lcd.print("     Releasing      ");
          lcd.setCursor(0,1);
          lcd.print("     Final Seal     ");
          LCD_progress();
          //Set Servo EEPROM to Unlock
          //Unlock Servo
          delay(5000);
          lcd.clear();
          //unlock box
          EEPROM.write(lockaddr, 0);
          delay(500);
          ServoLock();
          //turn off
        }
        break;
      default:
        //do nothing or corrupt eeprom just open
        Serial.println("Default");
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("Invalid EEPROM Value");
        lcd.setCursor(0,1);
        lcd.print(" Execute Subroutine ");
        lcd.setCursor(0,2);
        lcd.print("    ServoLock()     ");
        //unlock box
        ServoLock();
        //turn off
    }
  }
  else
  {
    if (millis() > 60000)
      badsignal();  
  }
}