/*
* Copyright (c) 2013 - 2014, Freescale Semiconductor, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* o Redistributions of source code must retain the above copyright notice, this list
*   of conditions and the following disclaimer.
*
* o Redistributions in binary form must reproduce the above copyright notice, this
*   list of conditions and the following disclaimer in the documentation and/or
*   other materials provided with the distribution.
*
* o Neither the name of Freescale Semiconductor, Inc. nor the names of its
*   contributors may be used to endorse or promote products derived from this
*   software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
* ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

////////////////////////////////////////////////////////////////////////////////
// Includes
////////////////////////////////////////////////////////////////////////////////

#include "board.h"
#include "pin_mux.h"
#include "fsl_usart.h"
#include "fsl_common.h"

#include "freemaster.h"



#include "main.h"
#include "mlib.h"
#include "mlib_types.h"
#include "gflib.h"

#include "sct_mc.h"
/*******************************************************************************
* Definitions
******************************************************************************/
#define MOTOR_START        1
#define MOTOR_STOP         2
#define MOTOR_ORT          5

/*******************************************************************************
* Prototypes
******************************************************************************/
static void init_freemaster_uart(void);

static void AppInit(void);
static void AppAlignment(void);
static void AppStart(void);
static void AppRun(void);
static void AppStop(void);
static void AppError(void);
static void AppInitToStop(void);
static void AppStopToAlignment(void);
static void AppAlignmentToStart(void);
static void AppStartToStop(void);
static void AppStopToStart(void);
static void AppStartToRun(void);
static void AppRunToStop(void);
static void AppStopToError(void);
static void AppStartToError(void);
static void AppRunToError(void);

static tPointerFcn AppStateMachine[] = {AppInit,AppAlignment,AppStart,AppRun,AppStop,AppError};

/*******************************************************************************
* Variables
******************************************************************************/
uint8_t ui8AppStartFlag;
int16_t cap_cnt;
int16_t cap_cntFilt;
static GFLIB_RAMP_T_F16 sSpeedRampParam;
static frac16_t f16TargetSpeed, f16TargetSpeedRamped,f16InErr,f16PIout;
GFLIB_CTRL_PI_P_AW_T_A32 sSpeedCtrl;
GFLIB_CTRL_PI_P_AW_T_A32 sCurrentCtrl;
bool_t bStopIntegFlag;
frac16_t f16CaptureSpeed,f16SpeedCoef;
frac16_t f16IdcbHighLimit;
frac16_t f16IdcbLowLimit;
volatile uint16_t motor_start = 0;
volatile uint16_t motor_ort =1;
static uint8_t 	appState;
volatile MOTOR_STATE_T motor_state=M_STOP;
extern unsigned char g_InitPosVal;
extern volatile unsigned char g_Direction;
extern volatile uint32_t speed_sp; 

volatile uint32_t speed_limit_monitor = 0;
volatile uint32_t tick_limit = 45;
/*******************************************************************************
* Code
******************************************************************************/
void SysTick_Handler(void)
{
 static uint32_t speed;
 
 speed_limit_monitor++;
 
 AppStateMachine[appState]();

}

