/**
 * @file temperature_control.c
 * @brief Temperature-control services for the laser TEC loops.
 */

#include "temperature_control.h"

uint16_t temperature_control_compute_pid(const laser_channel_config_t *channel_config,
                                         laser_runtime_t *runtime_state,
                                         uint8_t channel_index,
                                         uint32_t current_tick_1ms,
                                         uint32_t *shared_pid_reference_tick)
{
    int32_t error;
    float proportional_gain;
    float integral_term;
    int32_t output;

    if ((channel_config == NULL) || (runtime_state == NULL) || (shared_pid_reference_tick == NULL))
    {
        return 32768u;
    }

    error = (int32_t)runtime_state->current_temperature_raw - (int32_t)channel_config->target_temperature_raw;
    integral_term = runtime_state->integral_error;

    if ((error < 3000) && (error > -3000))
    {
        integral_term += channel_config->pid_i *
                         (float)error *
                         (float)(current_tick_1ms - *shared_pid_reference_tick) /
                         100.0f;
    }

    proportional_gain = channel_config->pid_p;

    if (integral_term > 32000.0f)
    {
        integral_term = 32000.0f;
    }
    else if (integral_term < -32000.0f)
    {
        integral_term = -32000.0f;
    }

    runtime_state->integral_error = integral_term;

    output = 32768 + (int32_t)(proportional_gain * (float)error) + (int32_t)integral_term;

    if (output < 1000)
    {
        output = 8800;
    }
    else if (output > 56800)
    {
        output = 56800;
    }

    /* Both PID channels use a shared timing reference updated after the
     * second TEC computation. This preserves the original controller timing. */
    if (channel_index == 2u)
    {
        *shared_pid_reference_tick = current_tick_1ms;
    }

    return (uint16_t)output;
}