#include "include/pid.hpp"
#include <chrono>
#include <algorithm>
#include <spdlog/spdlog.h>

PID::PID(float P, float I, float D, float ramp, float limit)
    : P(P)
    , I(I)
    , D(D)
    , setpoint_ramp(ramp)
    , limit(limit)
    , error_prev(0.0f)
    , setpoint_prev(0.0f)
    , integral_prev(0.0f)
{
    timestamp_prev = std::chrono::high_resolution_clock::now();
}

float PID::operator() (float setpoint, float process_variable){
    // calculate the time from the last call
    auto timestamp_now = std::chrono::high_resolution_clock::now();
    long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(timestamp_now - timestamp_prev).count() / 1000000.0;

    if (delta_seconds > PID_RESET_DELAY_S) {
        reset();
        delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(timestamp_now - timestamp_prev).count() / 1000000.0;
    }

    if(setpoint_ramp > 0){
        float setpoint_rate = (setpoint - setpoint_prev) / delta_seconds;
        if (setpoint_rate > setpoint_ramp) {
            setpoint = setpoint_prev + setpoint_ramp * delta_seconds;
        } else if (setpoint_rate < -setpoint_ramp) {
            setpoint = setpoint_prev - setpoint_ramp * delta_seconds;
        }
    }

    float error = setpoint - process_variable;

    float proportional = P * error;
    // Tustin transform of the integral part
    float integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev);
    float derivative = D * ((error - error_prev) / delta_seconds);

    float output = proportional + integral + derivative;

    // antiwindup - limit the output variable
    output = std::min(std::max(-limit, output), limit);
    // spdlog::info("E{} P{} I{} D{} O{} EP{} DS{}", error, proportional, integral, derivative, output, error_prev, delta_seconds);
    
    integral_prev = integral;
    setpoint_prev = setpoint;
    error_prev = error;
    timestamp_prev = timestamp_now;
    return output;
}

void PID::reset() {
    spdlog::info("RESET");
    integral_prev = 0;
    setpoint_prev = 0;
    error_prev = 0;
    timestamp_prev = std::chrono::high_resolution_clock::now();
}