/*
 * @Author: wzh
 * @Date: 2024-05-27 08:56:58
 * @LastEditors: mypx mypx_coder@163.com
 * @LastEditTime: 2025-09-16 15:28:26
 * @Description:
 *
 * Copyright (c) 2024 by wzh, All Rights Reserved.
 */
#include "etk_pid.h"
#include <float.h>
#include <math.h>
#include <stdio.h>

/**
 * @brief Initialize a positional PID controller with specified gains.
 * @param pid Pointer to the positional PID structure to initialize.
 * @param kp Proportional gain.
 * @param ki Integral gain.
 * @param kd Derivative gain.
 * @param set_point Desired target value.
 * @param integral_limit Maximum limit for the integral term to prevent wind-up.
 */
int etk_position_pid_init(etk_pos_pid_t *pid, float kp, float ki, float kd, float set_point, float integral_limit,
                          float min_output, float max_output, int8_t direction)
{
    if (pid == NULL || kp < 0 || ki < 0 || kd < 0 || integral_limit < 0 || min_output > max_output)
    {
        return -1;
    }
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    // Set the desired setpoint value
    pid->set_point       = set_point;
    pid->prev_error      = 0;
    pid->integral        = 0;
    pid->prev_derivative = 0;
    pid->integral_limit  = fabs(integral_limit); // Ensure positive limit
    pid->min_output      = min_output;
    pid->max_output      = max_output;
    pid->direction       = (direction >= 0) ? 1 : -1; // Normalize direction to 1 or -1

    return 0;
}

/**
 * @brief Update the positional PID controller output based on feedback.
 * @param pid Pointer to the positional PID structure.
 * @param feedback Current feedback value from the system.
 * @param dt Time difference (in seconds) between the current and previous update.
 * @param derivative_filter_gain Gain for smoothing the derivative term.
 * @return PID output after applying the control law.
 */
float etk_position_pid_update(etk_pos_pid_t *pid, float feedback, float dt, float derivative_filter_gain)
{
    // Null pointer check for safety
    if (pid == NULL)
        return 0;

    /* 1: Avoid division by zero;
     * Very small dt (for example, close to 0) may cause the derivative
     * term to be very large and cause numerical instability */
    if (dt <= 1e-6f)
        return 0;
    /* derivative_filter_gain = 0->Ignore the derivative term completely
    derivative_filter_gain = 1.0f -> No filtering at all, using the original
    derivative directly.*/

    if (derivative_filter_gain < 0.0f || derivative_filter_gain > 1.0f)
    {
        derivative_filter_gain = 0.2f; // Default smoothing with 80% filtering
    }

    // Calculate current error with direction control
    float error = pid->direction * (pid->set_point - feedback);

    // Update integral term with error over time
    pid->integral += error * dt;

    // Limit integral to prevent wind-up and numerical overflow
    if (pid->integral > pid->integral_limit)
        pid->integral = pid->integral_limit;
    else if (pid->integral < -pid->integral_limit)
        pid->integral = -pid->integral_limit;

    // Derivative term with optional filtering and numerical stability
    float derivative_raw = (error - pid->prev_error) / dt;
    // Limit derivative term to prevent numerical instability
    const float max_derivative = 1e6f; // Reasonable upper limit for derivative
    if (derivative_raw > max_derivative)
        derivative_raw = max_derivative;
    else if (derivative_raw < -max_derivative)
        derivative_raw = -max_derivative;

    pid->prev_derivative = derivative_filter_gain * derivative_raw
                           + (1.0f - derivative_filter_gain) * pid->prev_derivative;

    // Calculate PID output based on error, integral, and filtered derivative
    float output = pid->kp * error + pid->ki * pid->integral + pid->kd * pid->prev_derivative;
    // Update previous error for next cycle
    pid->prev_error = error;

    /* Output limiting with saturation protection */
    if (output > pid->max_output)
    {
        output = pid->max_output;
    }
    else if (output < pid->min_output)
    {
        output = pid->min_output;
    }

    return output;
}

