#pragma once
#include "stdint.h"

enum PID_MODE
{
  PID_POSITION = 0,
  PID_DELTA
};

typedef struct pid_type
{
  uint8_t mode;
  // PID parameters
  float Kp;
  float Ki;
  float Kd;

  float max_out;  // max output
  float max_iout; // max integral output

  float set;
  float fdb;

  float out;
  float Pout;
  float Iout;
  float Dout;
  float Dbuf[3];  // diff buffer 0 save now 1 save last 2 save last last
  float error[3]; // 

} pid_type_t;
extern void pid_init(pid_type_t *pid, uint8_t mode, float Kp, float Ki, float Kd, float max_out, float max_iout);
extern float pid_calc(pid_type_t *pid, float ref, float set);
extern void pid_clear(pid_type_t *pid);