/*
 * Generated by EML-lang C backend
 * Source module: pid_controller
 * Source file:   /home/monogate/monogate/forge/examples/pid_controller.eml
 * Functions:     1
 * Constants:     5
 * Types:         0
 */

#include "libmonogate.h"
#include <stdint.h>
#include <math.h>
#include <assert.h>

static const double Kp = 1.5;
static const double Ki = 0.4;
static const double Kd = 0.05;
static const double OUT_MIN = (-1.0);
static const double OUT_MAX = 1.0;

/*
 * pid
 * Chain order: 0     Cost class: p0-d4-w0-c0
 * EML depth:   4  Drift risk: LOW
 * Dynamics:    0 osc, 0 decay  (predicted_r=0)
 * FPGA est:   4 MAC, 0 exp, 0 ln, 0 trig -> 8 cy @ 32-bit
 */
double pid(double error, double integral, double derivative) {
    assert(((mg_abs(error) <= 100.0)) && "pid: refinement violated on error: (mg_abs(error) <= 100.0)");
    assert(((mg_abs(integral) <= 100.0)) && "pid: refinement violated on integral: (mg_abs(integral) <= 100.0)");
    assert(((mg_abs(derivative) <= 100.0)) && "pid: refinement violated on derivative: (mg_abs(derivative) <= 100.0)");
    double raw = (((Kp * error) + (Ki * integral)) + (Kd * derivative));
    return mg_clamp(raw, OUT_MIN, OUT_MAX);
}