/**
 * @brief Update the set point of the positional PID controller.
 * @param pid Pointer to the positional PID structure.
 * @param set_point New desired target value.
 */
void etk_position_pid_update_set_point(etk_pos_pid_t *pid, float set_point)
{
    pid->set_point       = set_point;
    pid->integral        = 0;
    pid->prev_error      = 0;
    pid->prev_derivative = 0;
}

/**
 * @brief Reset the states of the positional PID controller.
 * @param pid Pointer to the positional PID structure.
 */
void etk_position_pid_reset(etk_pos_pid_t *pid)
{
    pid->integral        = 0;
    pid->prev_error      = 0;
    pid->prev_derivative = 0;
}

/**
 * @brief Set the direction control for positional PID controller.
 * @param pid Pointer to the positional PID structure.
 * @param direction Error direction: 1 for normal, -1 for inverted.
 */
void et_position_pid_set_direction(etk_pos_pid_t *pid, int8_t direction)
{
    if (pid == NULL)
        return;

    pid->direction = (direction >= 0) ? 1 : -1; // Normalize direction to 1 or -1
}

/**
 * @brief Initialize an incremental PID controller with specified gains.
 * @param pid Pointer to the incremental PID structure.
 * @param kp Proportional gain.
 * @param ki Integral gain.
 * @param kd Derivative gain.
 */
void et_incremental_pid_init(et_inc_pid_t *pid, float kp, float ki, float kd, float min_output, float max_output)
{
    // Null pointer check for safety
    if (pid == NULL)
        return;

    pid->kp          = kp;
    pid->ki          = ki;
    pid->kd          = kd;
    pid->prev_error  = 0;
    pid->prev2_error = 0;
    pid->prev_output = 0;

    pid->set_point = 0;
    // Prevent division by zero when ki is zero
    pid->max_integral = (ki != 0.0f) ? (max_output / ki) : FLT_MAX;
    pid->min_output   = min_output;
    pid->max_output   = max_output; // Set maximum output value
    pid->integral_sum = 0.0f;
}

/**
 * @brief Set a new set point for the incremental PID controller.
 * @param pid Pointer to the incremental PID structure.
 * @param set_point New target set point.
 */
void et_incremental_pid_set_point(et_inc_pid_t *pid, float set_point)
{
    pid->set_point = set_point;
}

/**
 * @brief Reset the states of the incremental PID controller.
 * @param pid Pointer to the incremental PID structure.
 */
void et_incremental_pid_reset(et_inc_pid_t *pid)
{
    pid->prev_error  = 0;
    pid->prev2_error = 0;
    pid->prev_output = 0;
}

/**
 * @brief Update the incremental PID controller with current feedback.
 * @param pid Pointer to the incremental PID structure.
 * @param feedback Current feedback value.
 * @return The output value after applying the control law.
 */
float et_incremental_pid_update(et_inc_pid_t *pid, float feedback)
{
    float error        = pid->set_point - feedback;
    float delta_error  = error - pid->prev_error;
    float delta2_error = delta_error - (pid->prev_error - pid->prev2_error);

    pid->integral_sum += error;
    if (pid->integral_sum > pid->max_integral)
    {
        pid->integral_sum = pid->max_integral;
    }
    else if (pid->integral_sum < -pid->max_integral)
    {
        pid->integral_sum = -pid->max_integral;
    }

    float integral_term = pid->ki * pid->integral_sum;

    float output = pid->prev_output + pid->kp * delta_error + integral_term + pid->kd * delta2_error;

    // limit the output range.
    if (output < pid->min_output)
    {
        output = pid->min_output;
    }
    else if (output > pid->max_output)
    {
        output = pid->max_output;
    }

    pid->prev2_error = pid->prev_error;
    pid->prev_error  = error;
    pid->prev_output = output;

    return output;
}

