/**
 * @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 measureTask.c
 * @Author: Jack.Pan
 * @E-mail:jack.pan@indiemicro.com
 * @Date: 2020/09/10
 */
#include <crc32.h>
#include <measureTask.h>
#include <motorControlTask.h>
#include <gtimer_device.h>

#define AVERAGE_MEASURE_POS     (4U)
#define AVERAGE_MEASURE_GAIN    (1U << AVERAGE_MEASURE_POS)

#define MEASURE_GAIN_POS         12U
#define MEASURE_GAIN             4096 /*(1UL << MEASURE_GAIN_POS)*/

#define MEASURE_TEMP_GAIN_POS    14U
#define MEASURE_TEMP_GAIN        7208 /*16384*0.43994*/


#define RAW_PN_VOLT_SAMPLE_NUM_POS    4U
#define RAW_PN_VOLT_SAMPLE_NUM        (1U << RAW_PN_VOLT_SAMPLE_NUM_POS)

#if RAW_PN_VOLT_SAMPLE_NUM_POS <=0U
    #error RAW_PN_VOLT_SAMPLE_NUM_POS >= 1U
#endif

typedef struct{
  int32_t coefficient;
  int32_t offset;
}coeffParam_t;

typedef struct{
  uint32_t currCode;
  uint16_t vampbuff[255];//vampbuff[24][10];行：一个周期的步数 列：（步与步之间的时间 ）÷（PWM周期）
  uint16_t count;
  coeffParam_t coeff;
  int16_t target;
}AdcDatabuff_t;


typedef struct{
  AdcDatabuff_t         vBatt;
  AdcDatabuff_t         vChipTemp;
  AdcDatabuff_t         vAmp;
  AdcDatabuff_t         vPB3;
  AdcDatabuff_t         vPC4;
  coeffParam_t          tChip;
  int16_t              chipTemperature;
}AdcResult_t;


typedef struct{
  uint8_t CoilAOpencnt;
  uint8_t CoilAShortcnt;
  uint8_t CoilBOpencnt;
  uint8_t CoilBShortcnt; 
  uint8_t MotorOCcnt;
}ErrorCnt_t;


static TaskState_t adcTaskState = TASK_STATE_INIT;
static AdcResult_t adcResult;

static AdcGeneralTriggerSRC_t  measSyncMode = ADC_TRIG_SRC_SOFT_INPUT;
static ADCMeasureParam_t  measItem;
static volatile uint32_t measStart = 0U;
static volatile uint32_t updateSystemInfoStart = 0U;
static volatile uint32_t updatarow = 0U;
void add_data_to_vampbuff(uint32_t *original, uint16_t newData, uint16_t *bufferIndex,uint16_t *buff);
void add_data_to_buff(uint32_t *original, uint16_t newData, uint16_t *bufferIndex,uint16_t *buff);
void measureDoneISR(ADCMeasureParam_t item, uint16_t *const result);
void measureParamInit(void);

void updateTimerExpired(SoftTimer_t *timer);

static volatile uint32_t adcConvertDone = 0U;
static volatile uint16_t measGeneralAdcCode[4];
static uint16_t vAmpbuff[24];
static uint8_t  SamplesNum = 0U; 
static uint8_t  updateSystemInfocnt = 0U;
void add_data_to_vampbuff(uint32_t *original, uint16_t newData, uint16_t *bufferIndex,uint16_t *buff)
{
    *original = newData;       
    uint16_t  row= 0U; 
    
    if(measItem.item == ADC_MEASURE_ITEM_VAMP){
      
        SamplesNum = Motor_GetOnestep_csa_adcnt();//（步与步之间的时间 ）÷（PWM周期）
        row = Motor_GetMotorStep();
      
        if (*original == INVALID_PARAM){
            for (uint8_t i = 0U; i < SamplesNum; i++){
                buff[(row*SamplesNum)+i] = newData;
            }
        }else{          
            buff[(*bufferIndex) + (row*SamplesNum)] = newData;
        }
        (*bufferIndex) ++;
        if ((*bufferIndex) >= SamplesNum){
             (*bufferIndex) = 0U;
             updatarow = row;
             updateSystemInfoStart = 1U;
        }
    }
}


