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

// Default address is 0x48. ADDR pin to GND.
#define I2C_ADS1115_SENSOR_01 0x48
#define I2C_ADS1115_SENSOR_02 0x49
// Create object
ADS1115_WE ADS1115_SENSOR_01 = ADS1115_WE(I2C_ADS1115_SENSOR_01);
ADS1115_WE ADS1115_SENSOR_02 = ADS1115_WE(I2C_ADS1115_SENSOR_02);


bool  ADS1115_SENSOR_01_STATE = true;
bool  ADS1115_SENSOR_02_STATE = true;

double ADS1115_SENSOR_01_A0_SETTINGS = ADS1115_COMP_0_GND;
int   ADS1115_SENSOR_01_A0_ARRAY_INDEX = 5;
float ADS1115_SENSOR_01_A0_ARRAY [] = {3.2, 3.2, 3.2, 3.2, 3.2};
float ADS1115_SENSOR_01_A0_AVERAGE_VOLTAGE = 3.8; //4.0229885057
float ADS1115_SENSOR_01_A0_ACTUAL_VOLTAGE = 4.01; //4.0229885057
bool  ADS1115_SENSOR_01_A0_STATE = true;

double ADS1115_SENSOR_01_A1_SETTINGS = ADS1115_COMP_1_GND;
int   ADS1115_SENSOR_01_A1_ARRAY_INDEX = 5;
float ADS1115_SENSOR_01_A1_ARRAY [] = {3.2, 3.2, 3.2, 3.2, 3.2};
float ADS1115_SENSOR_01_A1_AVERAGE_VOLTAGE = 3.8; //4.0229885057
float ADS1115_SENSOR_01_A1_ACTUAL_VOLTAGE = 4.01; //4.0229885057
bool  ADS1115_SENSOR_01_A1_STATE = true;

double ADS1115_SENSOR_01_A2_SETTINGS = ADS1115_COMP_2_GND;
int   ADS1115_SENSOR_01_A2_ARRAY_INDEX = 5;
float ADS1115_SENSOR_01_A2_ARRAY [] = {3.2, 3.2, 3.2, 3.2, 3.2};
float ADS1115_SENSOR_01_A2_AVERAGE_VOLTAGE = 3.8; //4.0229885057
float ADS1115_SENSOR_01_A2_ACTUAL_VOLTAGE = 4.01; //4.0229885057
bool  ADS1115_SENSOR_01_A2_STATE = true;

double ADS1115_SENSOR_01_A3_SETTINGS = ADS1115_COMP_3_GND;
int   ADS1115_SENSOR_01_A3_ARRAY_INDEX = 5;
float ADS1115_SENSOR_01_A3_ARRAY [] = {3.2, 3.2, 3.2, 3.2, 3.2};
float ADS1115_SENSOR_01_A3_AVERAGE_VOLTAGE = 3.8; //4.0229885057
float ADS1115_SENSOR_01_A3_ACTUAL_VOLTAGE = 4.01; //4.0229885057
bool  ADS1115_SENSOR_01_A3_STATE = true;



double ADS1115_SENSOR_02_A0_SETTINGS = ADS1115_COMP_0_GND;
int   ADS1115_SENSOR_02_A0_ARRAY_INDEX = 5;
float ADS1115_SENSOR_02_A0_ARRAY [] = {3.2, 3.2, 3.2, 3.2, 3.2};
float ADS1115_SENSOR_02_A0_AVERAGE_VOLTAGE = 3.8; //4.0229885057
float ADS1115_SENSOR_02_A0_ACTUAL_VOLTAGE = 4.01; //4.0229885057
bool  ADS1115_SENSOR_02_A0_STATE = true;