/**
 * @brief Initialize a fuzzy PID controller with specified gains.
 * @param pid Pointer to the fuzzy PID structure.
 * @param kp Proportional gain.
 * @param ki Integral gain.
 * @param kd Derivative gain.
 */
void fuzzy_pid_init(et_fuzzy_pid_t *pid, float kp, float ki, float kd)
{
    pid->error      = 0.0f;
    pid->prev_error = 0.0f;
    pid->integral   = 0.0f;
    pid->kp         = kp;
    pid->ki         = ki;
    pid->kd         = kd;
    pid->set_point  = 0.0f;
}

/**
 * @brief Update the fuzzy PID controller based on feedback and adjust gains.
 * @param pid Pointer to the fuzzy PID structure.
 * @param set_point Desired target value.
 * @param feedback Current feedback value.
 * @param dt Time difference since the last update.
 * @return The PID output after fuzzy adjustment.
 */
float fuzzy_pid_update(et_fuzzy_pid_t *pid, float set_point, float feedback, float dt)
{
    // Null pointer check for safety
    if (pid == NULL)
        return 0;

    // Use consistent threshold with other PID functions
    if (dt <= 1e-6f)
        return 0; // Avoid division by zero and numerical instability

    pid->error     = set_point - feedback;
    pid->integral += pid->error * dt;

    // Fuzzy adjustment of PID parameters based on error trends
    // Add bounds checking to prevent gains from becoming negative or too large
    if (pid->error > 0 && pid->prev_error > 0)
    {
        pid->kp += 0.1f;
        pid->ki += 0.05f;
        pid->kd += 0.02f;
    }
    else if (pid->error < 0 && pid->prev_error < 0)
    {
        pid->kp -= 0.1f;
        pid->ki -= 0.05f;
        pid->kd -= 0.02f;
    }

    // Limit gains to reasonable ranges to prevent instability
    const float max_gain = 1000.0f;
    const float min_gain = 0.001f;
    pid->kp              = fmaxf(min_gain, fminf(pid->kp, max_gain));
    pid->ki              = fmaxf(min_gain, fminf(pid->ki, max_gain));
    pid->kd              = fmaxf(min_gain, fminf(pid->kd, max_gain));

    // Calculate derivative term with numerical stability
    float derivative = (pid->error - pid->prev_error) / dt;
    // Limit derivative term to prevent numerical instability
    const float max_derivative = 1e6f;
    if (derivative > max_derivative)
        derivative = max_derivative;
    else if (derivative < -max_derivative)
        derivative = -max_derivative;

    float output = pid->kp * pid->error + pid->ki * pid->integral + pid->kd * derivative;

    // Store current error for next update
    pid->prev_error = pid->error;

    return output;
}

/*--------------------------------Adaptive PID---------------------------------*/
/**
 * @brief Initialize an adaptive PID controller with specified gains.
 * @param pid Pointer to the adaptive PID structure.
 * @param kp Proportional gain.
 * @param ki Integral gain.
 * @param kd Derivative gain.
 * @param adapt_gain Gain for adapting PID parameters.
 */
void et_adaptive_pid_init(et_adaptive_pid_t *pid, float kp, float ki, float kd, float adapt_gain)
{
    pid->kp         = kp;
    pid->ki         = ki;
    pid->kd         = kd;
    pid->adapt_gain = adapt_gain;
    pid->set_point  = 0.0f;
    pid->prev_error = 0.0f;
    pid->integral   = 0.0f;
}

/**
 * @brief Update the adaptive PID controller output based on feedback.
 * @param pid Pointer to the adaptive PID structure.
 * @param feedback Current feedback value.
 * @param dt Time difference since the last update.
 * @return The adaptive PID output after applying the control law.
 */
