//Incubator
/*
    Temp 1

    Temp 2

    HeatMat 1

    HeatMat 2

*/


OneWire t1(2); 

DallasTemperature incubatorSensors(&t1);
unsigned long lastIncubatorReading;
int incubatorReadIntervalMs = 500;  

// OneSensor tempPin1 = { {0, 0, 0, 0, 0},  0,  { 0x28, 0xEE, 0xD5, 0x64, 0x1A, 0x16, 0x02, 0xEC }, 0};
// OneSensor tempPin2 = { {0, 0, 0, 0, 0},  0,  { 0x28, 0xEE, 0xD5, 0x64, 0x1A, 0x16, 0x02, 0xEC }, 0};
int mainSectionPin = 11;
int secondarySectionPin = 12;
IncubatorDrain heatSection1 = {mainSectionPin, false, 'H', { {0, 0, 0, 0, 0},  0,  {  0x28, 0xF6, 0xF2, 0x96, 0xF0, 0x01, 0x3C, 0x47 }, 0}};
IncubatorDrain heatSection2 = {secondarySectionPin, false, 'H', { {0, 0, 0, 0, 0},  0,  { 0x28, 0xEB, 0x74, 0x96, 0xF0, 0x01, 0x3C, 0x01 }, 0}};
 

void setupIncubator(int goal1, int goal2){
    pinMode( heatSection1.pin, OUTPUT); 
    pinMode( heatSection2.pin, OUTPUT); 
    digitalWrite(heatSection1.pin, HIGH);
    digitalWrite(heatSection2.pin, HIGH);
    incubatorSensors.begin();
    lastIncubatorReading = millis();
    initDrainSensor(heatSection1);
    initDrainSensor(heatSection2);
    setAllGoalTemps(goal1, goal2);
    
}

void IncubatorControlLoop () {
    unsigned long currentTime = millis();
    if(currentTime - lastIncubatorReading >= incubatorReadIntervalMs){
        updateIncubatorSensors();
        updateIncubatorDrains();

        lastIncubatorReading = currentTime; 
    }
}
void updateIncubatorDrains(){
    if(abs(heatSection1.sensor.goalVal - heatSection1.sensor.currentAgg) > HEAT_DELTA){
        if(heatSection1.sensor.goalVal > heatSection1.sensor.currentAgg){
            turnOnIncubatorDrain(heatSection1);
        }else if(heatSection1.sensor.goalVal < heatSection1.sensor.currentAgg){
            turnOffIncubatorDrain(heatSection1);
        }
    }
    
    if(abs(heatSection2.sensor.goalVal - heatSection2.sensor.currentAgg) > HEAT_DELTA){
        if(heatSection2.sensor.goalVal > heatSection2.sensor.currentAgg){
            turnOnIncubatorDrain(heatSection2);
        }else if(heatSection2.sensor.goalVal < heatSection2.sensor.currentAgg){
            turnOffIncubatorDrain(heatSection2);
        }
    }
}

void turnOnIncubatorDrain(IncubatorDrain &device)
{
    if(!device.isActive){
        device.isActive = true;
        digitalWrite(device.pin, LOW);
    }
}

void turnOffIncubatorDrain(IncubatorDrain &device)
{
    if(device.isActive){
        device.isActive = false;
        digitalWrite(device.pin, HIGH);
    }
}

void initDrainSensor(IncubatorDrain &device)
{
    float tempC = incubatorSensors.getTempC(device.sensor.address);
    device.sensor.currentAgg = tempC;
    device.sensor.history[0] = tempC;
    device.sensor.history[1] = tempC;
    device.sensor.history[2] = tempC;
    device.sensor.history[3] = tempC;
    device.sensor.history[4] = tempC;
}

void updateIncubatorSensors(){
    incubatorSensors.requestTemperatures();
    getTemperature(heatSection1);
    getTemperature(heatSection2);
}

void setMainChamber(float goal){
    heatSection1.sensor.goalVal = goal; 
}

void setSecondaryChamber(float goal){
    heatSection2.sensor.goalVal = goal; 
}

void setAllGoalTemps(float g1, float g2){
    heatSection1.sensor.goalVal = g1; 
    heatSection2.sensor.goalVal = g2; 
}
 


float *getSectionData(String isMain, float (& array)[8]){ 
    if(isMain.equals("Main")){
        array[0] = heatSection1.sensor.currentAgg;
        array[1] = heatSection1.sensor.history[0];
        array[2] = heatSection1.sensor.history[1];
        array[3] = heatSection1.sensor.history[2];
        array[4] = heatSection1.sensor.history[3];
        array[5] = heatSection1.sensor.history[4]; 
        array[6] = heatSection1.sensor.goalVal; 
        array[7] = heatSection1.isActive ? 1 : 0; 
    }else {
        array[0] = heatSection2.sensor.currentAgg;
        array[1] = heatSection2.sensor.history[0];
        array[2] = heatSection2.sensor.history[1];
        array[3] = heatSection2.sensor.history[2];
        array[4] = heatSection2.sensor.history[3];
        array[5] = heatSection2.sensor.history[4]; 
        array[6] = heatSection2.sensor.goalVal; 
        array[7] = heatSection2.isActive ? 1 : 0; 
    }
}

void setTemperature(String isMain, float goal){
    if(isMain.equals("Main")){ 
        setMainChamber(goal);
    }else {
        setSecondaryChamber(goal);
    }
}


void getTemperature(IncubatorDrain &device)
{
    float tempC = incubatorSensors.getTempC(device.sensor.address); 
    if(abs(device.sensor.history[0] - tempC) > WARNING_LOG_DELTA_THRESHOLD){
        //SOME SORT OF LOGGING HERE
    }
    if(tempC > -120){ //-127c is disconnected
        for(byte i = (sizeof(device.sensor.history) / sizeof(device.sensor.history[0])) - 1; i >= 1; i--){
            device.sensor.history[i] = device.sensor.history[i - 1];
        }
        device.sensor.history[0] = tempC; 
        float sum = 0;
        for(byte i = 0; i < (sizeof(device.sensor.history) / sizeof(device.sensor.history[0])); i++){
            sum += device.sensor.history[i];
        }
        float newAgg = sum / (sizeof(device.sensor.history) / sizeof(device.sensor.history[0]));
        device.sensor.currentAgg = newAgg;   
        printTemperature(device.sensor.address); 
    }else {
        Serial.println("DHT Sensor disconnected");
    }
}

void printTemperature(DeviceAddress deviceAddress)
{
  float tempC = incubatorSensors.getTempC(deviceAddress);
  Serial.print(tempC);
  Serial.print((char)176);
  Serial.println("C  ");
//   Serial.print(DallasTemperature::toFahrenheit(tempC));
//   Serial.print((char)176);
//   Serial.println("F");
}