#include "pid_controller_embedded.h"
#include <string.h>
#include <math.h>

/* ==================== 静态变量 ==================== */
static PID_DataPoint s_data_buffer[256];
static uint16_t s_data_count = 0;
static uint16_t s_data_index = 0;
static float s_steady_start_time = 0;

/* ==================== PID控制器 ==================== */

void PID_Init(PID_Controller *pid, float kp, float ki, float kd)
{
    pid->base_kp = kp;
    pid->base_ki = ki;
    pid->base_kd = kd;
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    
    pid->setpoint = 0.0f;
    pid->last_error = 0.0f;
    pid->integral = 0.0f;
    pid->output = 0.0f;
    
    pid->integral_max = 100.0f;
    pid->integral_min = -100.0f;
    pid->output_max = 100.0f;
    pid->output_min = 0.0f;
    
    pid->current_zone = 1; /* MID */
}

void PID_SetSetpoint(PID_Controller *pid, float setpoint)
{
    pid->setpoint = setpoint;
    pid->integral = 0.0f;
    pid->last_error = 0.0f;
}

void PID_SetParams(PID_Controller *pid, float kp, float ki, float kd)
{
    pid->base_kp = kp;
    pid->base_ki = ki;
    pid->base_kd = kd;
}

void PID_Reset(PID_Controller *pid)
{
    pid->integral = 0.0f;
    pid->last_error = 0.0f;
    pid->output = 0.0f;
}

uint8_t PID_GetTemperatureZone(float temp)
{
    if (temp < TEMP_ZONE_LOW_MAX) {
        return 0; /* LOW */
    } else if (temp < TEMP_ZONE_MID_MAX) {
        return 1; /* MID */
    } else {
        return 2; /* HIGH */
    }
}

void PID_UpdateZoneFactors(PID_Controller *pid, float current_temp)
{
    uint8_t zone = PID_GetTemperatureZone(current_temp);
    float factor;
    
    switch (zone) {
        case 0: factor = TEMP_ZONE_LOW_FACTOR; break;
        case 1: factor = TEMP_ZONE_MID_FACTOR; break;
        case 2: factor = TEMP_ZONE_HIGH_FACTOR; break;
        default: factor = 1.0f; break;
    }
    
    pid->current_zone = zone;
    pid->kp = pid->base_kp * factor;
    pid->ki = pid->base_ki * factor;
    pid->kd = pid->base_kd * factor;
}

float PID_Update(PID_Controller *pid, float current_temp, float dt)
{
    float error, p_term, i_term, d_term, derivative;
    float output;
    
    /* 更新温度区补偿 */
    PID_UpdateZoneFactors(pid, current_temp);
    
    /* 计算误差 */
    error = pid->setpoint - current_temp;
    
    /* 比例项 */
    p_term = pid->kp * error;
    
    /* 积分项 */
    pid->integral += error * dt;
    if (pid->integral > pid->integral_max) {
        pid->integral = pid->integral_max;
    } else if (pid->integral < pid->integral_min) {
        pid->integral = pid->integral_min;
    }
    i_term = pid->ki * pid->integral;
    
    /* 微分项 */
    derivative = (error - pid->last_error) / dt;
    d_term = pid->kd * derivative;
    
    /* 计算输出 */
    output = p_term + i_term + d_term;
    
    /* 输出限幅 */
    if (output > pid->output_max) {
        output = pid->output_max;
    } else if (output < pid->output_min) {
        output = pid->output_min;
    }
    
    pid->last_error = error;
    pid->output = output;
    
    return output;
}

/* ==================== ZCD转换 ==================== */

uint16_t PowerToZCD(float power_percent)
{
    uint16_t zcd;
    
    if (power_percent < 0.0f) {
        power_percent = 0.0f;
    } else if (power_percent > 100.0f) {
        power_percent = 100.0f;
    }
    
    zcd = ZCD_MAX - (uint16_t)((power_percent / 100.0f) * ZCD_RANGE);
    return zcd;
}

float ZCDToPower(uint16_t zcd)
{
    float power;
    
    if (zcd < ZCD_MIN) {
        zcd = ZCD_MIN;
    } else if (zcd > ZCD_MAX) {
        zcd = ZCD_MAX;
    }
    
    power = ((float)(ZCD_MAX - zcd) / ZCD_RANGE) * 100.0f;
    return power;
}

/* ==================== 性能分析器 ==================== */

void PID_Analyzer_Reset(void)
{
    s_data_count = 0;
    s_data_index = 0;
    s_steady_start_time = 0;
}

void PID_Analyzer_AddData(float time, float temp, float setpoint, float power)
{
    s_data_buffer[s_data_index].time = time;
    s_data_buffer[s_data_index].temp = temp;
    s_data_buffer[s_data_index].setpoint = setpoint;
    s_data_buffer[s_data_index].power = power;
    
    s_data_index = (s_data_index + 1) & 0xFF;
    if (s_data_count < 256) {
        s_data_count++;
    }
}

uint8_t PID_CheckSteadyState(float setpoint, float threshold, float duration, float *steady_time)
{
    uint16_t i, start_idx, count;
    float max_error, error;
    float first_time, last_time;
    
    if (s_data_count < 30) {
        *steady_time = 0;
        return 0;
    }
    
    /* 检查最近30个数据点 */
    count = (s_data_count < 30) ? s_data_count : 30;
    start_idx = (s_data_index + 256 - count) & 0xFF;
    
    max_error = 0.0f;
    first_time = s_data_buffer[start_idx].time;
    last_time = s_data_buffer[(s_data_index + 255) & 0xFF].time;
    
    for (i = 0; i < count; i++) {
        uint16_t idx = (start_idx + i) & 0xFF;
        error = s_data_buffer[idx].temp - setpoint;
        if (error < 0) error = -error;
        if (error > max_error) {
            max_error = error;
        }
    }
    
    if (max_error < threshold) {
        if (s_steady_start_time == 0) {
            s_steady_start_time = first_time;
        }
        *steady_time = last_time - s_steady_start_time;
        return (*steady_time >= duration) ? 1 : 0;
    } else {
        s_steady_start_time = 0;
        *steady_time = 0;
        return 0;
    }
}