float et_adaptive_pid_update(et_adaptive_pid_t *pid, float feedback, float dt)
{
    // Null pointer check for safety
    if (pid == NULL)
        return 0;

    // Use consistent threshold with other PID functions
    if (dt <= 1e-6f)
        return 0;

    float error    = pid->set_point - feedback;
    pid->integral += error * dt;

    // Adaptive gain adjustment with numerical stability
    float error_diff  = error - pid->prev_error;
    pid->kp          *= (1.0f + pid->adapt_gain * fabs(error_diff));
    pid->ki          *= (1.0f + pid->adapt_gain * fabs(error));

    // Prevent division by zero in derivative calculation
    float derivative_term  = (dt > 1e-6f) ? (error_diff / dt) : 0.0f;
    pid->kd               *= (1.0f + pid->adapt_gain * fabs(derivative_term));

    // Limit gains to reasonable ranges to prevent instability
    const float max_gain = 1000.0f;
    const float min_gain = 0.001f;
    pid->kp              = fmaxf(min_gain, fminf(pid->kp, max_gain));
    pid->ki              = fmaxf(min_gain, fminf(pid->ki, max_gain));
    pid->kd              = fmaxf(min_gain, fminf(pid->kd, max_gain));

    // Calculate derivative with numerical stability
    float derivative = derivative_term;
    // Limit derivative term to prevent numerical instability
    const float max_derivative = 1e6f;
    if (derivative > max_derivative)
        derivative = max_derivative;
    else if (derivative < -max_derivative)
        derivative = -max_derivative;

    float output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;

    pid->prev_error = error;
    return output;
}

/*---------------------------Integral Separation PID---------------------------*/
/**
 * @brief Initialize an integral separation PID controller.
 * @param pid Pointer to the integral separation PID structure.
 * @param kp Proportional gain.
 * @param ki Integral gain.
 * @param kd Derivative gain.
 * @param threshold Threshold for enabling integral action.
 */
void et_integral_separation_pid_init(et_integral_separation_pid_t *pid, float kp, float ki, float kd, float threshold)
{
    pid->kp         = kp;
    pid->ki         = ki;
    pid->kd         = kd;
    pid->set_point  = 0.0f;
    pid->prev_error = 0.0f;
    pid->integral   = 0.0f;
    /* 
     * Ensure the integral threshold is a positive value. 
     * Integral action depends on the MAGNITUDE of error (not direction), 
     * so we use absolute value to handle both positive/negative threshold inputs safely.
     */
    pid->integral_threshold = fabs(threshold);
}

/**
 * @brief Update the integral separation PID controller output based on feedback.
 * @param pid Pointer to the integral separation PID structure.
 * @param feedback Current feedback value.
 * @param dt Time difference since the last update.
 * @return PID output after applying the control law.
 */
float et_integral_separation_pid_update(et_integral_separation_pid_t *pid, float feedback, float dt)
{
    // null pointer check for safety
    if (pid == NULL)
        return 0.0f;

    // use consistent threshold with other pid functions
    if (dt <= 1e-6f)
        return 0.0f;

    float error = pid->set_point - feedback;

    // integral separation with numerical stability
    if (fabs(error) < pid->integral_threshold)
    {
        pid->integral += error * dt;
        // add integral limit to prevent wind-up
        const float max_integral = 1e6f;
        if (pid->integral > max_integral)
            pid->integral = max_integral;
        else if (pid->integral < -max_integral)
            pid->integral = -max_integral;
    }

    // calculate derivative with numerical stability
    float derivative = (error - pid->prev_error) / dt;
    // limit derivative term to prevent numerical instability
    const float max_derivative = 1e6f;
    if (derivative > max_derivative)
        derivative = max_derivative;
    else if (derivative < -max_derivative)
        derivative = -max_derivative;

    float output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;

    pid->prev_error = error;
    return output;
}

void et_integral_separation_pid_reset(et_integral_separation_pid_t *pid)
{
    pid->kp                 = 0.0f;
    pid->ki                 = 0.0f;
    pid->kd                 = 0.0f;
    pid->set_point          = 0.0f;
    pid->prev_error         = 0.0f;
    pid->integral           = 0.0f;
    pid->integral_threshold = 0.0f;
    ;
}

