#include <motorControlTask.h>
#include <pwm_aux_device.h>
#include <linStackTask.h>
#include <pdsTask.h>
#include <gtimer_device.h>
#include <adc_device.h>
#include <safetyMonitorTask.h>
#include <lin_device.h>
#include <flash_device.h>
#include <flash_sfrs.h>
#include <linSlaveTask.h>

static TaskState_t motorState = TASK_STATE_INIT;
static ADCMeasureParam_t  motoradcItem;

/*Data sent from LIN host, including setting parameters and control parameters*/
static M_Re_pra_t  M_Re_pra = {    
  .M_ON = 0,
  .Constantspeed = 500U,
  .Startspeed = 1000U,
  .Slowspeed = 1000U,
  .LockACoilValtage = 500U,
  .LockBCoilValtage = 500U,
  .ClimbTime = 120U,
  .SlowTime = 120U,
  .Position = 0,
};

static uint8_t SysPowerup = 0;
static uint16_t MotorPosition = 0;
static uint8_t Motor_runnstatuschanged = 0;
static uint8_t Onestep_csa_adcnt = 0U;
static uint16_t Count_20ms = 0;
static uint16_t MotorOCstop10s = 0;

static Motorpra_t  motorpra;
static Motor_and_ChipPar_t motorandchippar = {
  .state.byte[0] = 0,
  .state.byte[1] = 0, 
};

static uint16_t bufftemp[TransferCNT];

void delay(uint32_t delay)//1000000   1s
{
  for (uint32_t i = 0; i < delay; i++){
      for (uint32_t j = 0; j < 3; j++){
        __asm("NOP");
      }
  }
}

void Get_MotorprafromLINM(LIN_Device_Frame_t const *frame)
{  
    MotorCtrlFrame_t const *motor = (MotorCtrlFrame_t const*)((void const*)frame->data);
//    if ( (motor->nodeAddress == ls_read_nad()) || (motor->nodeAddress == LIN_NAD_WILDCARD))
//    {
      M_Re_pra.M_ON  = (motor->data[1])&0x01U ;                   //0X01 ON 
      M_Re_pra.Position = motor->data[3];
      M_Re_pra.Position = (M_Re_pra.Position << 8U) + motor->data[2];
      
      if((M_Re_pra.M_ON != Motor_runnstatuschanged) || (MotorPosition != M_Re_pra.Position)){     
/*The position has changed*/      
        motorpra.ClimbTime = 0U;
        motorpra.SlowTime = 0U;
      }
      MotorPosition = M_Re_pra.Position;
      Motor_runnstatuschanged = M_Re_pra.M_ON;
      motorandchippar.MotorErrorState10s = 10000000U/M_Re_pra.Constantspeed;
//    }    
}

void StopMotor(void)
{  
  MotorPower_OFF;
  VDD3V3EXT_OFF;
  Twobridge_OFF;  
  
  motorpra.ClimbTime = 0U;
  motorpra.SlowTime = 0U;
  
  motorandchippar.state.Motor_runsta = (uint8_t)MotorStop;
  motorandchippar.state.Motor_runstage = (uint8_t)Climbstage; 
  
  if(SysPowerup == 0U){
    
      PDS_PowerONRead_MotorParamfromFlash(bufftemp,TransferCNT);
      motorpra.stepn = PDS_GetMotorstepnfromflash();/*From flash*/
      
      M_Re_pra.Constantspeed  = bufftemp[0];
      M_Re_pra.Startspeed  = bufftemp[1]; 
      M_Re_pra.Slowspeed  = bufftemp[2];  
      M_Re_pra.LockACoilValtage  = bufftemp[3];
      M_Re_pra.LockBCoilValtage  = bufftemp[4];  
      M_Re_pra.ClimbTime = (uint8_t)bufftemp[5];
      M_Re_pra.SlowTime = (uint8_t)(bufftemp[5]>>8U); 
      
      motorpra.ClimbSteper = (uint16_t)((uint32_t)(M_Re_pra.ClimbTime)*1000U*2U/((uint32_t)(M_Re_pra.Startspeed )+ (uint32_t)(M_Re_pra.Constantspeed)));        
      motorpra.SlowSteper = (uint16_t)((uint32_t)(M_Re_pra.SlowTime)*1000U*2U/((uint32_t)(M_Re_pra.Slowspeed) + (uint32_t)(M_Re_pra.Constantspeed)));
      Onestep_csa_adcnt =  M_Re_pra.Constantspeed*16 / PWMAUX_Period; //计算一步内的采样次数
      SysPowerup = 1u;
  }
  if(LINS_TransferMotorPhyParamtobuffBy3C(bufftemp,TransferCNT) == 1U){//3C 
    
      M_Re_pra.Constantspeed  = bufftemp[0];
      M_Re_pra.Startspeed  = bufftemp[1]; 
      M_Re_pra.Slowspeed  = bufftemp[2];  
      M_Re_pra.LockACoilValtage  = bufftemp[3];
      M_Re_pra.LockBCoilValtage  = bufftemp[4];  
      M_Re_pra.ClimbTime = (uint8_t)bufftemp[5];
      M_Re_pra.SlowTime = (uint8_t)(bufftemp[5]>>8U);
      
      motorpra.ClimbSteper = (uint16_t)((uint32_t)(M_Re_pra.ClimbTime)*1000U*2U/((uint32_t)(M_Re_pra.Startspeed )+ (uint32_t)(M_Re_pra.Constantspeed)));        
      motorpra.SlowSteper = (uint16_t)((uint32_t)(M_Re_pra.SlowTime)*1000U*2U/((uint32_t)(M_Re_pra.Slowspeed) + (uint32_t)(M_Re_pra.Constantspeed)));
      Onestep_csa_adcnt =  M_Re_pra.Constantspeed*16 / PWMAUX_Period; 
  }
  
}


