/*
 * pid.h
 *
 *  Created on: 19 Dec 2025
 *      Author: janik
 */

#ifndef INC_PID_H_
#define INC_PID_H_

// pid_motor_wrap_i32.h
typedef struct
{
    // Gains
    int32_t kp;
    int32_t ki;
    int32_t kd;

    // State
    int32_t integrator;
    int32_t prev_error;

    // Limits
    int32_t integrator_min;
    int32_t integrator_max;
    int32_t out_max;         // max |u|, e.g. 2400

    // Slew limit
    int32_t max_output_step; // max |u(k) - u(k-1)| per control step
    int32_t last_output;     // u(k-1)
} pid_i32_t;

typedef struct
{
    uint16_t pwm;  // 0..out_max
    uint8_t  dir;  // 0 or 1
} pid_motor_cmd_t;

static inline int32_t clamp_i32(int32_t x, int32_t lo, int32_t hi)
{
    if (x < lo) return lo;
    if (x > hi) return hi;
    return x;
}

static inline void pid_init(pid_i32_t *pid,
                            int32_t kp,
                            int32_t ki,
                            int32_t kd,
                            int32_t integrator_min,
                            int32_t integrator_max,
                            int32_t out_max,
                            int32_t max_output_step)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;

    pid->integrator = 0;
    pid->prev_error = 0;

    pid->integrator_min = integrator_min;
    pid->integrator_max = integrator_max;
    pid->out_max        = out_max;

    pid->max_output_step = max_output_step;
    pid->last_output     = 0;
}

static inline void pid_reset(pid_i32_t *pid)
{
    pid->integrator = 0;
    pid->prev_error = 0;
    pid->last_output = 0;
}

static inline pid_motor_cmd_t pid_update_motor(pid_i32_t *pid,
                                               int32_t setpoint,
                                               int32_t position)
{
    pid_motor_cmd_t cmd;

    int32_t error = setpoint - position;

    // Deadband: if within ±3 counts, hold position and reset integrator
    if (error >= -3 && error <= 3) {
        pid->integrator = 0;
        pid->prev_error = 0;
        pid->last_output = 0;
        cmd.pwm = 0;
        cmd.dir = 1;
        return cmd;
    }

    int32_t p = pid->kp * error;

    int32_t i = pid->integrator + pid->ki * error;
    i = clamp_i32(i, pid->integrator_min, pid->integrator_max);
    pid->integrator = i;

    int32_t d_error = error - pid->prev_error;
    int32_t d = pid->kd * d_error;
    pid->prev_error = error;

    // Raw control effort
    int32_t u_raw = p + i + d;

    // Clamp to [-out_max, +out_max]
    if (u_raw >  pid->out_max) u_raw =  pid->out_max;
    if (u_raw < -pid->out_max) u_raw = -pid->out_max;

    // ---------- Slew limit: avoid instant full reverse ----------
    int32_t u_prev = pid->last_output;
    int32_t u = u_raw;

    int32_t max_step = pid->max_output_step;
    if (max_step > 0) {
        int32_t du = u_raw - u_prev;

        if (du > max_step) {
            u = u_prev + max_step;
        } else if (du < -max_step) {
            u = u_prev - max_step;
        } else {
            u = u_raw;
        }
    }

    // Save for next time
    pid->last_output = u;

    // Map signed u to dir + pwm
    if (u >= 0) {
        cmd.dir = 1;
        cmd.pwm = (uint16_t)u;
    } else {
        cmd.dir = 0;
        cmd.pwm = (uint16_t)(-u);
    }

    if (cmd.pwm > pid->out_max)
        cmd.pwm = (uint16_t)pid->out_max;

    return cmd;
}
#endif /* INC_PID_H_ */