/*---------------------------Feed forward PID---------------------------*/

/**
 * @brief Initialize a feed forward PID controller with specified gains.
 * @param pid Pointer to the feed forward PID structure to initialize.
 * @param kp Proportional gain.
 * @param ki Integral gain.
 * @param kd Derivative gain.
 * @param ff_gain Feedforward gain to adjust the system based on disturbance.
 */
void et_ff_pid_init(et_ff_pid_t *pid, float kp, float ki, float kd, float ff_gain)
{
    pid->kp         = kp;
    pid->ki         = ki;
    pid->kd         = kd;
    pid->ff_gain    = ff_gain;
    pid->set_point  = 0.0f;
    pid->prev_error = 0.0f;
    pid->integral   = 0.0f;
}

/**
 * @brief Update the feed forward PID controller output based on feedback and disturbance.
 * @param pid Pointer to the feed forward PID structure.
 * @param feedback Current feedback value from the system.
 * @param disturbance External disturbance affecting the system (to be compensated with feed forward).
 * @param dt Time difference (in seconds) between the current and previous update.
 * @return The PID output after applying the control law and feed forward compensation.
 */
float et_ff_pid_update(et_ff_pid_t *pid, float feedback, float disturbance, float dt)
{
    // null pointer check for safety
    if (pid == NULL)
        return 0;

    // use consistent threshold with other pid functions
    if (dt <= 1e-6f)
        return 0;

    // Calculate the error between the set point and feedback
    float error = pid->set_point - feedback;
    // Update the integral term with wind-up protection
    pid->integral += error * dt;

    // add integral limit to prevent wind-up
    const float max_integral = 1e6f;
    if (pid->integral > max_integral)
        pid->integral = max_integral;
    else if (pid->integral < -max_integral)
        pid->integral = -max_integral;

    // Calculate the derivative of the error with numerical stability
    float derivative = (error - pid->prev_error) / dt;
    // limit derivative term to prevent numerical instability
    const float max_derivative = 1e6f;
    if (derivative > max_derivative)
        derivative = max_derivative;
    else if (derivative < -max_derivative)
        derivative = -max_derivative;

    // PID output
    float output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;

    // Add the feed forward term to compensate for disturbances
    output += pid->ff_gain * disturbance;

    // Update the previous error for next iteration
    pid->prev_error = error;
    // Return the total output (PID + feed forward)
    return output;
}

/*---------------------------Digital PID---------------------------*/
/**
 * @brief Initialize a digital PID controller with specified gains.
 * @param pid Pointer to the digital PID structure to initialize.
 * @param kp Proportional gain.
 * @param ki Integral gain.
 * @param kd Derivative gain.
 */
void et_digital_pid_init(et_digital_pid_t *pid, float kp, float ki, float kd)
{
    pid->kp         = kp;   // Proportional gain
    pid->ki         = ki;   // Integral gain
    pid->kd         = kd;   // Derivative gain
    pid->set_point  = 0.0f; // Desired target value
    pid->prev_error = 0.0f; // Previous error for derivative calculation
    pid->integral   = 0.0f; // Integral term to accumulate the error
}

/**
 * @brief Update the digital PID controller output based on feedback and time difference.
 * @param pid Pointer to the digital PID structure.
 * @param feedback Current feedback value from the system.
 * @param dt Time difference (in seconds) between the current and previous update.
 * @return The PID output after applying the control law.
 */