/*!
* @brief   Initialization of ADC0
*
*          8 MHz System Oscillator Bus Clock is the source clock.
*          single-ended 12-bit conversion
*
* @param   void
*
* @return  None
*/
#define FRO_CLK 30000000
void Adc0Setup(void)
{
  uint8_t clkdiv = 0;
  uint32_t temp;
  // Step 1. Power up and reset the ADC, and enable clocks to peripheral
  SYSCON->PDRUNCFG       &= ~(SYSCON_PDRUNCFG_ADC_PD_MASK);
  SYSCON->PRESETCTRL0    &= ~(SYSCON_PRESETCTRL0_ADC_RST_N_MASK);
  SYSCON->PRESETCTRL0    |=  (SYSCON_PRESETCTRL0_ADC_RST_N_MASK);
  SYSCON->SYSAHBCLKCTRL0 |= (SYSCON_SYSAHBCLKCTRL0_ADC_MASK); /* Enable Clock to ADC */
  SYSCON->ADCCLKDIV       = 1;                 // Enable clock, and divide-by-1 at this clock divider
  SYSCON->ADCCLKSEL       = 0; // Use fro_clk as source for ADC async clock
  
  temp =  ADC0->CTRL;
  // Step 2. Perform a self-calibration
  // Choose a CLKDIV divider value that yields about 500 KHz.
  clkdiv = (FRO_CLK / 500000) - 1;
  
  // Start the self-calibration
  // Calibration mode = true, low power mode = false, CLKDIV = appropriate for 500,000 Hz
  ADC0->CTRL = ( (ADC_CTRL_CALMODE(1)) | (ADC_CTRL_LPWRMODE(0)) | (ADC_CTRL_CLKDIV(clkdiv)) );
  
  // Poll the calibration mode bit until it is cleared
  while (ADC0->CTRL & ADC_CTRL_CALMODE_MASK);
  /*  Here is ADC calibrated */
  
  ADC0->CTRL = temp; /*REstroe ADC setting*/
  ADC0->CTRL |= 0x1; /*Clock Divider 2*/
  ADC0->TRM = 0; /*HIGH voltage range 2,7 - 3,6V VDD*/
  
//  ADC0->SEQ_CTRL[0] |=  ADC_SEQ_CTRL_TRIGPOL_MASK; //polarity of triger
  
 //TODO Some macro for easy edit? 
  SWM0->PINENABLE0 &= ~(SWM_PINENABLE0_ADC_2_MASK|SWM_PINENABLE0_ADC_3_MASK); 
  
}

volatile uint32_t dcb_cur,dcb_volt;

volatile frac16_t f16Vdcb,f16Idcb;

void ADC0_SEQA_IRQHandler(void)
{
  //indicate ADC isr begin, (only for debugging)
  GPIO->CLR[0] = 1<<6;
  GPIO->DIRSET[0] = 1<<6;
  GPIO->SET[0] = 1<<6;

  
  f16Idcb  =  ((ADC0->DAT[2]& ADC_DAT_RESULT_MASK)>>  ADC_DAT_RESULT_SHIFT) - 2048;
  f16Vdcb =   ((ADC0->DAT[3]& ADC_DAT_RESULT_MASK)>>  ADC_DAT_RESULT_SHIFT);
  
  
  ADC0->FLAGS = ADC_FLAGS_SEQA_INT(1);  //clear flag to enable new interrupt
	
  //indicate ADC isr end,(only for debugging)
  GPIO->CLR[0] = 1<<6;
  
  FMSTR_Recorder(0);
  
  return;
}

