/**
 * @file Bikethrottle.c
 * @author Wang, Zhiyu(wangzy49@midea.com)
 * @brief throttle of ebike
 * @version 0.1
 * @date 2021-09-29
 *
 * @copyright Copyright (c) 2021
 *
 */

/************************************************************************
 Beginning of File, do not put anything above here except notes
 Compiler Directives:
*************************************************************************/
#include "syspar.h"
#include "typedefine.h"
#include "mathtool.h"
#include "bikethrottle.h"
//#include "api.h"
#include "board_config.h"
#include "hwsetup.h"
#include "CodePara.h"
#include "power.h"
#include "bikespeed.h"
#include "alarm.h"
#include "FSM_1st.h"
#include "Cadence.h"
#include "bikebrake.h"
#include "UserGpio_Config.h"
/******************************
 *
 * static Parameter
 *
 ******************************/
static BIKETHROTTLE_COF bikethrottle_stBikeThrottleCof = BIKETHROTTLE_COF_DEFAULT;
static LPF_OUT          scm_stBikeThrottleLpf;
BIKETHROTTLE_OUT        bikethrottle_stBikeThrottleOut = BIKETHROTTLE_OUT_DEFAULT;
BIKECRUISE_COF          bikeCruise=BIKECRUISE_OUT_DEFAULT;
/******************************
 *
 * extern Parameter
 *
 ******************************/
UWORD Bikethrottle_uwDMAReg = 0;
//UBYTE   SetZeroRunMode=0;
/***************************************************************
 Function: bikethrottle_voBikeThrottleCof;
 Description: bikethrottle coef cal
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void bikethrottle_voBikeThrottleCof(void)
{
    ULONG ulLpfTm = 0;

    bikethrottle_stBikeThrottleCof.uwMaxThrottleVolOutputPu = ((ULONG)BIKETHROTTLE_VOLTAGE_MAX_RANGE << 14) / (VBASE * 100); // Q15
    bikethrottle_stBikeThrottleCof.uwMinThrottleVolOutputPu = ((ULONG)BIKETHROTTLE_VOLTAGE_MIN_RANGE << 14) / (VBASE * 100); // Q15
    bikethrottle_stBikeThrottleCof.uwThrottleVoFaultlPu     = ((ULONG)BIKETHROTTLE_VOLTAGE_FAULT_RANGE << 14) / (VBASE * 100);
    bikethrottle_stBikeThrottleCof.uwThrottleVolSen2McuGain = BIKETHROTTLE_VOLTAGE_SEN2MCUGAIN;
    bikethrottle_stBikeThrottleCof.uwThrottleVolLPFFrq = BIKETHROTTLE_LPF_FRQ;
    bikethrottle_stBikeThrottleCof.uwThrottleVolLPFDisFrq = BIKETHROTTLE_LPF_DISCRETEHZ;
    bikethrottle_stBikeThrottleCof.uwThrottleVolReg2Pu = (((SQWORD)33 << 24) / 10) / (1 << ADC_RESOLUTION_BIT) /
                                                         bikethrottle_stBikeThrottleCof.uwThrottleVolSen2McuGain * 100 * 10 /
                                                         VBASE; // Real Voltage to Pu Q24
    // 3.3/4096/harwaregain/VBase;
    bikethrottle_stBikeThrottleCof.uwThrottleOffsetPu = bikethrottle_stBikeThrottleCof.uwMinThrottleVolOutputPu;
//	bikethrottle_stBikeThrottleCof.uwThrottleOffsetPu = iAdc_GetResultPointer(0)[HW_ADC_THRO_CH] * bikethrottle_stBikeThrottleCof.uwThrottleVolReg2Pu >> 10; 
    /* Bike Throttle limit coef */
    ulLpfTm = 1000000 / bikethrottle_stBikeThrottleCof.uwThrottleVolLPFFrq;
    mth_voLPFilterCoef(ulLpfTm, bikethrottle_stBikeThrottleCof.uwThrottleVolLPFDisFrq, &scm_stBikeThrottleLpf.uwKx);

    bikethrottle_stBikeThrottleCof.uwThrottleErrorCnt = BIKETHROTTLE_ERROR_TIME / BIKETHROTTLE_ERROE_TIMEUNIT;
    bikethrottle_stBikeThrottleCof.uwThrottleRecoverCnt = BIKETHROTTLE_RECOVER_TIME / BIKETHROTTLE_ERROE_TIMEUNIT;
}

