#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);

Servo Signal;

bool toggletf = true;
bool deploytf = false;
bool data = true;
bool liftwarn = false;
bool deploystat = false;
bool ad = false;
int maxwind = 0;

#define TRIG 3
#define ECHO 2
#define distance 40 //change this value to change how far to open. in cm

void setup() {
  Signal.write(0);
  Serial.begin(115200);
  // put your setup code here, to run once:
  if (!i2CAddrTest(0x27)) {
    lcd = LiquidCrystal_I2C(0x3F, 16, 2);
  }
  Signal.attach(7);
  lcd.init();                // initialize the lcd
  lcd.backlight();           // Turn on backlight
}

void loop() {
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  int duration = pulseIn(ECHO, HIGH);
  int cm = duration / 58; //change 58 to 148 if you wanna make it inch
  int deploytct = digitalRead(6);
  int p1 = analogRead(A0);
  int p2 = analogRead(A1);
  int drt = map(p1, 0, 1023, 0, 360);
  int wskph = map(p2, 0, 1023, 0, 108);
  Serial.print(cm);
  Serial.print(" ");
  Serial.print(toggletf);
  Serial.print(" ");
  Serial.print(drt);
  Serial.print(" ");
  Serial.print(wskph);
  Serial.print(" ");
  Serial.println(ad);
  if (data == true && deploystat == false && liftwarn == false && ad == false) {
    lcd.setCursor(0, 0);
    lcd.print("Wind: ");
    lcd.setCursor(6, 0);
    lcd.print(wskph);
    lcd.print("   ");
    lcd.setCursor(0, 1);
    lcd.print("Drct: ");
    lcd.setCursor(6, 1);
    lcd.print(drt);
    lcd.print("   ");
    lcd.setCursor(11, 0);
    lcd.print(maxwind);

  } else if (data == false && deploystat == true) {
    status();
  } else if (data == false && deploystat == false && liftwarn == true && ad == false) {
    warn();
  } else {
    data = true;
    deploystat = false;
    liftwarn = false;
  }

  if (wskph >= 70 && deploytf == false) {
    ad = true;
    Signal.write(180);
    status();
  } else if (!deploytf == true) {
    Signal.write(0);
    ad = false;
    delay(20);
  } else if (wskph <= 70) {
    ad = false;
    delay(20);
  }

  if (maxwind <= wskph) {
    maxwind = wskph;
  }
  // put your main code here, to run repeatedly:
  //
  // for(int i=0; i<=180; i++) {    //< made for testing read function
  // int sstat = Signal.read();
  // Signal.write(i);
  // Serial.print(i);
  // Serial.print(" ");
  // Serial.println(sstat);
  // status();
  // }
  // delay(500);
  if (deploytct == HIGH && toggletf == true && deploytf == false) {
    toggletf = false;
    delay(40);
    Signal.write(180);
    toggletf = true;
    deploystat = true;
    deploytf = true;
    data = false;
  } else if (deploytct == HIGH && toggletf == true && deploytf == true) {
    toggletf = false;
    delay(40);
    Signal.write(0);
    toggletf = true;
    deploystat = true;
    deploytf = false;
    data = false;
  }


}

void status() {
  lcd.clear();
  int sstat = Signal.read();
  int deploytct = digitalRead(6);
  int p1 = analogRead(A0);
  int p2 = analogRead(A1);
  int drt = map(p1, 0, 1023, 0, 360);
  int wskph = map(p2, 0, 1023, 0, 108);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  int duration = pulseIn(ECHO, HIGH);
  int cm = duration / 58; //change 58 to 148 if you wanna make it inch

  if (maxwind <= wskph) {
    maxwind = wskph;
  }

  if (cm >= 10) {
  liftwarn=true;
  }

  if (ad == true) {
    Serial.println(ad);
    lcd.setCursor(0, 0);
    lcd.print("Auto-Deployed:");
    lcd.setCursor(0, 1);
    lcd.print("Wind Speed over 70");
    lcd.setCursor(0, 2);
    lcd.print("W:");
    lcd.setCursor(3, 2);
    lcd.print(wskph);
    lcd.setCursor(7, 2);
    lcd.print(maxwind);
    lcd.setCursor(0, 3);
    lcd.print("D:");
    lcd.setCursor(3, 3);
    lcd.print(drt);
  } else {
    lcd.setCursor(0, 0);
    lcd.print("Manual-Deployed");
  }
  if (sstat <= 45 && ad == false) {
    lcd.setCursor(1, 3);
    lcd.print("    [retracted]    ");
  }
  if (sstat >= 45 && sstat <= 90 && ad == false) {
    lcd.setCursor(0, 3);
    lcd.print("   > .[deploy]. <   ");
  }
  if (sstat >= 90 && sstat <= 135 && ad == false) {
    lcd.setCursor(0, 3);
    lcd.print("  >>..[deploy]..<<  ");
  }
  if (sstat >= 135 && sstat <= 180 && ad == false) {
    lcd.setCursor(0, 3);
    lcd.print(" >>>-.[deploy].-<<< ");
  }
  if (sstat == 180 && ad == false) {
    lcd.setCursor(0, 3);
    lcd.print(">>>>--[deploy]--<<<<");
  }
  delay(250);
  lcd.clear();
  deploystat = false;
  data = true;
}

void warn() {
  lcd.setCursor(0, 0);
  lcd.print("      [WARNING]     ");
  lcd.setCursor(0, 2);
  lcd.print("Probe flew away.");
  lcd.setCursor(0, 3);
  lcd.print("'She was a fairy'");
}

bool i2CAddrTest(uint8_t addr) {
  Wire.begin();
  Wire.beginTransmission(addr);
  if (Wire.endTransmission() == 0) {
    return true;
  }
  return false;
}