#include "History.h"
#include "Config.h"

extern class Preferences preferences;
CHistory history;

CHistory::CHistory() 
    : head(0), tail(0), count(0), lastTemp(0), lastHumid(0)
{
    Kd_Humidity = 3.6;       // Load Kd for humidity
    Kd_Humidity = 1.8;
    LR_Humidity = 0.25;

    Kp_Temp1 = 3.6;
    Kd_Temp1 = 1.8;
    LR_Temp1 = 0.25;

    head = 0;
    tail = 0;
    count = 0;

    lastTemp = 0;
    lastHumid = 0;
    for (int i = 0; i < RING_SIZE; i++)
        ring[i] = {{0}};
}

void CHistory::init(int16_t _lastTemp, int16_t _lastHumid) {
    lastTemp = _lastTemp;
    lastHumid = _lastHumid;
}

void CHistory::loadPID() {
    Kp_Humidity = config.Kp_Humidity;
    Kd_Humidity = config.Kd_Humidity;
    LR_Humidity = config.LR_Humidity;

    Kp_Temp1 = config.Kp_Temp1;
    Kd_Temp1 = config.Kd_Temp1;
    LR_Temp1 = config.LR_Temp1;

    Kp_Temp2 = config.Kp_Temp2;
    Kd_Temp2 = config.Kd_Temp2;
    LR_Temp2 = config.LR_Temp2;

    Kp_Temp3 = config.Kp_Temp3;
    Kd_Temp3 = config.Kd_Temp3;
    LR_Temp3 = config.LR_Temp3;

}

void CHistory::savePID() {
    config.Kp_Humidity = Kp_Humidity;
    config.Kd_Humidity = Kd_Humidity;
    config.LR_Humidity = LR_Humidity;

    config.Kp_Temp1 = Kp_Temp1;
    config.Kd_Temp1 = Kd_Temp1;
    config.LR_Temp1 = LR_Temp1;

    config.Kp_Temp2 = Kp_Temp2;
    config.Kd_Temp2 = Kd_Temp2;
    config.LR_Temp2 = LR_Temp2;

    config.Kp_Temp3 = Kp_Temp3;
    config.Kd_Temp3 = Kd_Temp3;
    config.LR_Temp3 = LR_Temp3;
}

// Method to add status data to the history buffer
MY_IRAM_ATTR short CHistory::add(STATUS_TYPE &status) {

    ring[head] = status;

    if (++head >= RING_SIZE) head = 0;  // Mask with 0xFF for wrap-around
    if (count < RING_SIZE) {
        count++;
    } else {
        if (++tail > RING_SIZE) tail = 0;  // Advance tail if buffer is full
    }
    return count;
}

// Main PD Control method for heater duty calculation
MY_IRAM_ATTR int16_t CHistory::calculateDutyForTemp1(int16_t setpoint, int16_t curTemp, int16_t lastDuty) {
    // Calculate Proportional term
    float currentError = setpoint - curTemp;
    float P = Kp_Temp1 * currentError;

    // Calculate Derivative term based on temperature slope
    float timeInterval = 10.0;  // Assuming a 5-second interval between updates; adjust as needed
    float slope = (curTemp - lastTemp) / timeInterval;
    float D = Kd_Temp1 * (-slope);  // Negative sign to counteract the rising trend

    // Limit magnitute of change    
    int16_t diff = roundf(P + D);
    if (diff > 500) diff = 500;
    else if (diff < -500) diff = -500;

    // Calculate new heater duty cycle
    int16_t newDuty = lastDuty + diff;

    // Constrain the duty cycle within the range of 0 to 10000
    if (newDuty < 0) newDuty = 0;
    else if (newDuty > 10000) newDuty = 10000;

    // Adjust Kp and Kd using gradient descent based on the current and previous errors
    adjustGainsUsingGradientDescent(Kp_Temp1, Kd_Temp1, currentError, setpoint, lastTemp, 100.0f);
    if (Kp_Temp1 < 0.5f * config.Kp_Temp1) Kp_Temp1 = 0.5f * config.Kp_Temp1;
    else if (Kp_Temp1 > 2.0f * config.Kp_Temp1) Kp_Temp1 = 2.0f * config.Kp_Temp1; 
    if (Kd_Temp1 < 0.5f * config.Kd_Temp1) Kd_Temp1 = 0.5f * config.Kd_Temp1;
    else if (Kd_Temp1 > 2.0f * config.Kd_Temp1) Kd_Temp1 = 2.0f * config.Kd_Temp1; 

    lastTemp = curTemp;
    return newDuty;
}