void measureDoneISR(ADCMeasureParam_t param, uint16_t *const result)
{
   measGeneralAdcCode[0] = (result[0]>=0x800U)? 0U: result[0]; 
   measGeneralAdcCode[1] = (result[1]>=0x800U)? 0U: result[1];
   measGeneralAdcCode[2] = (result[2]>=0x800U)? 0U: result[2];
   measGeneralAdcCode[3] = (result[3]>=0x800U)? 0U: result[3];
   measItem = ADC_GetadcMeasParamm();
   
   adcConvertDone = 1U;
   measStart = 1U;
   TM_PostTask(TASK_ID_ADC_MEASURE);
}

void measureParamStart(void)
{
    switch(measItem.item){
    case ADC_MEASURE_ITEM_VBAT_VTEMP:
//      add_data_to_buff(&adcResult.vBatt.currCode,     measGeneralAdcCode[1], &adcResult.vBatt.count,adcResult.vBatt.buff);
//      add_data_to_buff(&adcResult.vChipTemp.currCode, measGeneralAdcCode[2], &adcResult.vChipTemp.count,adcResult.vChipTemp.buff);
      break;
    case ADC_MEASURE_ITEM_VAMP:
      add_data_to_vampbuff(&adcResult.vAmp.currCode,  measGeneralAdcCode[0], &adcResult.vAmp.count,adcResult.vAmp.vampbuff);
      add_data_to_vampbuff(&adcResult.vBatt.currCode,     measGeneralAdcCode[1], &adcResult.vBatt.count,adcResult.vBatt.vampbuff);
      add_data_to_vampbuff(&adcResult.vPB3.currCode, measGeneralAdcCode[2], &adcResult.vPB3.count,adcResult.vPB3.vampbuff);
      add_data_to_vampbuff(&adcResult.vPC4.currCode,     measGeneralAdcCode[3], &adcResult.vPC4.count,adcResult.vPC4.vampbuff);

      break;
    default:
      break;
    }
}

int16_t get_volt(AdcDatabuff_t *const param)
{
    int16_t volt = (int16_t)INVALID_VOLT;
    int32_t sVolt;
    uint32_t uVolt;
    uint32_t sum = 0;

    if (param->currCode != INVALID_PARAM){
      if(measItem.item == ADC_MEASURE_ITEM_VBAT_VTEMP){ 
      
          
      }else if(measItem.item == ADC_MEASURE_ITEM_VAMP){ //求一步以内的母线电流、VBAT、PB3、PC4
          sum = 0; 
          SamplesNum = Motor_GetOnestep_csa_adcnt();//（步与步之间的时间 ）÷（PWM周期）
          for (uint8_t i = 0U; i < SamplesNum; i++){
            sum += param->vampbuff[updatarow*SamplesNum + i];            
          }
          sVolt = param->coeff.coefficient * (int32_t)sum/SamplesNum + param->coeff.offset; 
          uVolt = (uint32_t)sVolt;
          uVolt = uVolt/4096;
          volt = (int16_t)uVolt; 
      }     
    }
    return volt;
}

int16_t get_chip_temperature(int16_t vTemp)
{
    int32_t  stemp;
    uint32_t uTemp;
    uint8_t  negative = 0U;
    stemp = adcResult.tChip.coefficient*vTemp + adcResult.tChip.offset;
    if (stemp < 0){
        stemp = -stemp;
        negative = 1U;
    }
    uTemp = (uint32_t)stemp;
    uTemp = uTemp >> MEASURE_TEMP_GAIN_POS;
    stemp = (int32_t)uTemp;
    if (negative != 0U){
        stemp = -stemp;
    }
    return (int16_t)stemp;
}

