/**
* @file ALGO_PID_ATY.c
*
* @param Project ALGO_Algorithm_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2023 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 Familiar 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
*           -Undone
**********************************************************************************
*/

#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 __DEBUG_ALGO_PID_ATY
double pidSetNum = 0, pidCurNum = 0;
/**
 * @brief Calculate pidCurNum
 * @param rDelta     - rDelta
 */
void Actuator(double rDelta)
{
    pidCurNum += rDelta;
}

/**
 * @brief Pid control
 */
void PidControl(void)
{
    PID sPID[2];                                                //PID Control Struct
    double rOut[2];                                             //PID Response (Output)
    double rIn[2];                                              //PID Feedback (Input)
    uint8_t tempPid_i = 0;

    PidInitZero(&sPID[0]);                                      //Initialize Struct
    sPID[0].Proportion = 0.5;                                   //Set PID Coefficients
    sPID[0].Integral = 0.5;
    sPID[0].Derivative = 0;
    sPID[0].SetPoint = pidSetNum;                               //Set PID Setpoint (Goal)

    while(1)                                                    //Better torite in timer interrupt call back function
    {                                                           //Mock Up of PID processing
        if(tempPid_i > 50)
        {
            tempPid_i = 0;
            pidSetNum += 10;
            if(pidSetNum > 100)
                pidSetNum = 0;
        }
        tempPid_i++;

        sPID[0].SetPoint = pidSetNum;

        rIn[0] = pidCurNum;                                     //Reads the input variable function (Read Input )
        rOut[0] = PidCalc(&sPID[0], rIn[0]);                    //PID calculation function (Perform PID Interation)
        Actuator(rOut[0]);                                      //Output variable control function (Effect Needed Changes)

        // printf("In: %f\nOut: %f\nPidCur: %f\n\n", rIn[0], rOut[0], pidCurNum);
        // printf("%f\n", pidCurNum);

        DelayMs(1);
    }
}
#endif /* __DEBUG_ALGO_PID_ATY */


#endif  /* __ALGO_PID_ATY_C */

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