/*
 * cc_controller.c
 *
 *  Created on: Jun 11, 2025
 *      Author: janik
 */


#include "cc_controller.h"

void CC_Init(CCController *cc, float gain, float min_step, float max_step,
             uint16_t out_min, uint16_t out_max, float initial_output)
{
    cc->gain = gain;
    cc->min_step = min_step;
    cc->max_step = max_step;
    cc->out_min = out_min;
    cc->out_max = out_max;
    cc->output_f = initial_output;
}

uint16_t CC_Update(CCController *cc, float target, float measurement)
{
    float error = target - measurement;
    float step = error * cc->gain;

    if (step < cc->min_step) step = cc->min_step;
    if (step > cc->max_step) step = cc->max_step;

    cc->output_f += step;


    // Clamp float output
    if (cc->output_f > (float)cc->out_max) cc->output_f = (float)cc->out_max;
    if (cc->output_f < (float)cc->out_min) cc->output_f = (float)cc->out_min;

    return (uint16_t)roundf(cc->output_f);
}