/**
 * @copyright 2015 Indie Semiconductor.
 *
 * This file is proprietary to Indie Semiconductor.
 * All rights reserved. Reproduction or distribution, in whole
 * or in part, is forbidden except by express written permission
 * of Indie Semiconductor.
 *
 * @file safetyMonitorTask.c
 * @Author: Jack.Pan
 * @E-mail:jack.pan@indiemicro.com
 * @Date: 2020/09/10
 */

#include <safetyMonitorTask.h>
#include <motorControlTask.h>
#include <measureTask.h>

#define OVER_TEMPERATURE_HIGH           (130)   /*celsius degree */
#define OVER_TEMPERATURE_RELEASE        (OVER_TEMPERATURE_HIGH - 10U)   /*celsius degree */


#define MotorLocked 230u        //mA 

static void UnderVoltageProtection_ISR(void);
static void OverVoltageProtection_ISR(void);
static void safetyHandle(void);
static void safetyInit(void);

//static int16_t vCurrentMotor; 
uint16_t *p;
static int16_t vAmp[24];
static uint32_t sum=0;
static uint16_t Motorcurrent=0;
static uint32_t VbatMotorcurrent=0;
static uint16_t Lockcnt=0;

static TaskState_t safeMonitorState = TASK_STATE_INIT;
static BatteryState_t battState = BATT_STATE_NORMAL;
static BatteryState_t battLastState = BATT_STATE_NORMAL;
static ChipTemperatureState_t chipTemperatureState = CHIP_TEMPERATURE_STATE_NORMAL;
static ChipTemperatureState_t chipTemperatureLastState = CHIP_TEMPERATURE_STATE_NORMAL;

static void UnderVoltageProtection_ISR(void);
static void OverVoltageProtection_ISR(void);
static void safetyHandle(void);
static void safetyInit(void);
static void safeMonitorTimerExpired(SoftTimer_t *timer);
static Motor_and_ChipPar_t motorandchipstate;

static SoftTimer_t safeMonitorTimer = {
    .mode     = TIMER_PERIODIC_MODE,
    .interval = 200U,
    .handler  = safeMonitorTimerExpired
};


static void safeMonitorTimerExpired(SoftTimer_t *timer)
{
    TM_PostTask(TASK_ID_SAFETY_MONITOR);
}

void SAFM_TaskHandler(void)
{
    switch(safeMonitorState){
    case TASK_STATE_INIT:
      safetyInit();
//      SoftTimer_Start(&safeMonitorTimer);
      safeMonitorState = TASK_STATE_ACTIVE;
      break;
    case TASK_STATE_ACTIVE:
      safetyHandle();
      break;
    default:
      break;
    }
}

static void safetyHandle(void)
{
    /*  Battery Voltage Protection Handling     */
    battState = (BatteryState_t)HW_PROT_GetBattVoltState();
    if ((chipTemperatureLastState != chipTemperatureState) || (battLastState != battState)){
        chipTemperatureLastState = chipTemperatureState;
        battLastState = battState;
        if (battState == BATT_STATE_LOW){
          StopMotor();          
        }else{
            if (chipTemperatureState == CHIP_TEMPERATURE_STATE_HIGH){
              StopMotor();
            }else{
            }
            /* Restart OV UV ISR   */
            PMUA_SFRS->PMUIRQ.CLEAR.OV_CLR  = 1U;
            PMUA_SFRS->PMUIRQ.CLEAR.UV_CLR  = 1U;
            NVIC_ClearPendingIRQ(UV_OV_IRQn);
            NVIC_EnableIRQ(UV_OV_IRQn);
        }
    }
    if(Motor_GetMotorRunStage() == (uint8_t)Constantstage){
      
//      vCurrentMotor = MEAGet_vAmp();
      p = MEAGet_vAmpbuff();
      sum = 0;
      for(uint8_t i=0;i<24;i++){
        vAmp[i]=*p;
        if(vAmp[i]<0){
          vAmp[i] = 0-vAmp[i];
        }
        sum += vAmp[i];
        p++;              
      }
      Motorcurrent = sum*4/24;
//      DEBUG_OUT("Current\t%3d\n",Motorcurrent); 
      VbatMotorcurrent = MEA_GetVbat();
      VbatMotorcurrent = (VbatMotorcurrent*176-120460)/10000;//
      if(Motorcurrent > VbatMotorcurrent){
        Lockcnt++;
        if(Lockcnt>2){
//           StopMotor();      
//           motorandchipstate.state.Motor_overcur = (uint8_t)MotorOC;
        }
       }else{
         Lockcnt = 0;
        motorandchipstate.state.Motor_overcur = (uint8_t)MotorNoOC;
     }     
   }    
}


static void UnderVoltageProtection_ISR(void)
{
    TM_PostTask(TASK_ID_SAFETY_MONITOR);
}

static void OverVoltageProtection_ISR(void)
{
    TM_PostTask(TASK_ID_SAFETY_MONITOR);
}


static void safetyInit(void)
{
    HW_PROT_SetUnderVoltage(UV_VOLT_5_1V,UNDER_VOLT_HYS_830mV, VOLT_DEBOUNCE_TIME_10ms, VOLT_DEBOUNCE_TIME_10ms);
    HW_PROT_RegisterUnderVoltageIRQ(UnderVoltageProtection_ISR);
    
    HW_PROT_SetOverVoltage(OV_VOLT_20_0V, OV_VOLT_HYS_1440mV,   VOLT_DEBOUNCE_TIME_10ms, VOLT_DEBOUNCE_TIME_10ms);
    HW_PROT_RegisterOverVoltageIRQ(OverVoltageProtection_ISR);
    
}


BatteryState_t SAFM_GetBatteryState(void)
{
    return battState;
}

ChipTemperatureState_t SAFM_GetChipTemperatureState(void)
{
    return chipTemperatureState;
}
uint16_t SAFM_GetMotorcurrent(void)
{
  return Motorcurrent;
}
/*Call this function when you need to use the status value*/
uint8_t SAFM_TransferState(void)
{
  motorandchipstate.state.VBAT_sta = (uint8_t)HW_PROT_GetBattVoltState();
  motorandchipstate.state.tchipstate = (uint8_t)HW_PROT_ChipIsOverHeat();
  return motorandchipstate.state.byte[0];
}

void ClearMotorandchipparstate(void){
  motorandchipstate.state.byte[0] = 0;
}