#include "s.h"
#include "ezButton.h"

//#include <SoftwareSerial.h>

// PODLACZYC SYGNAL Z (TX z modulu GPS) D4? pod pin D0
// SERIAL MONITOR NA 9600
/*
Neutralny 0.0 off

Pierwszy skok 0.5 przy pierwszej prędkości gps

Drugi skok 1.0 przy drugiej prędkości gps

Trzeci skok 1.5 w przypadku ciśnięcia hamulca - tego jeszcze nie ma w tej wersji ino
*/
// Define the GPS serial pins
//SoftwareSerial gpsSerial(4, 3); // RX, TX
TinyGPSPlus gps;

#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27,16,2); 

// Define relay control pins, buzzer pin, and demo mode pin
const int liftRelayPin = 7;  // Relay for forward motion
const int dropRelayPin = 6;  // Relay for reverse motion
const int buzzerPin = 8;     // PWM-capable pin for passive buzzer
const int demoModePin = 2;   // Pin to enable demo mode
const int breakModePin = 3;   // Pin to enable break mode

ezButton demoMode(demoModePin);
ezButton breakMode(breakModePin);

// Wing lift durations
int liftPosition = 0;
int liftPosition_old = 0;

int liftPos = 0;
int liftPos_old = 0;
bool up_old = false;

unsigned long lastLiftTime = 0;
unsigned long lastPrintTime = 0;
unsigned long demoModeLastTime = 0;
bool demoModeActive = false;

void setup() {
  // Start the serial communication
  Serial.begin(9600); // For debugging, using hardware serial
  //gpsSerial.begin(9600); // Start GPS serial communication

  lcd.init();
  lcd.backlight();
  lcd.setCursor(4,0);
  lcd.print("GPS WING");

  // Set relay, buzzer, and demo mode pins as output or input
  pinMode(liftRelayPin, OUTPUT);
  pinMode(dropRelayPin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);

  demoMode.setDebounceTime(50);;
  breakMode.setDebounceTime(50);;

  // Initialize relays and buzzer to off
  digitalWrite(liftRelayPin, LOW);
  digitalWrite(dropRelayPin, LOW);
  digitalWrite(buzzerPin, LOW);

  Serial.print("Drop ...");

  dropWing(-20); // go to start position
  Serial.println("OK");
  
  // Sound 2 short buzzes on startup
  soundStartupBuzz();
  Serial.println("START");

  String a = "10-11-2012 11:12:13.txt - old";
  Serial.println(a);
  Serial.println(a.indexOf("-"));
  Serial.println(a.indexOf("!"));
  Serial.println(a.substring(10));

  Serial.println();
  Serial.println(sizeof(size_t));
  Serial.println((int)(size_t)-1);

}


bool demo = false;
bool breakActive = false;
int b = 0;
char bu[100];
//GPS:
// $GNRMC,160727.00,A,5348.46589,N,00618.76437,W,0.229,,060824,,,A*7A

float speedKmh = 0.0;
float speedKmh_old = 0.0;

int valid = 0;

char bufs[12];