void Calculation_position(void){ 

  if(M_Re_pra.M_ON == 1U){  

    VDD3V3EXT_ON; 
     if(M_Re_pra.Position > (uint16_t)(motorpra.stepn)){
       
       if((SAFM_TransferState() & 0xFFU) == 0U){
         
           motorpra.s = M_Re_pra.Position - motorpra.stepn;
            
           if(motorpra.s>0u){
             
              MotorPower_ON;          
              
               if(motorpra.ClimbTime < motorpra.ClimbSteper){           
                   motorpra.ClimbTime++;           
                }else{  
                  if(motorpra.s <= motorpra.SlowSteper){
                   motorpra.SlowTime++;
                  }
                }
              if(motorpra.ClimbTime < motorpra.ClimbSteper){
                    TIMER_Disable(TIMER1);
                    motorpra.T1timercnt = (uint32_t)(M_Re_pra.Startspeed)*16U-(uint32_t)(16U*((uint32_t)(M_Re_pra.Startspeed)-(uint32_t)(M_Re_pra.Constantspeed))*(uint32_t)(motorpra.ClimbTime))/(uint32_t)(motorpra.ClimbSteper);
                    
                    TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, motorpra.T1timercnt, Timer1_INTTest);
                    motorandchippar.state.Motor_runstage = (uint8_t)Climbstage;
                }else if((motorpra.SlowTime<motorpra.SlowSteper)&&(motorpra.SlowSteper > motorpra.s)){
                    TIMER_Disable(TIMER1); 
                    motorpra.T1timercnt = (uint32_t)((uint32_t)(M_Re_pra.Constantspeed)*16U+(16U*(((uint32_t)(M_Re_pra.Slowspeed)-(uint32_t)(M_Re_pra.Constantspeed)))*(uint32_t)(motorpra.SlowTime))/(uint32_t)(motorpra.SlowSteper));
                    TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, motorpra.T1timercnt, Timer1_INTTest);
                    motorandchippar.state.Motor_runstage = (uint8_t)Slowstage;
                }else{
                  if(motorandchippar.state.Motor_runstage != (uint8_t)Constantstage){
                    TIMER_Disable(TIMER1); 
                    TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, (uint32_t)M_Re_pra.Constantspeed*16U, Timer1_INTTest);
                  }
                  motorandchippar.state.Motor_runstage = (uint8_t)Constantstage;                  
                }
             #if control_type == Divide_2 
               motorpra.MM_Step=(uint8_t)(motorpra.stepn % 8U);
             #elif control_type == Divide_6
                motorpra.MM_Step=(uint8_t)(motorpra.stepn % 24U);
             #endif
                motorpra.stepn++;
                motorandchippar.state.Motor_runsta  = (uint8_t)MotorRun;
             }
          }
        }else if(M_Re_pra.Position < (uint16_t)(motorpra.stepn)){
          if((SAFM_TransferState() & 0xffu) == 0U){
           motorpra.s = motorpra.stepn - M_Re_pra.Position;

              if(motorpra.s>0U)
              {
                MotorPower_ON;

                if(motorpra.ClimbTime < motorpra.ClimbSteper){          
                   motorpra.ClimbTime++;           
                }else{ 
                  if(motorpra.s <= motorpra.SlowSteper){
                   motorpra.SlowTime++;
                  }
                }             
		if(motorpra.ClimbTime < motorpra.ClimbSteper){
                  TIMER_Disable(TIMER1);
                  motorpra.T1timercnt = (uint32_t)((uint32_t)M_Re_pra.Startspeed*16U-(16U*((uint32_t)M_Re_pra.Startspeed-(uint32_t)M_Re_pra.Constantspeed)*motorpra.ClimbTime)/motorpra.ClimbSteper);
                  TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, motorpra.T1timercnt, Timer1_INTTest);
                  motorandchippar.state.Motor_runstage = (uint8_t)Climbstage;                  
                }else if((motorpra.SlowTime < motorpra.SlowSteper)&&(motorpra.SlowSteper > motorpra.s)){
                  TIMER_Disable(TIMER1); 
                  motorpra.T1timercnt = (uint32_t)((uint32_t)M_Re_pra.Constantspeed*16U+(16U*((uint32_t)M_Re_pra.Slowspeed-(uint32_t)M_Re_pra.Constantspeed)*motorpra.SlowTime)/motorpra.SlowSteper);
                  TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, motorpra.T1timercnt, Timer1_INTTest);
                  motorandchippar.state.Motor_runstage = (uint8_t)Slowstage;                 
                }else{
                  if(motorandchippar.state.Motor_runstage != (uint8_t)Constantstage){
                    TIMER_Disable(TIMER1); 
                    TIMER_Init(TIMER1, HW_TIMER_PERIODIC_MODE, GTIMER_CLK_DIV_1, (uint32_t)M_Re_pra.Constantspeed*16U, Timer1_INTTest);
                  }
                  motorandchippar.state.Motor_runstage = (uint8_t)Constantstage;                  
                }
                
             #if control_type == Divide_2 
               motorpra.MM_Step=(uint8_t)(motorpra.stepn % 8U);
             #elif control_type == Divide_6
                motorpra.MM_Step=(uint8_t)(motorpra.stepn % 24U);
             #endif               
                motorpra.stepn--;
                motorandchippar.state.Motor_runsta  = (uint8_t)MotorRun;
              }
          }
        }else{
            StopMotor(); 
        }
   }
   else{
          StopMotor();     
       } 
   motorandchippar.Motor_position = motorpra.stepn;
}