void updateSystemInfo(void)
{
      /* calculate chip temperature */
    adcResult.vChipTemp.target  = get_volt(&adcResult.vChipTemp);
    adcResult.chipTemperature   = get_chip_temperature(adcResult.vChipTemp.target);
    adcResult.vBatt.target      = get_volt(&adcResult.vBatt); 
    adcResult.vPB3.target      = get_volt(&adcResult.vPB3); 
    adcResult.vPC4.target      = get_volt(&adcResult.vPC4); 
    adcResult.vAmp.target      = get_volt(&adcResult.vAmp); 
    vAmpbuff[Motor_GetMotorStep()] = adcResult.vAmp.target;

//    TM_PostTask(TASK_ID_SAFETY_MONITOR);
    updateSystemInfocnt++;
    if(updateSystemInfocnt > 23U){
      updateSystemInfocnt = 0;  
      TM_PostTask(TASK_ID_SAFETY_MONITOR);
    }    
}
void MES_TaskHandler(void)
{
    switch(adcTaskState){
    case TASK_STATE_INIT:
      measureParamInit();
      ADC_GeneralInit();/*  init ADC */
      ADC_RegisterIRQ(measureDoneISR);
      adcTaskState = TASK_STATE_ACTIVE;
      break;
    case TASK_STATE_ACTIVE:
      /*  Start parameters measuring */
      
      if (updateSystemInfoStart == 1U){
          updateSystemInfo();
          updateSystemInfoStart = 0U;
      }
      
      if (measStart != 0U){
          measStart = 0U;
          measureParamStart();
      }
      break;
    default:
      break;
    }
}


void measureParamInit(void)
{
    uint16_t adc0V5 = 426;//HWCFG_SFRS->ADC_0V5_CODE;//816
    uint16_t adc1V0 = 853;//HWCFG_SFRS->ADC_1V0_CODE;//1652
    
    uint16_t adc0V2 = 1773;//HWCFG_SFRS->ADC_1V0_CODE;//1652
    uint16_t adc0V0 = 975;//5gain 31/32
    
    uint16_t batt13V5 = 802;//
    uint16_t batt8V0 = 468;//    
    
    adcResult.vChipTemp.coeff.coefficient = (1000 -500)*MEASURE_GAIN/((int32_t)adc1V0 - (int32_t)adc0V5);
    adcResult.vChipTemp.coeff.offset =  500*MEASURE_GAIN - adcResult.vChipTemp.coeff.coefficient*(int32_t)adc0V5;
    adcResult.vChipTemp.currCode = INVALID_PARAM;
    /* v = a*t + b;  a = 2.273,  t = v/a - b/a;   gain = 1/a, offset = - b/a*/
    adcResult.tChip.coefficient = MEASURE_TEMP_GAIN;
    adcResult.tChip.offset      = 25*(int32_t)(1UL << MEASURE_TEMP_GAIN_POS) - MEASURE_TEMP_GAIN*(int32_t)HWCFG_SFRS->TSENSOR_mV_25C;
    /*vAmp  5gain 31/32 200mv 20mv*/
    adcResult.vAmp.coeff.coefficient = (200 -0)*MEASURE_GAIN/((int32_t)adc0V2 - (int32_t)adc0V0);
    adcResult.vAmp.coeff.offset =  0*MEASURE_GAIN - adcResult.vAmp.coeff.coefficient*(int32_t)adc0V0;    
    adcResult.vAmp.currCode = INVALID_PARAM; 
    /* battery volt*/
    adcResult.vBatt.coeff.coefficient = (int32_t)(13500 - 8000)*MEASURE_GAIN/((int32_t)batt13V5 - (int32_t)batt8V0);
    adcResult.vBatt.coeff.offset      = (int32_t)(8000*MEASURE_GAIN) - adcResult.vBatt.coeff.coefficient*(int32_t)batt8V0;
    adcResult.vBatt.currCode    = INVALID_PARAM;
    /* PB3 volt*/
    adcResult.vPB3.coeff.coefficient = (int32_t)(1000 - 500)*MEASURE_GAIN/((int32_t)adc1V0 - (int32_t)adc0V5);
    adcResult.vPB3.coeff.offset      = (int32_t)(500*MEASURE_GAIN) - adcResult.vPB3.coeff.coefficient*(int32_t)adc0V5;
    adcResult.vPB3.currCode    = INVALID_PARAM;
    /* PC4 volt*/    
    adcResult.vPC4.coeff.coefficient = (int32_t)(1000 - 500)*MEASURE_GAIN*32/((int32_t)adc1V0 - (int32_t)adc0V5)/22;
    adcResult.vPC4.coeff.offset      = (int32_t)(500*MEASURE_GAIN) - adcResult.vPC4.coeff.coefficient*(int32_t)adc0V5;
    adcResult.vPC4.currCode    = INVALID_PARAM; 
}

int16_t Get_vAmp(void){  
  return adcResult.vAmp.target;
}

uint16_t *MEAGet_vAmpbuff(void){
  
return vAmpbuff;
  
}
uint16_t MEA_GetVbat(void)
{
  return adcResult.vBatt.target;
}