/*
  Controlling a servo position using a potentiometer (variable resistor)
  by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>

  modified on 8 Nov 2013
  by Scott Fitzgerald
  http://www.arduino.cc/en/Tutorial/Knob
*/

#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <dht.h>
#include <ezButton.h>

Servo blendServo;  // create servo object to control a servo

int potpin = 0;  // analog pin used to connect the potentiometer
int val;    // variable to read the value from the analog pin

int setTempPin = 2;  // analog pin used to connect the potentiometer
int setTempVal;    // variable to read the value from the analog pin
dht DHT;
#define DHT22_PIN 5

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

#define BACKLIGHT_BUTTON 4

//ToDo: set all the variables in memory, and only display them/send them if they change
int currentTemp;
int heatIndex;
int engineTemp;
int setTemp;
int fanSpeed;
int blend = 0;
String mode;

ezButton backlightButton(BACKLIGHT_BUTTON);

const float BETA = 3950; // should match the Beta Coefficient of the thermistor
int backLightOn = 0;


LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

double Setpoint, Input, Output;


void setup() {
  Serial.begin(9600);
  blendServo.attach(9);  // attaches the servo on pin 9 to the servo object
  backlightButton.setDebounceTime(50);
  lcd.init();
  //lcd.backlight();
  splash();

  prepareDisplay();

}



void loop() {
  backlightButton.loop();

  int chk = DHT.read22(DHT22_PIN); //Reads the values from teh DHT22
  float celsius = DHT.temperature;
  double fahrenheit = cToF(celsius);
  double humidity = DHT.humidity;
  
  int engineAnalogValue = analogRead(A0);
  float engineTempC = 1 / (log(1 / (1023. / engineAnalogValue - 1)) / BETA + 1.0 / 298.15) - 273.15;
  int engineTempF = (int) cToF(engineTempC);
  // Serial.println(engineTempF);
  printEngineTempF(engineTempF);

  printCurF(fahrenheit);

  //Heat Index
  double hi = calcHeatIndex(fahrenheit, humidity);
  printHi(hi);

  float setTempRaw = analogRead(setTempPin);            // reads the value of the potentiometer (value between 0 and 1023)
  double setTemp = potToTemp(setTempRaw);

  printSetTemp(setTemp);

  //Setpoint = setTemp;
  Input = hi;

  int tempDiff = diffInTemp(setTemp, hi);
  // Serial.println(tempDiff);
  if (tempDiff > 0){
    //Heating
    int fanSpeed = setFanSpeed(tempDiff);
    printFan(fanSpeed);
    printMode("H");
    blend = calculateBlend(hi, setTemp);
    
   
  }else{
    //Cooling
    int fanSpeed = setFanSpeed(tempDiff);
    printFan(fanSpeed);
    printMode("C");
    
    //Blend to zero (no warm water circulating)
    blend = 0;
  }

  //coolingPID.Compute();
  //printFan(Output);

  printBlend(blend);
  setBlend(blend);
  //Serial.println(coolingPID.GetDirection());
  // float currentHeat = 50.0;
  // float currentFan = 70.0;
  // float heatPercentage, fanPercentage;
  // calculateBlend(fahrenheit, setTempVal);

  readButton();

  Serial.println(freeMemory());
  // delay(1000);
  //loopTemp();
}

void readButton(){
  
  if (backlightButton.isPressed()){
    if (backLightOn == 1){
      lcd.noBacklight();
      backLightOn = 0;
    }else{
      lcd.backlight();
      backLightOn = 1;
    }

  }
}

// void loop() {
//   return;
// }

void printEngineTempF(int temp){
  if (temp == engineTemp){
    return;
  }
  //Clear the value
  lcd.setCursor(17, 0);
  lcd.print ("   ");
  
  if (count_digit(temp) == 3){
    lcd.setCursor(17, 0);
  }else{
    lcd.setCursor(18, 0);
  }
  lcd.setCursor(17, 0);
  lcd.print(temp);
  engineTemp = temp;
}

void printFan(double speed){
  if (speed == fanSpeed){
    return;
  }
  lcd.setCursor(5, 2);
  lcd.print("      ");
  lcd.setCursor(5, 2);
  lcd.print(speed);
  fanSpeed = speed;
}

void printCurF(double temp){
  if (temp == currentTemp){
    return;
  }
  //Clear the values
  lcd.setCursor(9, 0);
  lcd.print("   ");
  
  if (count_digit(temp) == 3){
    lcd.setCursor(9, 0);
  }else{
    lcd.setCursor(10, 0);
  }
  
  lcd.print(round(temp));
  currentTemp = temp;
}

void printHi(int hi){
  if (hi == heatIndex){
    return;
  }
  //Clear the values
  lcd.setCursor(13, 0);
  lcd.print("   ");

  if (count_digit(hi) == 3){
    lcd.setCursor(13, 0);
  }else{
    lcd.setCursor(14, 0);
  }
  
  lcd.print(hi);
  heatIndex = hi;
}
void printSetTemp(int temp){
  lcd.setCursor(9, 1);
  lcd.print("   ");
  lcd.setCursor(9, 1);
  lcd.print(temp);

}

void printMode(String mode)
{
  lcd.setCursor(16, 3);
  lcd.print(mode);
}