float et_digital_pid_update(et_digital_pid_t *pid, float feedback, float dt)
{
    // Null pointer check for safety
    if (pid == NULL)
        return 0;

    // Use consistent threshold with other PID functions
    if (dt <= 1e-6f)
        return 0;

    float error    = pid->set_point - feedback; // Calculate the error between the set point and feedback
    pid->integral += error * dt;                // Update the integral term

    // Calculate the derivative of the error using discrete time difference
    float derivative = (error - pid->prev_error) / dt;
    // Limit derivative term to prevent numerical instability
    const float max_derivative = 1e6f;
    if (derivative > max_derivative)
        derivative = max_derivative;
    else if (derivative < -max_derivative)
        derivative = -max_derivative;

    float output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative; // PID output

    pid->prev_error = error; // Update the previous error for next iteration
    return output;           // Return the output based on PID control law
}

/**
 * @brief Cascade control: outer positional PID and inner feed-forward PID.
 * @param outer_pid Pointer to the outer positional PID structure.
 * @param inner_pid Pointer to the inner feed-forward PID structure.
 * @param outer_feedback Feedback value for the outer loop.
 * @param inner_feedback Feedback value for the inner loop.
 * @param disturbance External disturbance for feed-forward compensation.
 * @param dt Time difference (in seconds) between updates.
 * @return The final output of the cascaded controller.
 */
float et_cascade_pos_ff_pid(etk_pos_pid_t *outer_pid, et_ff_pid_t *inner_pid, float outer_feedback,
                            float inner_feedback, float disturbance, float dt)
{
    // Null pointer checks for safety
    if (outer_pid == NULL || inner_pid == NULL)
        return 0;

    // Outer PID computes the target value for the inner loop
    float inner_set_point = etk_position_pid_update(outer_pid, outer_feedback, dt, 0.7f);

    // Update the set point of the inner PID
    inner_pid->set_point = inner_set_point;

    // Inner FF-PID computes the final output
    return et_ff_pid_update(inner_pid, inner_feedback, disturbance, dt);
}

/**
 * @brief Cascade control: outer adaptive PID and inner integral separation PID.
 * @param outer_pid Pointer to the outer adaptive PID structure.
 * @param inner_pid Pointer to the inner integral separation PID structure.
 * @param outer_feedback Feedback value for the outer loop.
 * @param inner_feedback Feedback value for the inner loop.
 * @param dt Time difference (in seconds) between updates.
 * @return The final output of the cascaded controller.
 */
float et_cascade_adaptive_integral_pid(et_adaptive_pid_t *outer_pid, et_integral_separation_pid_t *inner_pid,
                                       float outer_feedback, float inner_feedback, float dt)
{
    // Null pointer checks for safety
    if (outer_pid == NULL || inner_pid == NULL)
        return 0;

    // Outer adaptive PID computes the target value for the inner loop
    float inner_set_point = et_adaptive_pid_update(outer_pid, outer_feedback, dt);

    // Update the set point of the inner integral separation PID
    inner_pid->set_point = inner_set_point;

    // Inner integral separation PID computes the final output
    return et_integral_separation_pid_update(inner_pid, inner_feedback, dt);
}

/**
 * @brief Cascade control: outer fuzzy PID and inner incremental PID.
 * @param outer_pid Pointer to the outer fuzzy PID structure.
 * @param inner_pid Pointer to the inner incremental PID structure.
 * @param outer_feedback Feedback value for the outer loop.
 * @param inner_feedback Feedback value for the inner loop.
 * @param dt Time difference (in seconds) between updates.
 * @return The final output of the cascaded controller.
 */
float et_cascade_fuzzy_inc_pid(et_fuzzy_pid_t *outer_pid, et_inc_pid_t *inner_pid, float outer_feedback,
                               float inner_feedback, float dt)
{
    // Null pointer checks for safety
    if (outer_pid == NULL || inner_pid == NULL)
        return 0;

    // Outer fuzzy PID computes the target value for the inner loop
    float inner_set_point = fuzzy_pid_update(outer_pid, outer_pid->set_point, outer_feedback, dt);

    // Update the set point of the inner incremental PID
    et_incremental_pid_set_point(inner_pid, inner_set_point);

    // Inner incremental PID computes the final output
    return et_incremental_pid_update(inner_pid, inner_feedback);
}