#if control_type == Divide_2  

void  MOTORunP_8(uint8_t motorstep){    
  switch(motorstep){
  case 0://1000 Phase B zero crossing, detect reverse electromotive force of B phase
    Twobridge_OFF;
    
    Downpwm_APhasePos_High_ON; //PA3 ON(Upper bridge arm)   
    Downpwm_APhaseNeg_Low_ON;//PA5 OFF(Lower bridge arm)
    break;
  case 1://1010
    Twobridge_OFF; 
    
    Downpwm_APhasePos_High_ON;    
    Downpwm_APhaseNeg_Low_ON;

    Downpwm_BPhasePos_High_ON;    
    Downpwm_BPhaseNeg_Low_ON;
    break;
  case 2://0010 Phase A zero crossing, detect reverse electromotive force of A phase  
    Twobridge_OFF;
    
    Downpwm_BPhasePos_High_ON;    
    Downpwm_BPhaseNeg_Low_ON;
    break;
  case 3://0110
    Twobridge_OFF;
    
    Downpwm_APhaseNeg_High_ON;
    Downpwm_APhasePos_Low_ON;    

    Downpwm_BPhasePos_High_ON;    
    Downpwm_BPhaseNeg_Low_ON;
    break;
  case 4://0100   Phase B zero crossing, detect reverse electromotive force of B phase
    Twobridge_OFF;

    Downpwm_APhaseNeg_High_ON;
    Downpwm_APhasePos_Low_ON;
    break;
  case 5://0101
    Twobridge_OFF;
 
    Downpwm_APhaseNeg_High_ON;
    Downpwm_APhasePos_Low_ON;    
       
    Downpwm_BPhaseNeg_High_ON;
    Downpwm_BPhasePos_Low_ON;
    break;
  case 6://0001  Phase A zero crossing, detect reverse electromotive force of A phase
    Twobridge_OFF;
 
    Downpwm_BPhaseNeg_High_ON;
    Downpwm_BPhasePos_Low_ON;
    break; 
  case 7://1001
    Twobridge_OFF;
 
    Downpwm_APhasePos_High_ON;    
    Downpwm_APhaseNeg_Low_ON;    
       
    Downpwm_BPhaseNeg_High_ON;
    Downpwm_BPhasePos_Low_ON;
    break;
 default: 
    Twobridge_OFF;   
  break;    
  }
}
#elif control_type == Divide_6 

