/**
* @file ALGO_PID_ATY.c
*
* @param Project ALGO_Algorithm_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2026 MZ-ATY
*       - This code follows:
*           - MZ-ATY Various Contents Joint Statement -
*               <a href="https://mengze.top/MZ-ATY_VCJS">
*                        https://mengze.top/MZ-ATY_VCJS</a>
*           - CC 4.0 BY-NC-SA -
*               <a href="https://creativecommons.org/licenses/by-nc-sa/4.0/">
*                        https://creativecommons.org/licenses/by-nc-sa/4.0/</a>
*       - Your use will be deemed to have accepted the terms of this statement.
*
* @brief functions of PID for all codes
*
* @version
*       - 1_01_200318 > ATY
*           -# Preliminary version, first Release
*       - 1_02_200318 > ATY
*           -# Add Conditional Compilation
*           -# Add user port
**********************************************************************************
*/

#ifndef __ALGO_PID_ATY_C
#define __ALGO_PID_ATY_C

#include "ALGO_PID_ATY.h"

/******************************* For user *************************************/

/******************************************************************************/

/**
 * @brief Calculater PID
 * @param pidStruct pid struct
 * @param nextPoint next point
 * @return pidStruct
 */
double PidCalc(PID* pidStruct, double nextPoint){
    double dError, Error;
    Error = pidStruct->SetPoint - nextPoint;                    // Deviation
    pidStruct->SumError += Error;                               // Integral
    dError = pidStruct->LastError - pidStruct->PrevError;       // Current differential
    pidStruct->PrevError = pidStruct->LastError;
    pidStruct->LastError = Error;
    return (pidStruct->Proportion * Error                       // Proportional
        + pidStruct->Integral * pidStruct->SumError             // Integral item
        + pidStruct->Derivative * dError);                      // Differential item
}

/**
 * @brief Init pid to zero
 * @param pidStruct pid struct
 */
void PidInitZero(PID* pidStruct){
    pidStruct->SetPoint = 0;
    pidStruct->Proportion = 0;
    pidStruct->Integral = 0;
    pidStruct->Derivative = 0;
    pidStruct->LastError = 0;
    pidStruct->PrevError = 0;
    pidStruct->SumError = 0;
    // memset(pidStruct, 0, sizeof(PID));
}

/**
 * @brief Init pid to zero
 * @param pidStruct pid struct
 */
void PidClearZero(PID* pidStruct){
    // pidStruct->SetPoint = 0;
    // pidStruct->Proportion = 0;
    // pidStruct->Integral = 0;
    // pidStruct->Derivative = 0;
    pidStruct->LastError = 0;
    pidStruct->PrevError = 0;
    pidStruct->SumError = 0;
}


#ifdef ALGO_PID_ATY_Test_ATY
double pidCurNum = 0, pidAdjust = 0;
PID sPID = {-10.0, 1.0, 0.1, 0.01, 0, 0, 0};

/**
 * @brief Pid control, put at cycle
 */
void ALGO_PID_ATY_Test(void){
    // sPID.SetPoint = pidSetNum;
    pidAdjust = PidCalc(&sPID, pidCurNum);
    pidCurNum += pidAdjust * 0.1;   // simulate the delay
    printf_ATY_D("\r\npidAdjust = %f, pidCurNum = %f\r\n", pidAdjust, pidCurNum);
}
#endif /* ALGO_PID_ATY_Test_ATY */


#endif  /* __ALGO_PID_ATY_C */

/******************************** End Of File *********************************/