/* 
 * File:   PID.h
 * Author: Christian
 *
 * Created on 27 de mayo de 2019, 14:43
 */

extern volatile double GYR_angle;

#ifndef PID_H
#define	PID_H

//#define PID_MODEL_SERIES

#define errAv_MAX 10
#define errAvOT_MAX 10
#define BETA_Angle 1.0
#if (T4_Period == PR_2500)
    #define ITS 100
#else
    #define ITS 10 
#endif

typedef enum PID_TUNING_STATE_ENUM{
    Static_S, Recover_S, Steering_S, SStraight_S, STurn_S
}PID_TUNING_STATE;
typedef struct PID_TUNING_BOJ_STRUCT{
    double p;
    double i;
    double d;
}PID_TUNING_OBJ;


/**************
 *  Sensores 
 **************/
extern double bias;
extern double Curr_GYRO;
extern double ACC_angle;
extern double accData[3];
extern const double _180PI;


/**************
 * Balance PID 
 **************/
extern double B_SP;  // (Set Point) Valor en el que se alcanza el equilibrio
extern double B_PV;  // (Process Variable) Valor actual (de la inclinacion)
extern double B_Pout;  // (Process Out) Potencia transmitida a las ruedas (en 1000/1000)

extern double B_pGain;
extern double B_iGain;//2.6;
extern double B_dGain;//53.0;

extern double B_err;
extern double B_errOT; // (Error Over Time) Media del error acumulado (Aun o se si es un buen metodo)

extern double B_dPV; // Derived error

extern volatile bool BL_PID_ENABLED;

extern PID_TUNING_STATE B_TuningState;


/**************
 *  Speed PID 
 **************/
extern double S_SP;  
extern double S_PV;  
extern double S_Pout;  

extern double S_pGain;
extern double S_iGain;
extern double S_dGain;

extern double S_err;
extern double S_errOT; // (Error Over Time) Media del error acumulado (Aun o se si es un buen metodo)
extern double twoCmWithGain;

extern double S_dPV; // Derived error

extern volatile bool SP_PID_ENABLED;
extern volatile bool SPID_pout_enabled;
extern volatile bool SPID_AW_is_ON;
extern volatile char sampling_wheel; 


/**************
 * PID Tuner 
 **************/
#define B_pG_Increment 1
#define B_iG_Increment 10//0.02
#define B_dG_Increment 0.05

#define S_pG_Increment 0.005
#define S_iG_Increment 0.05
#define S_dG_Increment 0.00005
extern bool PID_TUNING_IS_ON;

void Inic_PID(void);


void Sensor_fusion(void);
    bool get_TiltDirection(void);
    bool get_BetweenStaticMargin(void);
    bool get_BetweenRecoveryMargin(void);
    bool get_BetweenBlindSpot(void);
    bool get_StaticVelocity(void);

/*
 * PID de equilibrio.
 * 
 * Controla directamente el equilibrio. El valor deseado (setpoint) es la suma 
 * del punto central de equilibrio y un desfase asignado por el control de valocidad.
 */
void Balance_PID(void);
    void toggle_BalancePID(void);
    void enable_BalancePID(void);
    void disable_BalancePID(void);
    void change_pGain(double v);
    void change_iGain(double v);
    void change_dGain(double v);
    void change_toStatic(void);
    void change_toRecover(void);
    void change_toSteering(void);
    void save_Static(void);
    void save_Recover(void);
    void save_Advance(void);
    void set_Differential(int);

/*
 * PID de velocidad. (control en casacada)
 * 
 * Este es un PID de baja ganancia que controla la velocidad alterando
 * paulatinamente el punto de consigna (set point) de PID que controla
 * el equilibrio.
 */
void Speed_PID(double);
    void set_SpeedGoal(double speedGoal);
    void toggle_SpeedPID(void);
    void enable_SpeedPID(void);
    void disable_SpeedPID(void);
    void change_S_toStraight(void);
    void change_S_toTurn(void);
    void toggle_SPID_AntiWindup(void);
    void set_sampling_wheel(char sw);
    
typedef struct PID_TUNER_OBJ_STRUCT{
    double peak;
    double peakdif;
    uint32_t ATime;
}PID_TUNER_OBJ;
typedef enum PID_Tuner_states_enum{
            Init,
            Calibrate_pG,
            Calibrate_iG_0,Calibrate_iG_1,Calibrate_iG_2,
            Calibrate_dG_0,Calibrate_dG_1,
            TuningFinished_0,TuningFinished_1,TuningFinished_2,TuningFinished_3,TuningFinished_4,TuningFinished_5,TuningFinished_6,TuningFinished_7,
}PID_Tuner_states;
extern double *FUNCTION;
extern bool direction;
extern bool fliped;
extern bool keepRunningTuner;
extern PID_Tuner_states PIDT_StateMachine;
#define PIDTFreq (250)
extern double *tuningGain;

void PID_tuner(bool restart);
void PID_B_tuner(bool restart);
void PID_S_tuner(bool restart);
    extern double Osc_Peak_mean;
    extern double Osc_Peakdif_mean;
    extern double Osc_ATime_mean;
    extern PID_TUNER_OBJ* extremeWindowHead;
    double PIDT_Oscilometer(void);
    double PIDT_Oscilometer2(void);
    void PIDT_resetOscilometer();

#endif	/* PID_H */