#!/bin/bash

gcc -x c - <<EOF
#include <stdio.h>
#include <stdint.h>
#define return \
    printf("x=%i, p=%i, i=%f, d=%f\n", measured, error, *integral, derivate); \
    return

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

    *integral = *integral + (error * kI);
    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 integral = 0;
    uint16_t x = 0, e = 0;
    for(i=0;i<50;i++) x += pid_compute(x, 10, &e, &integral);
    for(i=0;i<50;i++) x += pid_compute(x,  0, &e, &integral);
    return 0;
}
EOF

./a.out