double ADS1115_SENSOR_02_A1_SETTINGS = ADS1115_COMP_1_GND;
int   ADS1115_SENSOR_02_A1_ARRAY_INDEX = 5;
float ADS1115_SENSOR_02_A1_ARRAY [] = {3.2, 3.2, 3.2, 3.2, 3.2};
float ADS1115_SENSOR_02_A1_AVERAGE_VOLTAGE = 3.8; //4.0229885057
float ADS1115_SENSOR_02_A1_ACTUAL_VOLTAGE = 4.01; //4.0229885057
bool  ADS1115_SENSOR_02_A1_STATE = true;

double ADS1115_SENSOR_02_A2_SETTINGS = ADS1115_COMP_2_GND;
int   ADS1115_SENSOR_02_A2_ARRAY_INDEX = 5;
float ADS1115_SENSOR_02_A2_ARRAY [] = {3.2, 3.2, 3.2, 3.2, 3.2};
float ADS1115_SENSOR_02_A2_AVERAGE_VOLTAGE = 3.8; //4.0229885057
float ADS1115_SENSOR_02_A2_ACTUAL_VOLTAGE = 4.01; //4.0229885057
bool  ADS1115_SENSOR_02_A2_STATE = true;

double ADS1115_SENSOR_02_A3_SETTINGS = ADS1115_COMP_3_GND;
int   ADS1115_SENSOR_02_A3_ARRAY_INDEX = 5;
float ADS1115_SENSOR_02_A3_ARRAY [] = {3.2, 3.2, 3.2, 3.2, 3.2};
float ADS1115_SENSOR_02_A3_AVERAGE_VOLTAGE = 3.8; //4.0229885057
float ADS1115_SENSOR_02_A3_ACTUAL_VOLTAGE = 4.01; //4.0229885057
bool  ADS1115_SENSOR_02_A3_STATE = true;

float LED_ALERT_TEMPERATURE = 0.0;
float LED_TEMPERATURE = 0.0;
float LED_FAN_LOW_VOLTAGE = 0.0;
float LED_FAN_HIGH_VOLTAGE = 0.0;
int LED_PWM = 0;
int LED_FAN_PWM = 0;
bool LED_STATE = true;
int LED_WAITING = 10;

float TEMPERATURE = 0.0;
float FAN_LOW_VOLTAGE = 0.0;
float FAN_HIGH_VOLTAGE = 0.0;
int FAN_PWM = 0;
int FAN_WAITING = 10;

int SERVO_PWM = 0;
int SERVO_WAITING = 10;

//PWM Pins D3, D5, D6, D9, D10, D11
#define LED_FAN_PWM_PIN 5
#define FAN_PWM_PIN 6
#define LED_PWM_PIN 9
#define SERVO_PWM_PIN 10


void setup() {
  Wire.begin();
  Serial.begin(9600);
  
  if(!ADS1115_SENSOR_01.init()){
    Serial.println("ADS1115_SENSOR_01 not connected!");
    ADS1115_SENSOR_01_STATE = false;
  }
  if(!ADS1115_SENSOR_02.init()){
    Serial.println("ADS1115_SENSOR_02 not connected!");
    ADS1115_SENSOR_02_STATE = false;
  }
  /* Set voltage range:
     ADS1115_RANGE_6144  +/- 6.144V
     ADS1115_RANGE_4096  +/- 4.096V
     ADS1115_RANGE_2048  +/- 2.048V (default)
     ADS1115_RANGE_1024  +/- 1.024V
     ADS1115_RANGE_0512  +/- 0.512V
     ADS1115_RANGE_0256  +/- 0.256V
  */
  ADS1115_SENSOR_01.setVoltageRange_mV(ADS1115_RANGE_4096); 
  ADS1115_SENSOR_02.setVoltageRange_mV(ADS1115_RANGE_4096); 

  /* Set conversion rate:
     Higher rate = faster, lower resolution
     Lower rate = slower, higher resolution
  */
  pinMode(FAN_PWM_PIN, OUTPUT);
  pinMode(LED_PWM_PIN, OUTPUT);
  pinMode(LED_FAN_PWM_PIN, OUTPUT);
  analogWrite(FAN_PWM_PIN, FAN_PWM);
  analogWrite(LED_PWM_PIN, LED_PWM);
  analogWrite(LED_FAN_PWM_PIN, LED_FAN_PWM);
}