const uint16_t Downpwm_ACoil[24] = {247U,221U,181U,128U,66U,0U,66U,128U,181U,221U,247U,256U,247U,221U,181U,128U,66U,0U,66U,128U,181U,221U,247U,256U};
const uint16_t Downpwm_BCoil[24] = {66U,128U,181U,221U,247U,256U,247U,221U,181U,128U,66U,0U,66U,128U,181U,221U,247U,256U,247U,221U,181U,128U,66U,0U};

void  MOTORunP_24(uint8_t motorstep){
  
  uint16_t temp1;
  uint16_t temp2;
  
   temp1 = (uint16_t)(((uint32_t)PWM_AUX_SFRS->PWM_AUX[PWMAUX_BASE_0].BASE.PERIOD * Downpwm_ACoil[motorstep])>>8U);
   temp2 = (uint16_t)(((uint32_t)PWM_AUX_SFRS->PWM_AUX[PWMAUX_BASE_0].BASE.PERIOD * Downpwm_BCoil[motorstep])>>8U); 
   PWM_AUX_SFRS->INVERT = 0X3FU;
  
   switch(motorstep){    
      case 0://   
      case 1:
      case 2:
      case 3:
      case 4:
      case 5://A相电流由正向往负向的过零点 
           Downpwm_BPhasePos_Low_OFF;//PB1
           Downpwm_BPhaseNeg_High_OFF;//PA4 OFF(Upper bridge arm)
           Downpwm_APhasePos_Low_OFF;//PA2 PWMAUX0
           Downpwm_APhaseNeg_High_OFF;//PA6
           delay(25);
           
           Downpwm_BPhasePos_High_ON; //PB2 ON(Upper bridge arm)           
           PWMAUX_SetMatchValue(PWMAUX_CH1, 0, temp2);//PB0  PWMAUX1
           Downpwm_APhasePos_High_ON; //PA3 ON(Upper bridge arm)
           PWMAUX_SetMatchValue(PWMAUX_CH0, 0, temp1);//PA5 PWMAUX0
    break;  
      case 6:  
      case 7:
      case 8:
      case 9:
      case 10:
      case 11://B相电流由正向往负向的过零点 
           Downpwm_BPhasePos_Low_OFF;//PB2
           Downpwm_BPhaseNeg_High_OFF;//PA4 OFF(Upper bridge arm)
           Downpwm_APhasePos_High_OFF; //PA3 ON(Upper bridge arm)
           Downpwm_APhaseNeg_Low_OFF;//PA5 PWMAUX0
           delay(25);
           
           Downpwm_BPhasePos_High_ON; //PB2 ON(B phase Upper bridge arm)
           //Downpwm_BPhasePos_High_OFF; //PB2 ON(B phase Upper bridge arm)
           PWMAUX_SetMatchValue(PWMAUX_CH1, 0, temp2);//PB0  PWMAUX1
           PWMAUX_SetMatchValue(PWMAUX_CH5, 0, temp1);//PA2 PWMAUX5
           Downpwm_APhaseNeg_High_ON;//PA6            
    break;
    
      case 12:
      case 13:
      case 14:
      case 15:
      case 16:
      case 17://A相电流由负向往正向的过零点 
           Downpwm_BPhasePos_High_OFF; //PB2(Upper bridge arm)
           Downpwm_BPhaseNeg_Low_OFF;//PB0  PWMAUX1
           Downpwm_APhasePos_High_OFF; //PA3 ON(Upper bridge arm)
           Downpwm_APhaseNeg_Low_OFF;//PA5 PWMAUX0
           delay(25);
           
           PWMAUX_SetMatchValue(PWMAUX_CH2, 0, temp2);//PB1  PWMAUX2
           Downpwm_BPhaseNeg_High_ON;//PA4(Upper bridge arm)
           PWMAUX_SetMatchValue(PWMAUX_CH5, 0, temp1);//PA2 PWMAUX5
           Downpwm_APhaseNeg_High_ON;//PA6
   break;
      case 18:
      case 19:
      case 20:
      case 21:
      case 22:
//      case 23://B相电流由负向往正向的过零点 
           Downpwm_BPhasePos_High_OFF; //PB2(Upper bridge arm) 
           Downpwm_BPhaseNeg_Low_OFF;//PB0  PWMAUX1
           Downpwm_APhasePos_Low_OFF;//PA2 PWMAUX0
           Downpwm_APhaseNeg_High_OFF;//PA6
           delay(25);
           
           PWMAUX_SetMatchValue(PWMAUX_CH2, 0, temp2);//PB1  PWMAUX2
           Downpwm_BPhaseNeg_High_ON;//PA4(Upper bridge arm) 
           Downpwm_APhasePos_High_ON; //PA3 ON(Upper bridge arm)
           PWMAUX_SetMatchValue(PWMAUX_CH0, 0, temp1);//PA5 PWMAUX0
   break; 
      case 23://B相电流由负向往正向的过零点 
           Downpwm_BPhasePos_High_OFF; //PB2(Upper bridge arm) 
           Downpwm_BPhaseNeg_Low_OFF;//PB0  PWMAUX1
           Downpwm_APhasePos_Low_OFF;//PA2 PWMAUX0
           Downpwm_APhaseNeg_High_OFF;//PA6
           delay(25);
           
           PWMAUX_SetMatchValue(PWMAUX_CH2, 0, temp2);//PB1  PWMAUX2
           Downpwm_BPhaseNeg_High_OFF;//PA4(Upper bridge arm) 
           Downpwm_BPhasePos_High_ON;
             
           Downpwm_APhasePos_High_ON; //PA3 ON(Upper bridge arm)
           PWMAUX_SetMatchValue(PWMAUX_CH0, 0, temp1);//PA5 PWMAUX0
   break;    
   
 default: 
      
  break;    
  }  
}