void printBlend(int blend) {
  lcd.setCursor(6, 3);
  lcd.print("   ");
  lcd.setCursor(6, 3);
  lcd.print(blend);
}

void setBlend(int blend){
    val = map(blend, 0, 100, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
    blendServo.write(val);
}

void prepareDisplay() {
  unsigned int i, k;
  unsigned char ch[5];
  //setCursor(column, row) // 0 base

  lcd.setCursor(0, 0);
  lcd.print(F("T F/HI/E:"));
  lcd.setCursor(12, 0);
  lcd.print(F("/"));
  lcd.setCursor(16, 0);
  lcd.print(F("/"));
  lcd.setCursor(0, 1);
  lcd.print(F("Set Temp:"));
  lcd.setCursor(0, 2);
  lcd.print(F("Fan:"));
  lcd.setCursor(0, 3);
  lcd.print(F("Blend:"));
  lcd.setCursor(10, 3);
  lcd.print("Mode:");
  


}

void splash() {
  lcd.backlight();
  clearDisplay();
  
  centerPrint("CarHVAC", 0);
  centerPrint("v0.1", 1);

  delay(1000);
  lcd.noBacklight();
  clearDisplay();
}

void centerPrint(String text, int row)
{
  int start_col = int round((LCD_COLUMNS - text.length()) / 2);
  lcd.setCursor(start_col, row);
  lcd.print(text);
}

void clearDisplay() {
  lcd.setCursor(0, 0);
  lcd.print("                    ");
  lcd.setCursor(0, 1);
  lcd.print("                    ");
  lcd.setCursor(0, 2);
  lcd.print("                    ");
  lcd.setCursor(0, 3);
  lcd.print("                    ");
}

//Temp Set 65f - 85F
float potToTemp(int pos) {
  pos += 1;
  float InputLow = 1;
  float InputHigh = 1024;
  float OutputLow = 65;
  float OutputHigh = 85;
  //return pos;
  //return ((pos - InputLow) / (InputHigh - InputLow)) * 100;
  return ((pos - InputLow) / (InputHigh - InputLow)) * (OutputHigh - OutputLow) + OutputLow;
  //(500 - 0) / (1022) * (20) + 65;
}

int diffInTemp(int setTemp, int curTemp) {
  return setTemp - curTemp;
}

int setFanSpeed(int tempDiff) {
  tempDiff = abs(tempDiff);
  if (tempDiff > 30) {
    return 100;
  }
  if (tempDiff > 20 && tempDiff < 30) {
    return 75;
  }
  if (tempDiff > 10 && tempDiff < 20) {
    return 50;
  }
  if (tempDiff > 0 && tempDiff < 10) {
    return 25;
  }
  if (tempDiff == 0) {
    return 10;
  }
}

double cToF(float c) {
  double fahrenheit = round((c * 9 / 5) + 32);
  return fahrenheit;
}

int calculateBlend(float currentTemp, float desiredTemp) {
  // calculate the temperature difference
  float tempDifference = desiredTemp - currentTemp;

  // calculate the heat percentage
  float heatPercentage = tempDifference * 5;

  // cap the heat percentage to 100%
  if (heatPercentage > 100.0) {
    heatPercentage = 100.0;
  } else if (heatPercentage < 0.0) {
    heatPercentage = 0.0;
  }
  return heatPercentage;
  // calculate the fan percentage
  float fanPercentage = 100.0 - heatPercentage;


}

//temp in F
double calcHeatIndex(double tempF, double humidity) {
  if (tempF <= 40) {
    return tempF;
  }
  double hi = 0.5 * (tempF + 61.0 + ((tempF - 68.0) * 1.2) + (humidity * 0.094));

  if (hi >= 80.0) {
    hi = -42.379 + 2.04901523 * tempF + 10.14333127 * humidity - 0.22475541 * tempF * humidity - 0.00683783 * pow(tempF, 2) - 0.05481717 * pow(humidity, 2) + 0.00122874 * pow(tempF, 2) * humidity + 0.00085282 * tempF * pow(humidity, 2) - 0.00000199 * pow(tempF, 2) * pow(humidity, 2);

    if (humidity < 13.0 && tempF >= 80.0 && tempF <= 112.0) {
      hi -= ((13.0 - humidity) / 4.0) * sqrt((17.0 - abs(tempF - 95.0)) / 17.0);
    }
    else if (humidity > 85.0 && tempF >= 80.0 && tempF <= 87.0) {
      hi += ((humidity - 85.0) / 10.0) * ((87.0 - tempF) / 5.0);
    }
  }

  return hi;

}

int count_digit(int number) {
   return int(log10(number) + 1);
}

#ifdef __arm__
// should use uinstd.h to define sbrk but Due causes a conflict
extern "C" char* sbrk(int incr);
#else  // __ARM__
extern char *__brkval;
#endif  // __arm__

int freeMemory() {
  char top;
#ifdef __arm__
  return &top - reinterpret_cast<char*>(sbrk(0));
#elif defined(CORE_TEENSY) || (ARDUINO > 103 && ARDUINO != 151)
  return &top - __brkval;
#else  // __arm__
  return __brkval ? &top - __brkval : &top - __malloc_heap_start;
#endif  // __arm__
}