int main(void)
{
  appState = APP_INIT;
  uint32_t temp;
  
  BOARD_InitPins();
  BOARD_BootClockFRO30M();
  BOARD_InitDebugConsole();
  /* Select the main clock as source clock of USART0 (debug console) */
  CLOCK_Select(BOARD_DEBUG_USART_CLK_ATTACH);
  
  // FreeMASTER will communicate over default UART instance. This demo is common
  // for UART and LPSCI modules (which are compatible), but we need to use
  // different HAL driver for initialization
  
  // Initialize FreeMASTER driver
  FMSTR_Init();
  
  /* Set systick reload value to generate 100ms interrupt */
  SysTick_Config(SystemCoreClock / 1000U);
  
  Adc0Setup();
  
/*Set Chaneel 2 and 3 for sequence A (DCB CURRENT, DCB VOLT, enable sequence */

  //configure the ADC; prepare everything, enable as the last step
  ADC0->SEQ_CTRL[0] = 0;
  ADC0->INTEN = 1<<0;   //enable SEQA interrupt
  temp = ADC0->DAT[2];  //dump ADC_2 reading
  temp = ADC0->DAT[3];  //dump ADC_3 reading
	
  //IMPORTANT: make sure pins acting as ADC inputs have internal 
  //           pull-up/down disabled in BOARD_InitPins() !!!
  
  ADC0->SEQ_CTRL[0] = 
    ADC_SEQ_CTRL_CHANNELS(1<<2 | 1<<3) | //ADC_IN2/3
    ADC_SEQ_CTRL_TRIGGER(4)            | //SCT_OUT4 hw trigger
    ADC_SEQ_CTRL_TRIGPOL(1)            |  //trigger @ positive edge
    ADC_SEQ_CTRL_SYNCBYPASS(0)         |  //enable synchronization
    ADC_SEQ_CTRL_START(0)              |  //do not START
    ADC_SEQ_CTRL_BURST(0)              |  //no BURST
    ADC_SEQ_CTRL_SINGLESTEP(0)         |  //no SINGLESTEP
    ADC_SEQ_CTRL_LOWPRIO(1)            |  //set SEQ A as high priority
    ADC_SEQ_CTRL_MODE(1)               |  //retrieve data at the end of sequence
    ADC_SEQ_CTRL_SEQ_ENA(0);              //disable sequence
				 
  ADC0->SEQ_CTRL[0] = 
    ADC_SEQ_CTRL_CHANNELS(1<<2 | 1<<3) | //ADC_IN2/3
    ADC_SEQ_CTRL_TRIGGER(4)            | //SCT_OUT4 hw trigger
    ADC_SEQ_CTRL_TRIGPOL(1)            |  //trigger @ positive edge
    ADC_SEQ_CTRL_SYNCBYPASS(0)         |  //enable synchronization
    ADC_SEQ_CTRL_START(0)              |  //do not START
    ADC_SEQ_CTRL_BURST(0)              |  //no BURST
    ADC_SEQ_CTRL_SINGLESTEP(0)         |  //no SINGLESTEP
    ADC_SEQ_CTRL_LOWPRIO(1)            |  //set SEQ A as high priority
    ADC_SEQ_CTRL_MODE(1)               |  //retrieve data at the end of sequence
    ADC_SEQ_CTRL_SEQ_ENA(1);              //enable sequence
  
  NVIC_SetPriority(ADC0_SEQA_IRQn, 2);
  NVIC_EnableIRQ(ADC0_SEQA_IRQn);
  
  /* Code below updates the PWM dutycycle for Out */
  while (1)
  {
    // FreeMASTER example increments several variables periodically,
    // use the FreeMASTER PC Host tool to visualize the variables
    FMSTR_Poll();
  }
}

static void AppInit(void)
{

  f16IdcbLowLimit = F16_SPEED_CTRL_I_LIMIT_LOW;
  f16IdcbHighLimit = F16_SPEED_CTRL_I_LIMIT_HIGH;
  sSpeedCtrl.a32PGain = ACC32(0.5);
  sSpeedCtrl.a32IGain = ACC32(0.01);
  sSpeedCtrl.f16UpperLim = FRAC16(1.0);
  sSpeedCtrl.f16LowerLim = FRAC16(0.01);
  bStopIntegFlag = FALSE;
  GFLIB_CtrlPIpAWInit_F16(FRAC16(0.0), &sSpeedCtrl);

  sCurrentCtrl.a32PGain = ACC32(1);
  sCurrentCtrl.a32IGain = ACC32(0.01);
  sCurrentCtrl.f16UpperLim = FRAC16(1.0);
  sCurrentCtrl.f16LowerLim = FRAC16(0.0);
  GFLIB_CtrlPIpAWInit_F16(FRAC16(0.0), &sCurrentCtrl);
  
  sSpeedRampParam.f16RampUp   = FRAC16(0.01);
  sSpeedRampParam.f16RampDown = FRAC16(0.01);
  GFLIB_RampInit_F16(FRAC16(0.0), &sSpeedRampParam);
  f16SpeedCoef = FRAC16(0.07486);
  cap_cnt = 0;
  MC_INIT();
  AppInitToStop();
}


static void AppAlignment(void)
{
  
}