MY_IRAM_ATTR int16_t CHistory::calculateDutyForTemp2(int16_t setpoint, int16_t curTemp, int16_t lastDuty) {
    // Calculate Proportional term
    float currentError = setpoint - curTemp;
    float P = Kp_Temp2 * currentError;

    // Calculate Derivative term based on temperature slope
    float timeInterval = 10.0;  // Assuming a 5-second interval between updates; adjust as needed
    float slope = (curTemp - lastTemp) / timeInterval;
    float D = Kd_Temp2 * (-slope);  // Negative sign to counteract the rising trend

    // Limit magnitute of change    
    int16_t diff = roundf(P + D);
    if (diff > 500) diff = 500;
    else if (diff < -500) diff = -500;

    // Calculate new heater duty cycle
    int16_t newDuty = lastDuty + diff;

    // Constrain the duty cycle within the range of 0 to 10000
    if (newDuty < 0) newDuty = 0;
    else if (newDuty > 10000) newDuty = 10000;

    // Adjust Kp and Kd using gradient descent based on the current and previous errors
    adjustGainsUsingGradientDescent(Kp_Temp2, Kd_Temp2, currentError, setpoint, lastTemp, 100.0f);
    if (Kp_Temp2 < 0.5f * config.Kp_Temp2) Kp_Temp2 = 0.5f * config.Kp_Temp2;
    else if (Kp_Temp2 > 2.0f * config.Kp_Temp2) Kp_Temp2 = 2.0f * config.Kp_Temp2; 
    if (Kd_Temp2 < 0.5f * config.Kd_Temp2) Kd_Temp2 = 0.5f * config.Kd_Temp2;
    else if (Kd_Temp2 > 2.0f * config.Kd_Temp2) Kd_Temp2 = 2.0f * config.Kd_Temp2; 

    lastTemp = curTemp;
    return newDuty;
}


MY_IRAM_ATTR int16_t CHistory::calculateMistDuty(uint16_t setpoint, uint16_t curHumid, int16_t lastDuty) {
    // Calculate Proportional (P) term based on the error (difference between setpoint and current humidity)
    float currentError = setpoint - curHumid;
    float P = Kp_Humidity * currentError;

    // Calculate Derivative (D) term based on humidity slope
    float timeInterval = 10.0;  // Assuming a 10-second interval between updates; adjust as needed
    float slope = (curHumid - lastHumid) / timeInterval;
    float D = Kd_Humidity * (-slope);  // Negative sign to counteract the rising or falling trend

    // Limit magnitute of change    
    int16_t diff = roundf(P + D);
    if (diff > 500) diff = 500;
    else if (diff < -500) diff = -500;

    // Calculate the new mist duty cycle
    int16_t newDuty = lastDuty + diff;

    // Constrain the duty cycle within the allowable range (0 to 511 for 50% duty cycle)
    if (newDuty < 0) newDuty = 0;
    else if (newDuty > 10000) newDuty = 10000;

    // Adjust Kp and Kd using gradient descent based on current and previous errors
    adjustGainsUsingGradientDescent(Kp_Humidity, Kd_Humidity, currentError, setpoint, lastHumid, 300.0f);
    if (Kp_Humidity < 0.5f * config.Kp_Humidity) Kp_Humidity = 0.5f * config.Kp_Humidity;
    else if (Kp_Humidity > 2.0f * config.Kp_Humidity) Kp_Humidity = 2.0f * config.Kp_Humidity; 
    if (Kd_Humidity < 0.5f * config.Kd_Humidity) Kd_Humidity = 0.5f * config.Kd_Humidity;
    else if (Kd_Humidity > 2.0f * config.Kd_Humidity) Kd_Humidity = 2.0f * config.Kd_Humidity; 

    lastHumid = curHumid;
    return newDuty;
}

// Method to adjust Kp and Kd using gradient descent based on current and previous errors
MY_IRAM_ATTR void CHistory::adjustGainsUsingGradientDescent(float &Kp, float &Kd, float currentError, uint16_t setpoint, float prevTemperature, float MAX_EXPECTED_ERROR) {
    // Normalize the error (assuming a max expected error for normalization)
    float normalizedError = currentError / MAX_EXPECTED_ERROR;

    // Calculate change in temperature as the slope
    float temperatureSlope = (setpoint - prevTemperature);

    // Define limits for adjusting Kp and Kd
    const float proportionalLimitFactorKp = 0.25f;
    const float proportionalLimitFactorKd = 0.25f;

    // Scale the gradients based on normalized error and temperature slope
    float limitedProportionalGradient = proportionalLimitFactorKp * Kp;
    float limitedDerivativeGradient = proportionalLimitFactorKd * Kd;

    // Update Kp and Kd using their respective gradients, ensuring adjustments are within limits
    Kp -= LR_Temp1 * fmin(fabs(normalizedError), limitedProportionalGradient) * (normalizedError < 0 ? 1 : -1);
    Kd -= LR_Temp1 * fmin(fabs(temperatureSlope), limitedDerivativeGradient) * (temperatureSlope < 0 ? 1 : -1);

    // Constrain Kp and Kd to avoid going below minimum threshold values
    const float MIN_KP = 0.01f;
    const float MIN_KD = 0.01f;
    Kp = max(Kp, MIN_KP);
    Kd = max(Kd, MIN_KD);
}