void loop() {
  demoMode.loop();
  breakMode.loop();

  if (demoMode.isPressed())  demo = true;
  if (demoMode.isReleased()) demo = false;
  if (breakMode.isPressed())  {breakActive = true;Serial.print('B');Serial.println(breakActive);}
  if (breakMode.isReleased()) {breakActive = false;Serial.print('B');Serial.println(breakActive);}

  // Read data from the GPS module
  while (Serial.available() > 0) {
        char a = Serial.read();
        bu[b++] = a;

        if (a == '\n') {
          bu[b] = 0;
          Serial.println();
          Serial.print("GPS: ");
          Serial.write(bu);

          if(bu[0] == 's' || bu[0] == 'S')
            speedKmh = String(bu).substring(1).toFloat();
          b = 0;
        }
        
        if(gps.encode(a)) {
          Serial.println();
          Serial.print(millis());

          // Normal mode: Check if the GPS data is valid
          if (gps.speed.isValid()) {
            // Get speed in km/h
            speedKmh = gps.speed.kmph();

            // Deactivate brake mode if speed is below 10 km/h
            if (speedKmh < 10 && breakActive) {
            //  breakActive = false;
              Serial.println("Brake mode deactivated due to low speed.");
            }

          }

        }
    }

  int v = gps.satellites.value();
  if (v != valid){
    lcd.setCursor(0,0);
    if (gps.satellites.isValid()) {
      lcd.print(v);
      lcd.print(" ");
    } else lcd.print("    ");
    valid = v;
  }

  if (speedKmh_old != speedKmh){

    Serial.print(" S: ");
    Serial.print(speedKmh);
    Serial.print(" km/h");
    Serial.println();

    lcd.setCursor(0,1);
    if (speedKmh < 10)  lcd.print(" ");
    if (speedKmh < 100) lcd.print(" ");
    lcd.print(int(speedKmh));
    lcd.print(" km/h ");

    speedKmh_old = speedKmh;
  }

  if (breakActive) {
    lcd.setCursor(10,1);
    lcd.print("BREAK");

    liftPosition = 15;
    moveWing(liftPosition);

  } else if (demo && speedKmh < 9) {

    lcd.setCursor(10,1);
    lcd.print("DEMO ");

    if (millis() - demoModeLastTime > 10000) { // Every 10 seconds
      demoModeLastTime = millis();
      liftPosition = 5 * random(0, 4); // Randomly select lift pos 0 - 3
      Serial.println();
      Serial.print("DEMO: ");
      Serial.println(liftPosition);
      moveWing(liftPosition);
    }

  } else {

    lcd.setCursor(10,1);
    lcd.print("     ");

    // Check speed conditions and adjust wing lift duration
    if (speedKmh >= 50) {
      liftPosition = 10; // Lift wing for 2 seconds
    } else if (speedKmh >= 30) {
      liftPosition = 5; // Lift wing for 1 second
    } else {
      liftPosition = 0; // No action
    }

    // Control the wing lift based on the duration
    //if (millis() - lastLiftTime > 1000) { // Check every second
    //  lastLiftTime = millis();
      moveWing(liftPosition);
    //}
  }
}



void moveWing(int newPos) {
  if (newPos == liftPos) return;
  bool up = newPos > liftPos;
  int diff = newPos - liftPos;



  if (up) { 
    if(liftPos >= 15) return;
    if (breakActive) liftPos = 15;
    else liftPos += 5;
  } else {
    if (liftPos <=0) return;
    liftPos -= 5;
  }

  int offset = 0;
  if (up_old != up) offset = 10;

  tone(buzzerPin, up ? 1000 : 500); // Turn on the buzzer with 1000:500 Hz tone
  digitalWrite(liftRelayPin,  up ? HIGH : LOW); // Activate lift relay (Forward motion)
  digitalWrite(dropRelayPin, !up ? HIGH : LOW); // Ensure drop relay is off (Reverse motion)
  if (breakActive) delay(diff*100 + offset);
  else delay(500 + offset);
  digitalWrite(liftRelayPin, LOW);  // Deactivate lift relay
  digitalWrite(dropRelayPin, LOW);  // Ensure drop relay is off (Reverse motion)
  noTone(buzzerPin); // Turn off the buzzer

              Serial.print(" P: ");
              Serial.print(liftPos);
              Serial.print(" N: ");
              Serial.println(newPos);


  up_old = up;
}

void liftWing(int dseconds) {
  tone(buzzerPin, 1000); // Turn on the buzzer with 1000 Hz tone
  digitalWrite(liftRelayPin, HIGH); // Activate lift relay (Forward motion)
  digitalWrite(dropRelayPin, LOW);  // Ensure drop relay is off (Reverse motion)
  delay(abs(dseconds) * 100);
  digitalWrite(liftRelayPin, LOW);  // Deactivate lift relay
  noTone(buzzerPin); // Turn off the buzzer
}

void dropWing(int dseconds) {
  tone(buzzerPin, 500); // Turn on the buzzer with 500 Hz tone
  digitalWrite(dropRelayPin, HIGH); // Activate drop relay (Reverse motion)
  digitalWrite(liftRelayPin, LOW);  // Ensure lift relay is off (Forward motion)
  delay(abs(dseconds) * 100);
  digitalWrite(dropRelayPin, LOW);  // Deactivate drop relay
  noTone(buzzerPin); // Turn off the buzzer
}

void soundStartupBuzz() {
  for (int i = 0; i < 2; i++) {
    tone(buzzerPin, 1000); // 1 kHz tone
    delay(100);            // Duration of buzz
    noTone(buzzerPin);    // Turn off buzzer
    delay(100);            // Pause between buzzes
  }
}