static void AppAlignmentToStart(void)
{
  appState = APP_START;
}

/* Start STATE */
static void AppStart(void)
{
  /* Unhalt SCT0 counter ( Both part )  */
  SCT0->CTRL &= ~(SCT_CTRL_HALT_L_MASK | SCT_CTRL_HALT_H_MASK); 
  
  AppStartToRun();		
}
 /* Transit state  Start -> Run*/
static void AppStartToRun(void)
{
  appState = APP_RUN;
}
 /* Transit state  Start -> Stop*/
static void AppStartToStop(void)
{
  appState = APP_INIT;
}

static void AppStartToError(void)
{
  appState = APP_ERROR;
}

extern volatile uint16_t pwm_xy_deadtime;
extern volatile uint32_t speed_timer_rd_new;

/* Main state RUN */
static void AppRun(void)
{
  static frac16_t duty;

  if(speed_limit_monitor > tick_limit)/* Check if motor running */
  {
     f16CaptureSpeed=0;  
     speed_timer_rd_new = 0;
  }
  else
  {
      f16CaptureSpeed = MLIB_Div1QSat_F16(f16SpeedCoef, cap_cntFilt);  
  }

  f16TargetSpeedRamped = GFLIB_Ramp_F16(f16TargetSpeed, &sSpeedRampParam);
  //f16InErr = f16TargetSpeedRamped - f16CaptureSpeed;
/*
  if(f16Idcb > F16_I_DCB_LIMIT)
  {
    if(!ui8CurrentLimitDone)
    {
        ui8CurrentLimitDone = 1;
      
    }
  }
  else
  {
      
  }
*/
  
  if(f16Idcb > f16IdcbHighLimit)
  {
    f16InErr = f16IdcbHighLimit - f16Idcb;
    f16PIout = GFLIB_CtrlPIpAW_F16(f16InErr, &bStopIntegFlag, &sCurrentCtrl);
    sSpeedCtrl.f32IAccK_1 = sCurrentCtrl.f32IAccK_1;
  }
  
  if(f16Idcb < f16IdcbLowLimit)
  {
      f16InErr = f16TargetSpeedRamped - f16CaptureSpeed;
      f16PIout = GFLIB_CtrlPIpAW_F16(f16InErr, &bStopIntegFlag, &sSpeedCtrl);
      sCurrentCtrl.f32IAccK_1 = sSpeedCtrl.f32IAccK_1;
  }

  
 // f16PIout = GFLIB_CtrlPIpAW_F16(f16InErr, &bStopIntegFlag, &sSpeedCtrl);
                
  duty = MLIB_Mul_F16(CNT_MAXIMUN_VALUE, f16PIout);
  
  MR3 = pwm_xy_deadtime+duty-1;
  MR4 = pwm_xy_deadtime+duty+pwm_xy_deadtime-1;
  
  //update ADC sampling point (@ 25% pulse x)
  MR6 = pwm_xy_deadtime+(duty>>2)-1;//>>1
  
  if (!ui8AppStartFlag)
  {
    SCT_STOP();
    NVIC_ClearPendingIRQ((IRQn_Type) (SCT0_IRQn));
    
    AppRunToStop();
  }
 
}

static void AppRunToError(void)
{
  appState = APP_ERROR;
}


static void AppStop(void)
{
  if(ui8AppStartFlag)
  {
   AppStopToStart();		
  }
}

static void AppStopToError(void)
{
  appState = APP_ERROR;
}

static void AppError(void)
{
  
}


static void AppBrake(void)
{
  
}

static void AppBrakeToStop(void)
{
  appState = APP_STOP;
}

static void AppInitToStop(void)
{
  appState = APP_STOP; 
}

static void AppStopToStart(void)
{
  
  appState = APP_START;  
}

static void AppRunToStop(void)
{
  appState = APP_INIT;
}

////////////////////////////////////////////////////////////////////////////////
// EOF
/////////////////////////////////////////////////////////////////////////////////