uint8_t Motor_GetMotorState(void){
  return motorandchippar.state.Motor_runsta ;
}

uint8_t Motor_GetMotorRunStage(void){  
   return motorandchippar.state.Motor_runstage ;;
} 

uint32_t Motor_GetMotorErrorState10s(void){
  return motorandchippar.MotorErrorState10s;  
}

uint8_t Motor_GetMotorStep(void){
  return motorpra.MM_Step;
} 

/*Invoke all set parameters of the motor*/
M_Re_pra_t Motor_GetALLMotorSetparameters (void){
  return M_Re_pra;
}

uint16_t Motor_GetMotorConstantspeed(void){
  return M_Re_pra.Constantspeed*16U;
}

void Motor_GetSETLockCurrent(uint16_t *data1,uint16_t *data2){
  *data1 = M_Re_pra.LockACoilValtage;
  *data2 = M_Re_pra.LockBCoilValtage;
}

uint16_t Motor_GetCurrentPosition(void){
  return motorandchippar.Motor_position;
}

uint8_t Motor_GetOnestep_csa_adcnt(void){
  static uint8_t adcnt = 0;
  adcnt = Onestep_csa_adcnt;
  if(adcnt == 0){
    adcnt = 10u;
  }
  return adcnt;
}

#endif

#if control_type == Divide_2 

void Timer1_INTTest(void)
{

}
#elif control_type == Divide_6
//uint32_t *flashdata = (uint32_t *)0xAC00U;
//uint32_t dataflash;
//uint32_t dataflash_b;
void Timer1_INTTest(void)
{
    uint8_t Motosteper = 0;
    Count_20ms++;
  
    Calculation_position();

    if(Motor_GetMotorState() == (uint8_t)MotorRun){
      motoradcItem = ADC_GetadcMeasParamm();
      if(motoradcItem.item != ADC_MEASURE_ITEM_VAMP){
        ADC_Init(ADC_MEASURE_ITEM_VAMP, 0); 
        SAR_CTRL_SFRS->SARINT.CLEAR.INT_CONV_DONE_CLR  = 1U;
        SAR_CTRL_SFRS->SARCTRL.SARENAREQ = 1U;
        SAR_CTRL_SFRS->SARCTRL.CONVERT = 1U;
      }
        Motosteper = Motor_GetMotorStep();
        MOTORunP_24(Motosteper);

    }
  
  if((SAFM_TransferState() & 0xffu) != 0U) {
    MotorOCstop10s++;
    if(MotorOCstop10s > Motor_GetMotorErrorState10s()){
      MotorOCstop10s = 0U;
      ClearMotorandchipparstate();
    }
  } 
//    if(Count_20ms > 2000U){
//      Count_20ms = 0;
//      f_FLASH_EraseSector(0xAC00U);
//      FLASH_Write2WordsWithECC(0xAC00U,0XAAAAAAAA,0X55555555); 
//     dataflash = *flashdata;
//     flashdata+=1;
//     dataflash_b = *flashdata;
//    }
}
#endif

void MotorCtrl_TaskHandler(void)
{
    switch(motorState){
    case TASK_STATE_INIT:
      /* Motor control initialization */
      StopMotor(); 
      /* End of Motor control initialization */
      motorState = TASK_STATE_ACTIVE;
      break;
    case TASK_STATE_ACTIVE:
      break;
    default:
      break;
    }
}