float getTEMPERATUREValue(float actualVoltage){
  //10k NTC (25C° 10000 Oht - 100C° 680 Ohm) R2=2800 Ohm;
  // Umin=5V/(10000 Ohm+2800 Ohm)*2800 Ohm = 1.09375V
  // Umax=5V-(5 V/(680 Ohm+2800 Ohm)*680 Ohm) = 4.0229885057V
  float voltage = actualVoltage;
  float minVoltage = 1.09375; 
  float maxVoltage = 4.0229885057;
  float TEMPValue = ((voltage-minVoltage)/(maxVoltage))*100.0+25.0;
  return TEMPValue;
}

float getPWMValue(float actualVoltage, float lowVoltage, float highVoltage){
  //10k NTC (25C° 10000 Oht - 100C° 680 Ohm) R2=2800 Ohm;
  // Umin=5V/(10000 Ohm+2800 Ohm)*2800 Ohm = 1.09375V
  // Umax=5V-(5 V/(680 Ohm+2800 Ohm)*680 Ohm) = 4.0229885057V
  float voltage = actualVoltage;
  float minVoltage = 1.09375; 
  float maxVoltage = 4.0229885057;
  if (highVoltage < maxVoltage) {
    maxVoltage = highVoltage;
  }
  if (voltage < minVoltage){
    voltage = minVoltage;
  }
  if (voltage < lowVoltage){
    voltage = lowVoltage;
  }
  if (voltage > maxVoltage){
    voltage = maxVoltage;
  }
  int PWM = ((voltage-minVoltage)/(maxVoltage-minVoltage))*255;
  return PWM;
}

float getSERVOPWMValue(float actualVoltage, float lowVoltage, float highVoltage){
  //10k NTC (25C° 10000 Oht - 100C° 680 Ohm) R2=2800 Ohm;
  // Umin=5V/(10000 Ohm+2800 Ohm)*2800 Ohm = 1.09375V
  // Umax=5V-(5 V/(680 Ohm+2800 Ohm)*680 Ohm) = 4.0229885057V
  float voltage = actualVoltage;
  float minVoltage = 1.09375; 
  float maxVoltage = 4.0229885057;
  if (highVoltage < maxVoltage) {
    maxVoltage = highVoltage;
  }
  if (voltage < minVoltage){
    voltage = minVoltage;
  }
  if (voltage < lowVoltage){
    voltage = lowVoltage;
  }
  if (voltage > maxVoltage){
    voltage = maxVoltage;
  }
  int PWM = ((voltage-minVoltage)/(maxVoltage-minVoltage))*180;
  return PWM;
}


void ADS1115_READ_SENSOR(ADS1115_WE SENSOR, int16_t SENSOR_SETTINGS, bool SENSOR_STATE, float SENSOR_ARRAY[], int SENSOR_ARRAY_INDEX, float *SENSOR_ACTUAL, float *SENSOR_AVERAGE){
  if (SENSOR_STATE){
    float voltage = 0.0;
    SENSOR.setCompareChannels(SENSOR_SETTINGS);
    *SENSOR_ACTUAL = SENSOR.getResult_V();
    int i;
    for (i = 1; i < SENSOR_ARRAY_INDEX; i++) {
      SENSOR_ARRAY[i-1] = SENSOR_ARRAY[i];
    }
    SENSOR_ARRAY[SENSOR_ARRAY_INDEX-1] = *SENSOR_ACTUAL;
    
    *SENSOR_AVERAGE = 0.0;
    for (i = 0; i < SENSOR_ARRAY_INDEX; i++) {
      *SENSOR_AVERAGE = *SENSOR_AVERAGE + SENSOR_ARRAY[i];
      
      Serial.print(SENSOR_ARRAY[i]);
      Serial.print(", ");
      
    }
    *SENSOR_AVERAGE = *SENSOR_AVERAGE/SENSOR_ARRAY_INDEX;
    
    Serial.print("ACTUAL: ");
    Serial.print(*SENSOR_ACTUAL);
    Serial.print("V, AVERAGE: ");
    Serial.print(*SENSOR_AVERAGE);
    Serial.println(" V");
    
  } else {
    Serial.println(" ADS1115_SENSOR ERROR! ");
  }
}

