#include <TM1637Display.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <SoftwareSerial.h>
#include <DHT.h>

//Create TM1637 Object
#define CLK 2 // Connect CLK pin of TM1637 to Arduino digital pin 2
#define DIO 3 // Connect DIO pin of TM1637 to Arduino digital pin 3
TM1637Display display(CLK, DIO);

//Create 4 Servo Objects
/*
1 = bottom tower
2 = top tower
a = left side
b = right side
*/
Servo s1a;
Servo s1b;
Servo s2a;
Servo s2b;

//Create LCD Object
LiquidCrystal_I2C lcd(0x27, 16, 2);

//Set up Communication (Xbee Module)
SoftwareSerial xbee(0,1);

//Create DHT22
#define DHTPIN 6
#define DHTTYPE DHT22 
DHT dht(DHTPIN, DHTTYPE);

//Set up Anemometer 
const int sensorPin = A0; //Defines the pin that the anemometer output is connected to
int sensorValue = 0; //Variable stores the value direct from the analog pin
float sensorVoltage = 0; //Variable that stores the voltage (in Volts) from the anemometer being sent to the analog pin
float windSpeed = 0; // Wind speed in meters per second (m/s)

float voltageConversionConstant = .004882814; //This constant maps the value provided from the analog read function, which ranges from 0 to 1023, to actual voltage, which ranges from 0V to 5V
int sensorDelay = 1000; //Delay between sensor readings, measured in milliseconds (ms)

float voltageMin = .4; // Mininum output voltage from anemometer in mV.
float windSpeedMin = 0; // Wind speed in meters/sec corresponding to minimum voltage
 
float voltageMax = 2.0; // Maximum output voltage from anemometer in mV.
float windSpeedMax = 32; // Wind speed in meters/sec corresponding to maximum voltage
 
 

//Set up timer Variables
const float countdownTime = 10.00; // e.g., 10 seconds

void setup() 
{
  //Set Up TM1637
  display.setBrightness(7);

  //Start LCD Object
  lcd.begin(16,2);

  //Start XBee Object
  Serial.begin(9600);
  xbee.begin(9600);
  xbee.listen();

  //Start DHT Object
  dht.begin();

  //Start Anemometer
  Serial.begin(9600);  //Start the serial connection

  //Set up Servos
  s1a.attach(8);
  s1b.attach(9);
  s2a.attach(10);
  s2b.attach(11);

  releaseClamps();

  //Set up pyro channel
  pinMode(5, OUTPUT);
}

void loop() 
{
  idle();
  if(xbee.read() == "$ countdown")
  { 
    launchProtocol();
  }
}

// ********************************** LAUNCH PROTOCOLS  **********************************

void launchProtocol()
{
  if(checkWeather() == "O.")
  {
    lcd.print("Optimal Protocol");
    lcd.setCursor(0,1);
    lcd.print("Launching.");
    delay(1000);
    optimalProtocol();
  }
  else if(checkWeather() == "S.")
  {
    subOptimalProtocol();
  }
  else if(checkWeather() == "P.")
  {
    poorProtocol();
  }
}

void optimalProtocol()
{
  clampRocket();
  countdown();
  return;
}

void subOptimalProtocol()
{
  lcd.print("Continue?");

  if(xbee.read() == "$ yes")
  {
    countdown();
    return;
  }
  else if(xbee.read() == "$ no")
  {
    cancel();
    return;
  }
}

void poorProtocol()
{
  cancel();
  return;
}

// ********************************** LAUNCH METHODS  **********************************

void clampRocket()
{
  //Upper Servos
  s1a.write(90);
  s1b.write(90);

  //Lower Servos
  s2a.write(90);
  s2b.write(90);
}

void releaseClamps()
{
  //Lower Servos
  s1a.write(0);
  s1b.write(180);

  //Upper Servos
  s2a.write(0);
  s2b.write(180);
}

void countdown()
{
  float remainingTime = countdownTime;

  float idk = countdownTime;

  while (remainingTime >= 0) {
    display.showNumberDecEx(remainingTime * 100, 0b01000000, true);
    remainingTime -= 0.022;
    idk -= 0.022;

    if(countdownTime - idk >= 1)
    {
      idk = countdownTime;
      tone(4, 3000, 25);
    }
  }

  //Release Rocket Clamps
  if(remainingTime < 0.5)
  {
    
    launch();
    display.clear();
    lcd.clear();
    Serial.begin(9600);
    return;
  }

  if(xbee.read() == "$ cancel")
  {
    cancel();
    Serial.begin(9600);
    return;
  }
}

void launch()
{
  releaseClamps();
  digitalWrite(5, HIGH);
  noTone(4);
  delay(3000);
  digitalWrite(5, LOW);
}

void cancel()
{
  lcd.clear();
  lcd.print("Canceling...");
  releaseClamps();
  delay(2000);
}

// ********************************** WEATHER METHODS  **********************************

void idle()
{
  lcd.clear();
  displayConditions();
  delay(5000);
  lcd.clear();
}

void displayConditions()
{
  int temp = dht.readTemperature(true);
  int hum = dht.readHumidity();
  int wS = getWindSpeed();

  String status = "WX: " + checkWeather();

  lcd.print("T");
  lcd.print((char) 223);
  lcd.print(":");
  lcd.print(temp);

  lcd.print((char) 223);
  lcd.print("F ");
  lcd.print("HMD:");
  lcd.print(hum);
  lcd.print("%");


  lcd.setCursor(0,1);

  lcd.print("WS:");
  lcd.print(wS);
  lcd.print("MPH ");

  lcd.print(status);

}

String checkWeather()
{
  int temp = getTemperature();
  int hum = getHumidity();
  int ws = getWindSpeed();

  if(temp >= 90 && hum >= 50 && ws <= 20)
  {
    return "O.";
  }
  else if(temp >= 80 && hum >= 30 && ws <= 20)
  {
    return "S.";
  }
  else
  {
    return "P.";
  }
}

int getTemperature()
{
  return dht.readTemperature(true);
}

int getHumidity()
{
  return dht.readHumidity();
}

int getWindSpeed()
{
  int windSpeed = 0;

  sensorValue = analogRead(sensorPin); //Get a value between 0 and 1023 from the analog pin connected to the anemometer
 
  sensorVoltage = sensorValue * voltageConversionConstant; //Convert sensor value to actual voltage
 
  //Convert voltage value to wind speed using range of max and min voltages and wind speed for the anemometer
  if (sensorVoltage <= voltageMin)
  {
    windSpeed = 0; //Check if voltage is below minimum value. If so, set wind speed to zero.
  }

  else 
  {
    windSpeed = (sensorVoltage - voltageMin) * windSpeedMax / (voltageMax - voltageMin); //For voltages above minimum value, use the linear relationship to calculate wind speed.
  }

  return windSpeed;
}