PID_Performance PID_Analyzer_GetMetrics(void)
{
    PID_Performance perf;
    float setpoint, max_temp, final_temp;
    uint16_t i;
    float last_sign, sign;
    
    memset(&perf, 0, sizeof(perf));
    
    if (s_data_count < 10) {
        return perf;
    }
    
    setpoint = s_data_buffer[0].setpoint;
    max_temp = s_data_buffer[0].temp;
    final_temp = s_data_buffer[(s_data_index + 255) & 0xFF].temp;
    
    /* 计算最大温度 */
    for (i = 0; i < s_data_count; i++) {
        if (s_data_buffer[i].temp > max_temp) {
            max_temp = s_data_buffer[i].temp;
        }
    }
    
    /* 上升时间 (达到90%设定值) */
    for (i = 0; i < s_data_count; i++) {
        if (s_data_buffer[i].temp >= setpoint * 0.9f) {
            perf.rise_time = s_data_buffer[i].time;
            break;
        }
    }
    
    /* 超调量 */
    if (max_temp > setpoint) {
        perf.overshoot = ((max_temp - setpoint) / setpoint) * 100.0f;
    }
    
    /* 调节时间 (进入±2%范围) */
    for (i = s_data_count; i > 0; i--) {
        uint16_t idx = (s_data_index + 256 - (s_data_count - i + 1)) & 0xFF;
        float error = s_data_buffer[idx].temp - setpoint;
        if (error < 0) error = -error;
        if (error > setpoint * 0.02f) {
            perf.settling_time = s_data_buffer[idx].time;
            break;
        }
    }
    
    /* 稳态误差 */
    perf.steady_error = final_temp - setpoint;
    if (perf.steady_error < 0) perf.steady_error = -perf.steady_error;
    
    /* 振荡次数 */
    last_sign = 0;
    for (i = 1; i < s_data_count; i++) {
        float error = s_data_buffer[i].temp - setpoint;
        sign = (error > 0) ? 1.0f : ((error < 0) ? -1.0f : 0);
        if (sign != 0 && last_sign != 0 && sign != last_sign) {
            perf.oscillations++;
        }
        if (sign != 0) {
            last_sign = sign;
        }
    }
    
    perf.max_temp = max_temp;
    perf.final_temp = final_temp;
    
    return perf;
}

/* ==================== 自适应整定器 ==================== */

void PID_Tuner_Init(PID_Tuner *tuner, uint8_t max_cycles)
{
    tuner->cycle = 0;
    tuner->max_cycles = max_cycles;
    tuner->overshoot_threshold = 0.5f;
    tuner->steady_time_required = 3.0f;
}

uint8_t PID_Tuner_AdjustParams(PID_Tuner *tuner, PID_Controller *pid, PID_Performance *perf)
{
    float new_kp, new_ki, new_kd;
    uint8_t adjusted = 0;
    
    new_kp = pid->base_kp;
    new_ki = pid->base_ki;
    new_kd = pid->base_kd;
    
    /* 策略1: 超调过大 */
    if (perf->overshoot > tuner->overshoot_threshold * 2.0f) {
        if (pid->base_kd < 5.0f) {
            new_kd = pid->base_kd * 1.3f;
        } else {
            new_kp = pid->base_kp * 0.9f;
        }
        adjusted = 1;
    }
    else if (perf->overshoot > tuner->overshoot_threshold) {
        new_kd = pid->base_kd * 1.15f;
        new_kp = pid->base_kp * 0.95f;
        adjusted = 1;
    }
    
    /* 策略2: 响应慢 */
    if (perf->overshoot <= tuner->overshoot_threshold && perf->rise_time > 10.0f) {
        new_kp = pid->base_kp * 1.1f;
        adjusted = 1;
    }
    
    /* 策略3: 稳态误差大 */
    if (perf->steady_error > 1.0f) {
        new_ki = pid->base_ki * 1.2f + 0.02f;
        adjusted = 1;
    }
    
    /* 策略4: 振荡 */
    if (perf->oscillations > 2) {
        new_ki = pid->base_ki * 0.8f;
        new_kp = pid->base_kp * 0.95f;
        adjusted = 1;
    }
    
    /* 应用调整 */
    if (new_kp < 0.5f) new_kp = 0.5f;
    if (new_kp > 50.0f) new_kp = 50.0f;
    if (new_ki < 0.0f) new_ki = 0.0f;
    if (new_ki > 5.0f) new_ki = 5.0f;
    if (new_kd < 0.0f) new_kd = 0.0f;
    if (new_kd > 15.0f) new_kd = 15.0f;
    
    pid->base_kp = new_kp;
    pid->base_ki = new_ki;
    pid->base_kd = new_kd;
    
    return adjusted;
}

uint8_t PID_Tuner_IsOptimal(PID_Tuner *tuner, PID_Performance *perf)
{
    return (perf->overshoot <= tuner->overshoot_threshold && 
            perf->steady_error < 0.5f &&
            perf->oscillations <= 1) ? 1 : 0;
}