/* control.c */

#include <util/atomic.h>

#include "common.h"
#include "timer.h"
#include "config.h"
#include "therm.h"
#include "beep.h"

static temp_t last_temp;
static volatile bool preheat;
static bool control_running;
static uint32_t control_start_time;
static int16_t integral;
static int16_t control_output;

temp_t temperature_at_time(uint32_t time);

int16_t __attribute__ ((noinline)) clip16(int16_t val, int16_t min, int16_t max)
{
    if (val < min)
	val = min;
    if (val > max)
	val = max;
    return val;
}

uint32_t control_now(void)
{
    uint32_t this_seconds;

    ATOMIC_BLOCK(ATOMIC_FORCEON) {
	this_seconds = seconds;
    }

    if (preheat)
	control_start_time = this_seconds;
    return this_seconds - control_start_time;
}

void control_start(void)
{
    preheat = TRUE;
    last_temp = therm_temp();
    integral = 0;
    control_running = TRUE;
    control_output = 0;
}

#define TIME_UNIT 1
#define INTEGRAL_MAX (CONTROL_MAX * 2)
#define INTEGRAL_MIN (-(INTEGRAL_MAX))

#define CONTROL_MAX (10 * 128)
#define CONTROL_MIN (0)

/* P = 1.00 means 1 degree C leads to full-scale output
 * This is represented by config.p == 128
 * The calculations done here are in units of 0.1 degrees
 */

#define ABS(x) ((x) < 0 ? -(x) : (x))
#define INT_MAX 32767
#define INT_MIN (-32768)

#if 0
int16_t multiply_clip(int16_t a, int16_t b)
{
    if ((b == 0) || (ABS(a) > INT_MAX / ABS(b)))
	return ((a < 0) ^ (b < 0)) ? INT_MIN : INT_MAX;
    return a * b;
}
#endif

void control_poll(void)
{
    temp_t temp = therm_temp();
    temp_t profile_temp;
    int16_t error;
    int16_t temp_diff = temp - last_temp;
    int16_t maxgain;
    int16_t p, i, d;

    last_temp = temp;

    if (!control_running)
	return;

    profile_temp = temperature_at_time(control_now());
    if (profile_temp == INVALID_TEMPERATURE) {
	control_running = FALSE;
	output0 = 0;
	beep_off();
	return;
    }

    error = (int16_t)(profile_temp - temp);

    if (error <= 0) {
	beep_on();
	preheat = FALSE;
    } else {
	beep_off();
    }

    maxgain = (error == 0) ? INT_MAX : ((2*CONTROL_MAX) / error);
    maxgain = ABS(maxgain);

    p = clip16(config.p, -maxgain, maxgain);
    i = clip16(config.i, -maxgain, maxgain);
    d = clip16(config.d, -maxgain, maxgain);

    integral += i * error * TIME_UNIT;
    integral = clip16(integral, INTEGRAL_MIN, INTEGRAL_MAX);

    control_output += p * error + integral + d * temp_diff / TIME_UNIT;
    control_output = clip16(control_output, CONTROL_MIN, CONTROL_MAX);

    output0 = (TIMER_MAX/5) * control_output / (CONTROL_MAX/5);
}