/***************************************************************
 Function: bikethrottle_voBikeThrottleInit;
 Description: Bike throttle signal initialization
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void bikethrottle_voBikeThrottleInit(void)
{
    bikethrottle_stBikeThrottleOut.uwThrottleVolReg = 0;
    bikethrottle_stBikeThrottleOut.uwThrottleVolPu = 0;
    bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = 0;
    bikethrottle_stBikeThrottleOut.blThrottleErrorFlg = FALSE;
    bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt = 0;
    bikethrottle_stBikeThrottleOut.uwThrottleRecoverCnt = 0;
}

/***************************************************************
 Function: bikethrottle_voBikeThrottleADC;
 Description: bikethrottle get
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void bikethrottle_voBikeThrottleADC(void) // need to match ADC_StartConversion(ADC1);
{
    if (bikethrottle_stBikeThrottleOut.blThrottleErrorFlg == TRUE)
    {
        bikethrottle_stBikeThrottleOut.uwThrottleVolPu = 0;
        bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = 0;
        if(bikethrottle_stBikeThrottleOut.uwThrottleActPer<250)
        {
        bikethrottle_stBikeThrottleOut.uwThrottleRecoverCnt++;
           // if (bikethrottle_stBikeThrottleOut.uwThrottleRecoverCnt == bikethrottle_stBikeThrottleCof.uwThrottleRecoverCnt)
        {
            bikethrottle_stBikeThrottleOut.blThrottleErrorFlg = FALSE;
            bikethrottle_stBikeThrottleOut.uwThrottleRecoverCnt = 0;
        }
    }
    }
    else
    {
        bikethrottle_stBikeThrottleOut.uwThrottleRecoverCnt = 0;
    }
   if(cp_stBikeRunInfoPara.uwBikeGear!=bikethrottle_stBikeThrottleOut.uwThrottleGear)
   {
       if((cp_stBikeRunInfoPara.uwBikeGear==1) &&(bikethrottle_stBikeThrottleOut.uwThrottleGear==0))
       {
           if(bikethrottle_stBikeThrottleOut.uwThrottleVolPu>bikethrottle_stBikeThrottleCof.uwThrottleVoFaultlPu)//25% if(bikethrottle_stBikeThrottleOut.uwThrottleActPer>250)
            bikethrottle_stBikeThrottleOut.blThrottleErrorFlg = TRUE;
       }
       bikethrottle_stBikeThrottleOut.uwThrottleGear=cp_stBikeRunInfoPara.uwBikeGear;

   }
   else if(power_stPowStateOut.blPowerStartupFlg == FALSE)
   {
       if(bikethrottle_stBikeThrottleOut.uwThrottleActPer>250)
         bikethrottle_stBikeThrottleOut.blThrottleErrorFlg = TRUE;
   }

    {
        bikethrottle_stBikeThrottleOut.uwThrottleVolReg =  THROTTLEVOLREG();// DL_ADC12_getMemResult(ADC12_0_INST, DL_ADC12_MEM_IDX_3);//
        bikethrottle_stBikeThrottleOut.uwThrottleVolPu =
            ((SQWORD)bikethrottle_stBikeThrottleOut.uwThrottleVolReg * bikethrottle_stBikeThrottleCof.uwThrottleVolReg2Pu) >> 10; // Q14
        mth_voLPFilter(bikethrottle_stBikeThrottleOut.uwThrottleVolPu, &scm_stBikeThrottleLpf);
        bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = scm_stBikeThrottleLpf.slY.sw.hi;
        //故障判断
        if (bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu < bikethrottle_stBikeThrottleCof.uwMinThrottleVolOutputPu)
        {
            bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = bikethrottle_stBikeThrottleCof.uwMinThrottleVolOutputPu;
            bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt++;
            if (bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt == bikethrottle_stBikeThrottleCof.uwThrottleErrorCnt)
            {
                bikethrottle_stBikeThrottleOut.blThrottleErrorFlg = TRUE;
                bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt = 0;
            }
        }
//        else if (bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu > bikethrottle_stBikeThrottleCof.uwMaxThrottleVolOutputPu)
//        {
//            bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu = bikethrottle_stBikeThrottleCof.uwMaxThrottleVolOutputPu;
//            bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt++;
//            if (bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt == bikethrottle_stBikeThrottleCof.uwThrottleErrorCnt)
//            {
//                bikethrottle_stBikeThrottleOut.blThrottleErrorFlg = TRUE;
//                bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt = 0;
//            }
//        }
        else //无故障
        {
            bikethrottle_stBikeThrottleOut.uwThrottleErrorCnt = 0;


            bikethrottle_stBikeThrottleOut.uwThrottlePercent =
                (bikethrottle_stBikeThrottleOut.uwThrottleVolLPFPu - bikethrottle_stBikeThrottleCof.uwThrottleOffsetPu) * 1000 /
                (bikethrottle_stBikeThrottleCof.uwMaxThrottleVolOutputPu - bikethrottle_stBikeThrottleCof.uwMinThrottleVolOutputPu);
            bikethrottle_stBikeThrottleOut.uwThrottleActPer=bikethrottle_stBikeThrottleOut.uwThrottlePercent;
        }

      //  bikethrottle_stBikeThrottleOut.uwThrottlePercent=  1000;//      bikethrottle_stBikeThrottleOut.uwThrottleVolReg;

#if(THOTTLECRUISE_EN!=0)
        if((cp_stControlPara.uwControlFunEN &0xff)==0xaa)
        {
        if(bikeCruise.CruiseMode!=0)
           {
               bikethrottle_stBikeThrottleOut.uwThrottlePercent= bikeCruise.CruisePercent;
           }
        }
#endif
           #if(EMCDEAL_EN==0)
        if(bikethrottle_stBikeThrottleOut.blThrottleErrorFlg == TRUE)
        {
            bikethrottle_stBikeThrottleOut.uwThrottlePercent=0;
        }
        #endif
//#if(STARTRUNATSPEED!=0)
//        if((bikespeed_stFreGetOut.uwFrequencyPu<RESETTRUNSPEED) &&( bikethrottle_stBikeThrottleOut.uwThrottlePercent<250))
//        {
//            SetZeroRunMode=0;
//        }
//        else if(cp_stBikeRunInfoPara.BikeSpeedKmH>=STARTRUNSPEED)
//        {
//            SetZeroRunMode=1;
//        }
//        if(SetZeroRunMode==0)
//        {
//            bikethrottle_stBikeThrottleOut.uwThrottlePercent=0;
//    }
//
//#endif

    }
}


void Throttlecruise(void)
{
   if(  ( bikethrottle_stBikeThrottleOut.uwThrottleVolPu < (bikeCruise.ThrottlekeepVault+180) )
     && ( bikethrottle_stBikeThrottleOut.uwThrottleVolPu > (bikeCruise.ThrottlekeepVault-180) ))
   {
       if( (BikeBrake_blGetstate() == FALSE)
          &&(alm_blAlmOccrFlg==FALSE)
          &&(cp_stBikeRunInfoPara.uwBikeGear>0)
          &&(cp_stBikeRunInfoPara.uwBikeGear<=5)
          &&(bikethrottle_stBikeThrottleOut.uwThrottlePercent>250)
          &&(switch_flg.SysRun_Flag ==TRUE)
          &&(bikeCruise.CruiseMode==0))
       {
           if(bikeCruise.KeepInCnt<32767)
               bikeCruise.KeepInCnt++;
           if(bikeCruise.KeepInCnt >= CRUISESETINGTIME   ) //8秒指拨不动进入巡航
           {
               bikeCruise.CruiseMode=1;
               bikeCruise.CruisePercent=bikethrottle_stBikeThrottleOut.uwThrottlePercent;
               bikeCruise.ThrottleResetCruiseFlag=1;
           }
       }
       if(bikethrottle_stBikeThrottleOut.uwThrottlePercent<250)
                      bikeCruise.KeepInCnt=0;

   }
   else
   {
       bikeCruise.ThrottlekeepVault= bikethrottle_stBikeThrottleOut.uwThrottleVolPu;
       bikeCruise.KeepInCnt=0;
      // bikeCruise.ThrottleResetCruiseFlag=0;
   }


   //有故障退出，刹车退出，0档退出，有踏频退出，有力矩退出
   if((cadence_stFreGetOut.uwForwardCnt > (ass_stCadAssCoef.uwCadencePulses >> 3))
       ||(alm_blAlmOccrFlg==TRUE)
       ||(BikeBrake_blGetstate() == TRUE)
       ||(cp_stBikeRunInfoPara.uwBikeGear==0)
       ||(bikeCruise.ThrottleResetCruiseFlag  ==3))
   {
       bikeCruise.CruiseMode=0;
       bikeCruise.KeepInCnt=0;
       bikeCruise.ThrottleResetCruiseFlag =0;
   }
   //指拨重新调速退出
    if(bikethrottle_stBikeThrottleOut.uwThrottleVolPu>bikethrottle_stBikeThrottleCof.uwThrottleVoFaultlPu)
   {
      if( bikeCruise.ThrottleResetCruiseFlag==2 )
       bikeCruise.ThrottleResetCruiseFlag=3;
   }
    else
    {
        if((bikeCruise.CruiseMode) &&(bikeCruise.ThrottleResetCruiseFlag==1))
        {
            bikeCruise.ThrottleResetCruiseFlag=2;
        }

    }

}

/*************************************************************************
 End of this File (EOF)!
 Do not put anything after this part!
*************************************************************************/