/*
Vyvojova deska Arduino Uno
programator AVRISP mkII
*/
#include <LiquidCrystal.h>
//const int rs = 2, en = 3, d4 = 8, d5 = 7, d6 = 5, d7 = 6; // GPSnormal
const int rs = 2, en = 3, d4 = 8, d5 = 7, d6 = 6, d7 = 5, green = 9, red = 10, rele = 13; // GPSnormal2
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

#include <TinyGPS++.h>
TinyGPSPlus gps;

// NEO7M module comands
byte GPSdata1[]={0xB5,0x62,0x06,0x16,0x08,0,0,0x03,0x03,0,0xD1,0xA2,0x06,0,0xA3,0xE7};  // SBAS -16
byte GPSdata2[]={0xB5,0x62,0x06,0x1D,0x1C,0,0x01,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0x80,0x51,0x01,0,0x01,0,0,0,0x13,0xE8};  // TMODE -36
byte GPSdata3[]={0xB5,0x62,0x06,0x31,0x20,0,0,0x01,0,0,0,0,0,0,0x01,0,0,0,0x40,0x42,0x0F,0,0,0,0,0x80,0,0,0,0x80,0,0,0,0,0x2F,0,0,0,0x19,0xB2}; // TP5 -40

long nMiliSec = 0;

int Mode = 0;
/*   
 0 - po zapnuti
 1 - satelits > 3
 2 - ready
*/
    double zSirka, zDelka;
//    float zSirka, zDelka;
int krok=50;
boolean ModeFS = false;  // Mode of device (true == Service mode INT)

void setup() {  
  pinMode(rele, OUTPUT);
  digitalWrite(rele, LOW);  // rele

    pinMode(red, OUTPUT);   
    digitalWrite(red, HIGH); // cervena
  pinMode(green, OUTPUT);
  digitalWrite(green, LOW); // zelena
  cervena();
// Setup serial port and start display
  Serial.begin(9600);
  lcd.begin(40, 2);
  lcd.print("GPS FREKVENCNI NORMAL");
  lcd.setCursor(0,1);
  lcd.print(" Cekame na zachyceni satelitu");
  // Setup NEO7M module
  delay (4000);
  Serial.write(GPSdata1,16);
  delay (1000);
  Serial.write(GPSdata2,36);
  delay (1000);
  Serial.write(GPSdata3,40);
  lcd.clear();
}

void cervena()
{
  digitalWrite(green, LOW);  // zelena
  digitalWrite(red, HIGH); // cervena
}
void zelena()
{
  digitalWrite(green, HIGH);  // zelena
  digitalWrite(red, LOW); // cervena
}
void zluta()
{
  digitalWrite(green, HIGH);  // zelena
  digitalWrite(red, HIGH); // cervena
}

void loop() {
  while (Serial.available() > 0)
  gps.encode(Serial.read());
  delay(krok);                  
  //nMiliSec+=krok;

    int sat=gps.satellites.value();
if(sat<3){
Mode=0;
cervena();
digitalWrite(rele, LOW);  // rele
}

if(Mode==0 && sat >=3){
Mode=1;
nMiliSec=0;
digitalWrite(rele, HIGH);  // rele
zluta();
}

if(Mode==1 && sat>=3 && nMiliSec > 10000){
Mode=2;
digitalWrite(rele, HIGH);  // rele
zelena();
}
  nMiliSec+=krok;
  lcd.clear();
    lcd.setCursor(0,0);
    if(sat<3 || ModeFS) {lcd.print("INT");   cervena();
  digitalWrite(rele, LOW);  // rele
    }
    else {lcd.print("GPS  ");  
if(Mode==1) {
  lcd.print("Probiha stabilizace smycky PPL");
 // lcd.print(nMiliSec);
}

if(Mode==2)
{
if (gps.location.isValid())
  {
   zSirka=gps.location.lat();
   zDelka= gps.location.lng();  
lcd.print("lat.= ");
lcd.print(zSirka,5); // 5 míst za des. teckou (max.6)
lcd.print(" long.= ");
lcd.print(zDelka,5);
//lcd.print(" deg.");
  }
digitalWrite(rele, HIGH);  // rele
 zelena();
}
}
    // Display Mux frequency
    lcd.setCursor(4,0);    
    
    lcd.setCursor(0,1);
    String MyTime ="";
    int Hours=gps.time.hour();
    int Minutes=gps.time.minute();
    int Seconds=gps.time.second();
    
    if (Hours<10) { MyTime=MyTime+"0"+Hours;}
      else {MyTime=MyTime+Hours;}
      
    if (Minutes<10) { MyTime=MyTime+":"+"0"+Minutes;}
      else {MyTime=MyTime+":"+Minutes;} 
      
    if (Seconds<10){MyTime=MyTime+":"+"0"+Seconds;}
      else {MyTime=MyTime+":"+Seconds;} 
      
    lcd.print("UTC:"+MyTime+" pocet satelitu "+sat );
   //lcd.print(MyTime);
 
}