void loop() {

  ADS1115_READ_SENSOR(ADS1115_SENSOR_01, ADS1115_SENSOR_01_A0_SETTINGS, ADS1115_SENSOR_01_A0_STATE, ADS1115_SENSOR_01_A0_ARRAY, ADS1115_SENSOR_01_A0_ARRAY_INDEX, &ADS1115_SENSOR_01_A0_ACTUAL_VOLTAGE, &ADS1115_SENSOR_01_A0_AVERAGE_VOLTAGE);
  LED_ALERT_TEMPERATURE = getTEMPERATUREValue(ADS1115_SENSOR_01_A0_AVERAGE_VOLTAGE);
  LED_ALERT_TEMPERATURE = 85.0;
  delay(10);
  ADS1115_READ_SENSOR(ADS1115_SENSOR_01, ADS1115_SENSOR_01_A1_SETTINGS, ADS1115_SENSOR_01_A1_STATE, ADS1115_SENSOR_01_A1_ARRAY, ADS1115_SENSOR_01_A1_ARRAY_INDEX, &ADS1115_SENSOR_01_A1_ACTUAL_VOLTAGE, &ADS1115_SENSOR_01_A1_AVERAGE_VOLTAGE);
  FAN_LOW_VOLTAGE = ADS1115_SENSOR_01_A1_AVERAGE_VOLTAGE;
  LED_FAN_LOW_VOLTAGE = 1.6;
  FAN_LOW_VOLTAGE = 1.6;
  delay(10);
  ADS1115_READ_SENSOR(ADS1115_SENSOR_01, ADS1115_SENSOR_01_A2_SETTINGS, ADS1115_SENSOR_01_A2_STATE, ADS1115_SENSOR_01_A2_ARRAY, ADS1115_SENSOR_01_A2_ARRAY_INDEX, &ADS1115_SENSOR_01_A2_ACTUAL_VOLTAGE, &ADS1115_SENSOR_01_A2_AVERAGE_VOLTAGE);
  LED_FAN_HIGH_VOLTAGE = ADS1115_SENSOR_01_A2_AVERAGE_VOLTAGE;
  LED_FAN_HIGH_VOLTAGE = 4.08;
  delay(10);
  ADS1115_READ_SENSOR(ADS1115_SENSOR_01, ADS1115_SENSOR_01_A3_SETTINGS, ADS1115_SENSOR_01_A3_STATE, ADS1115_SENSOR_01_A3_ARRAY, ADS1115_SENSOR_01_A3_ARRAY_INDEX, &ADS1115_SENSOR_01_A3_ACTUAL_VOLTAGE, &ADS1115_SENSOR_01_A3_AVERAGE_VOLTAGE);
  LED_TEMPERATURE = getTEMPERATUREValue(ADS1115_SENSOR_01_A3_AVERAGE_VOLTAGE);
  LED_FAN_PWM = getPWMValue(ADS1115_SENSOR_01_A3_AVERAGE_VOLTAGE, LED_FAN_LOW_VOLTAGE, LED_FAN_HIGH_VOLTAGE);

  analogWrite(LED_FAN_PWM_PIN, LED_FAN_PWM);

  if (LED_TEMPERATURE > LED_ALERT_TEMPERATURE){
    LED_STATE = false;
  } else {
    LED_STATE = true;
  }
  delay(10);
  ADS1115_READ_SENSOR(ADS1115_SENSOR_02, ADS1115_SENSOR_02_A0_SETTINGS, ADS1115_SENSOR_02_A0_STATE, ADS1115_SENSOR_02_A0_ARRAY, ADS1115_SENSOR_02_A0_ARRAY_INDEX, &ADS1115_SENSOR_02_A0_ACTUAL_VOLTAGE, &ADS1115_SENSOR_02_A0_AVERAGE_VOLTAGE);
  SERVO_PWM = getSERVOPWMValue(ADS1115_SENSOR_02_A0_AVERAGE_VOLTAGE, LED_FAN_LOW_VOLTAGE, LED_FAN_HIGH_VOLTAGE);
  SERVO_PWM = 90.0;
  delay(10);
  ADS1115_READ_SENSOR(ADS1115_SENSOR_02, ADS1115_SENSOR_02_A1_SETTINGS, ADS1115_SENSOR_02_A1_STATE, ADS1115_SENSOR_02_A1_ARRAY, ADS1115_SENSOR_02_A1_ARRAY_INDEX, &ADS1115_SENSOR_02_A1_ACTUAL_VOLTAGE, &ADS1115_SENSOR_02_A1_AVERAGE_VOLTAGE);
  LED_PWM = getPWMValue(ADS1115_SENSOR_02_A0_AVERAGE_VOLTAGE, LED_FAN_LOW_VOLTAGE, LED_FAN_HIGH_VOLTAGE);
  delay(10);
  ADS1115_READ_SENSOR(ADS1115_SENSOR_02, ADS1115_SENSOR_02_A2_SETTINGS, ADS1115_SENSOR_02_A2_STATE, ADS1115_SENSOR_02_A2_ARRAY, ADS1115_SENSOR_02_A2_ARRAY_INDEX, &ADS1115_SENSOR_02_A2_ACTUAL_VOLTAGE, &ADS1115_SENSOR_02_A2_AVERAGE_VOLTAGE);
  FAN_HIGH_VOLTAGE = ADS1115_SENSOR_02_A2_AVERAGE_VOLTAGE;
  FAN_HIGH_VOLTAGE = 4.08;
  delay(10);
  ADS1115_READ_SENSOR(ADS1115_SENSOR_02, ADS1115_SENSOR_02_A3_SETTINGS, ADS1115_SENSOR_02_A3_STATE, ADS1115_SENSOR_02_A3_ARRAY, ADS1115_SENSOR_02_A3_ARRAY_INDEX, &ADS1115_SENSOR_02_A3_ACTUAL_VOLTAGE, &ADS1115_SENSOR_02_A3_AVERAGE_VOLTAGE);
  TEMPERATURE = getTEMPERATUREValue(ADS1115_SENSOR_02_A3_AVERAGE_VOLTAGE);
  FAN_PWM = getPWMValue(ADS1115_SENSOR_02_A3_AVERAGE_VOLTAGE, FAN_LOW_VOLTAGE, FAN_HIGH_VOLTAGE);
  
  analogWrite(FAN_PWM_PIN, FAN_PWM);
  analogWrite(LED_PWM_PIN, LED_PWM);
  analogWrite(SERVO_PWM_PIN, SERVO_PWM);

  Serial.print("S1 AD: ");
  Serial.print(ADS1115_SENSOR_01_A3_AVERAGE_VOLTAGE);
  Serial.print(" V, LED Temp: ");
  Serial.print(LED_TEMPERATURE);
  Serial.print(" °C, State: ");
  Serial.print(LED_STATE);
  Serial.print("LED FAN PWM: ");
  Serial.print(LED_FAN_PWM);
  Serial.print("");
  Serial.print("LED PWM: ");
  Serial.print(LED_PWM);
  Serial.println("");

  Serial.print("S2 AD: ");
  Serial.print(ADS1115_SENSOR_02_A3_AVERAGE_VOLTAGE);
  Serial.print(" V, Temp: ");
  Serial.print(TEMPERATURE);
  Serial.print(" °C, ");
  Serial.print("FAN_PWM: ");
  Serial.print(FAN_PWM);
  Serial.print("SERVO PWM: ");
  Serial.print(SERVO_PWM);
  Serial.println("");
  delay(100);
}