/*
class PID
{
private:
    float Kp, Ki, Kd;                             // Controller Gains
    float tau;                                    // Dericative low-pass Filter Time Const
    float T;                                      // Sampling Time
    float lim_min, lim_max;                       // Output Limits
    float integ, prev_err, diff, prev_meas, prop; // Controller Memory
    float val;                                    // PID output Value
    float err;                                    // Error (Setpoint - Measurement)
    float lim_min_integ, lim_max_integ;           // Anti Windup Integrator Limits
    float constrain(float, float, float);         // Function to Constrian a Value in Bounds

public:
    void init(float, float, float, float, float);
    float update(float, float);
    void set_gains(float, float, float, float, float);
    void set_bounds(float, float);
    float get_p();
    float get_i();
    float get_d();
    float get_tau();
    float get_err();
};

void PID::init(float Kp, float Ki, float Kd, float tau, float T) // Constructor
{
    this->Kp = Kp;
    this->Ki = Ki;
    this->Kd = Kd;
    this->tau = tau;
    this->T = T;

    // Init to Zero
    integ = 0.0f;
    diff = 0.0f;
    prev_err = 0.0f;
    prev_meas = 0.0f;
    val = 0.0f;
}

float PID::update(float setpoint, float meas) // Calculate PID
{
    // Error Signal
    err = setpoint - meas;

    // Proportional Term
    prop = Kp * err;

    // Integral Term
    integ += 0.5f * Ki * T * (err + prev_err);

    // Anti-wind-up via Dynamic Integrator Clamping
    // Limits Computationa and Application
    if (lim_max > prop)
        lim_max_integ = lim_max - prop;
    else
        lim_max_integ = 0.0f;

    if (lim_min < prop)
        lim_max_integ = lim_min - prop;
    else
        lim_min_integ = 0.0f;

    // Constrain Integrator
    if (integ > lim_max_integ)
        integ = lim_max_integ;

    else if (integ < lim_min_integ)
        integ = lim_min_integ;

    // Derivative (Band - Limited Differentiator)
    //  Derivative on Measurements 
    diff = (2.0f * Kd * (meas - prev_meas) + (2.0f * tau - T) * diff) / (2.0f * tau + T);

    // Calculate Output and Apply Limits
    val = prop + integ + diff;

    // Constrain Value in Given Bounds
    val = constrain(val, lim_min, lim_max);

    // Store error and Measurement for Later Use
    prev_err = err;
    prev_meas = meas;
    return val;
}

void PID::set_bounds(float min, float max)
{
    this->lim_max = max;
    this->lim_min = min;
}

void PID::set_gains(float Kp, float Ki, float Kd, float tau, float T)
{
    this->Kp = Kp;
    this->Ki = Ki;
    this->Kd = Kd;
    this->tau = tau;
    this->T = T;
}

float PID::get_p()
{
    return Kp;
}

float PID::get_i()
{
    return Ki;
}

float PID::get_d()
{
    return Kd;
}

float PID::get_tau()
{
    return tau;
}

float PID::get_err()
{
    return err;
}

float PID::constrain(float val, float low_bound, float up_bound)
{
    if (val < low_bound)
        val = low_bound;
    else if (val > up_bound)
        val = up_bound;

    return val;
}

*/

// https://wokwi.com/projects/390270069346310145

#include <avr/io.h> 
#define F_CPU 16000000 
#define BUAD 19200 
#define BUAD_RATE_CALC ((F_CPU/16/BUAD) - 1) 

void serialInit()
{
  //High and low bits
  UBRR0H = (BUAD_RATE_CALC >> 8); 
  UBRR0L = BUAD_RATE_CALC; 
  //////////////// 
  //transimit and recieve enable
  UCSR0B = (1 << TXEN0)| (1 << TXCIE0) | (1 << RXEN0) | (1 << RXCIE0); 
  UCSR0C = (1 << UCSZ01) | (1 << UCSZ00);  //8 bit data format
}

void serialWrite(char * buf)
{
  while (( UCSR0A & (1<<UDRE0))  == 0){};
  for (int i = 0; i < strlen(buf); i++){ 

      while (( UCSR0A & (1<<UDRE0))  == 0){};
      UDR0 = buf[2]; 
  }
}

#define F_CPU 16000000 
#define BUAD 9600 
#define BUAD_RATE_CALC ((F_CPU/16/BUAD) - 1)  
#include "PIDcontrol.h"

struct controler controler_i;
struct controler controler_u;

int main(void) {
  // put your setup code here, to run once:
  init(controler_i, 0.1, 0 , 0 ,0 ,0 );
  serialWrite(ar);
  while (1);
}