#!/bin/bash

rm -f a.out

gcc -x c - <<EOF
#include <stdio.h>
#include <stdint.h>

#define return \
    printf("m=%3.1f x=%2i, p=%3i, i=%7.3f", \
      magic, measured, error, *integral); \
    return

float magic;
float kP = 0, kI = 0.1, kD = 0;
float PID_INTERVAL = 1;
int16_t MOTOR_MAX_SPEED = +100;
int16_t MOTOR_MIN_SPEED = -100;
int16_t pid_compute(int measured, int setpoint, uint16_t *prev_error, float *integral)
{
    int16_t error = setpoint - measured;
    int16_t output;
    float derivate;

    *integral = *integral*magic + (error * (float)PID_INTERVAL);
    derivate = (error - *prev_error) / (float)PID_INTERVAL;
    output = (kP * error) + (kI * (*integral)) + (kD * derivate);
    *prev_error = error;

    if(output > MOTOR_MAX_SPEED) output = MOTOR_MAX_SPEED;
    if(output < MOTOR_MIN_SPEED) output = MOTOR_MIN_SPEED;

    return output;
}
#undef return
int main() {
    uint16_t i;
    float integral1, integral2;
    uint16_t x1, e1, x2, e2;

    integral1 = x1 = e1 = 0;
    integral2 = x2 = e2 = 0;
    for(i=0;i<100;i++) {
        magic = 1.0; x1 += pid_compute(x1, 10, &e1, &integral1);
        printf(" <-> ");
        magic = 0.9; x2 += pid_compute(x2, 10, &e2, &integral2);
        printf("\n");
    }
    return 0;
}
EOF

./a.out