/* ----------------------------------------------------------------------------
 *         ATMEL Microcontroller Software Support
 * ----------------------------------------------------------------------------
 * Copyright (c) 2012, Atmel Corporation
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * - Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the disclaimer below.
 *
 * Atmel's name may not be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
 * DISCLAIMED. IN NO EVENT SHALL ATMEL 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.
 * ----------------------------------------------------------------------------
 */


/**
 *  \page Periodic_Motion_Detect Periodic Motion Detect Demo
 *
 *  \section Purpose
 *
 *  This demo is to show the full function of the system which periodically
 *  enables the motion detection function and capture image if needed. When
 *  capturing and displaying are finished, the system should go back to wait
 *  mode.
 *
 *  \section Requirements
 *
 *  This package can only be used with sam4s-wpir-rd.
 *
 *  \section Description
 *
 *  This demo is aimed to give a reference for low power PIR camera design.
 *
 *  The purpose of the wait mode is to achieve very low power consumption while
 *  keep shorter activation time than the backup mode.
 *
 *  The main flow of the demo is like following:
 *  1) enter wait mode after startup to save power.
 *  2) waked up by pre-defined timer
 *  3) enable motion detector
 *  4) if motion is detected during some time, take a picture
 *     else go back to wait mode.
 *  5) do 2)-4) recurrently.
 *
 *  \section Usage
 *
 *  -# Build the program and download it inside the evaluation board. Please
 *     refer to the
 *     <a href="http://www.atmel.com/dyn/resources/prod_documents/doc6224.pdf">
 *     SAM-BA User Guide</a>,
 *     application note or to the
 *     <a href="ftp://ftp.iar.se/WWWfiles/arm/Guides/EWARM_UserGuide.ENU.pdf">
 *     IAR EWARM User Guide</a>,
 *     depending on your chosen solution.
 *  -# After downloading through SAM-BA or IAR flash loader, run the application.
 *  -# To evaluate this demo, make some movements in front of the mounted Pyro
 *     Sensor. If the motion is detected, a picture will be captured and
 *     displayed through LCD.
 */

/** \file
 *
 *  This file contains all the specific code for the Periodic_Motion_Detect.
 */


/*----------------------------------------------------------------------------
 *        Headers
 *----------------------------------------------------------------------------*/

#include "board.h"

#include "board_low_level_backup.h"
#include <stdint.h>
#include <stdio.h>
#include <stdbool.h>

#include "motion_detector.h"
#include "rtt_alarm.h"

/*----------------------------------------------------------------------------
 *        Local definitions
 *----------------------------------------------------------------------------*/

/** lcd debug output support, uncomment to remove */
#define LCD_DEBUG_OUT

/** macro to controll the staying in each state permanently,comment for
    continuous transition */
//#define STATE_STAYED

/** display information on LCD during State by State mode (uncomment STATE_STAYED).
    Uncomment it for current measure*/
//#define LCD_DISPLAY_INFORMATION

#define IMAGE_WIDTH     (320)
#define IMAGE_HEIGHT    (240)
/** the captured data length per line in different color mode,double the size
in YUV full output*/
#define IMAGE_LINE_COLORED    (IMAGE_WIDTH * 2)
#define IMAGE_LINE_MONO  (IMAGE_WIDTH)

#define SRAM_BASE (0x60000000)
#define CAP_DEST  (SRAM_BASE)
// reserved for future use*/
//#define DISP_DEST (SRAM_BASE + 0x4000)

/** TWI clock frequency in Hz (400KHz) */
#define TWCK            400000

/** for power on or off*/
#define ON  true
#define OFF false

/** The output formats supported by the sensor*/
typedef enum {
    OUT_RGB,
    OUT_YUV
}OUT_FMT;

/** the system state for each mode*/
typedef enum{
    STATE_INIT, /*initalized*/
    STATE_START, /* start */
    STATE_WAIT,  /* wait mode */
    STATE_SLEEP_ENTER, /* sleep mode enter */
    STATE_SLEEP_EXIT,  /* sleep mode exit */
    STATE_ACTIVE_PLL,  /*Active mode*/
    STATE_ACTIVE_TRANSFER_SETUP,/* setup hardware and transfer*/
    STATE_SLEEP_CAPTURE,/*enter sleep and capturing at the same time*/
    STATE_END /* reserved*/
}STATE_TYPE;

/** the default output mode is YUV422*/
#define DEFAULT_SENSOR_YUV422

/** uncomment for monochrome output*/
#define DEFAULT_MODE_COLORED

/** reserved for future*/
#define DEFAULT_MCK_FROM_RC  ((uint32_t)4000000)

void Display_info(uint32_t state);

/** for current measurement*/
#ifdef STATE_STAYED
  #ifdef LCD_DISPLAY_INFORMATION
        #define WAIT_FOR_STATE(STATE) \
                PIO_Clear(&pinTest); \
                Display_info(STATE); \
                _WaitForState(STATE); \
                PIO_Set(&pinTest);

  #else
        #define WAIT_FOR_STATE(STATE) \
                PIO_Clear(&pinTest); \
                _WaitForState(STATE); \
                PIO_Set(&pinTest);
  #endif
#else
#define WAIT_FOR_STATE(STATE)
#endif

static void _Init_LCD(void);
/*----------------------------------------------------------------------------
 *        Local variables
 *----------------------------------------------------------------------------*/
/** the output format of image sensor*/
OUT_FMT out_format = OUT_YUV;
/** the output color mode of image sensor*/
PIO_OUT_MODE out_mode = PIO_OUT_COLOR;

/** is displaying needed*/
bool isDisplayed = true;

/** capturing destination buffer*/
uint8_t *cap_dest_buf;

/** displaying destination buffer, RESERVED*/
uint8_t *disp_dest_buf;

/** cpaturing rows*/
uint16_t cap_rows = IMAGE_HEIGHT;

/** cap line length*/
uint16_t cap_line = IMAGE_LINE_COLORED;

/** VSYNC flag*/
volatile bool vsync_flag = false;

/** TWI driver*/
static Twid twid;

/** BP1*/
const Pin pinBP1 = PIN_PUSHBUTTON_1;

/** use PIOA_5 for duration test*/
const Pin pinTest = { 1<<5,PIOA,ID_PIOA,PIO_OUTPUT_1,PIO_DEFAULT};

/** vsync pin*/
const Pin pinVSYNC = PIN_OV_VSYNC;


/** all the pins init state*/
const Pin pinInitA = {0x200802, PIOA, ID_PIOA, PIO_INPUT, PIO_PULLUP};
const Pin pinInitB = {0xd8, PIOB, ID_PIOB, PIO_INPUT, PIO_PULLUP};
const Pin pinInitC = {0x03600, PIOC, ID_PIOC, PIO_INPUT, PIO_PULLUP};

#ifdef STATE_STAYED
//do not configure PA5 pin because it is use for duration test in State_stayed
const Pin pinInitAOut = {0xffbff7db, PIOA, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT};
#else
const Pin pinInitAOut = {0xffbff7fb, PIOA, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT};
#endif

const Pin pinInitBOut = {0xffffff27, PIOB, ID_PIOB, PIO_OUTPUT_0, PIO_DEFAULT};
const Pin pinInitCOut = {0xffffc9ff, PIOC, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT};

/** current state*/
volatile STATE_TYPE cur_state = STATE_INIT;

bool bIsMotionDetected = false;
/*----------------------------------------------------------------------------
 *        Local functions
 *----------------------------------------------------------------------------*/

#ifdef STATE_STAYED
static __ramfunc
void _WaitForState(uint32_t state)
{
    while(cur_state != state);
}
#endif


void Display_info(uint32_t state)
{
   switch (state)
   {
      case STATE_WAIT:
        TimeTick_Configure( (DEFAULT_MCK_FROM_RC) );
        _Init_LCD();
        LCDD_Fill(COLOR_TURQUOISE);
        LCDD_DrawString(0,10,"Initialization done",COLOR_BLACK);
        LCDD_DrawString(0,40,"Press BP1 to enter\nin wait mode",COLOR_BLACK);
      break;

      case STATE_SLEEP_ENTER:
        LCDD_Fill(COLOR_TURQUOISE);
        LCDD_DrawString(0,10,"Out of wait mode",COLOR_BLACK);
        LCDD_DrawString(0,40,"Press BP1 to enter\nin sleep mode",COLOR_BLACK);
      break;

      case STATE_SLEEP_EXIT:
          LCDD_Fill(COLOR_TURQUOISE);
          LCDD_DrawString(0,10,"Out of sleep mode",COLOR_BLACK);
          LCDD_DrawString(0,40,"Press BP1 to go at\nthe next state",COLOR_BLACK);
      break;

      case STATE_ACTIVE_PLL:
           LCDD_Fill(COLOR_TURQUOISE);
          if(bIsMotionDetected)
          {
            LCDD_DrawString(0,10,"motion detected",COLOR_BLACK);
          }
          else
          {
            LCDD_DrawString(0,10,"No motion detected",COLOR_BLACK);
          }
          LCDD_DrawString(0,40,"Press BP1 to go at\nthe next state",COLOR_BLACK);
      break;

      case STATE_ACTIVE_TRANSFER_SETUP:
          LCDD_Fill(COLOR_TURQUOISE);
          LCDD_DrawString(0,10,"switch MCK to PLL\ndone",COLOR_BLACK);
          LCDD_DrawString(0,50,"Press BP1 to go at\nthe next state",COLOR_BLACK);
      break;

      case STATE_SLEEP_CAPTURE:
          LCDD_Fill(COLOR_TURQUOISE);
          LCDD_DrawString(0,10,"Capture initialized",COLOR_BLACK);
          LCDD_DrawString(0,40,"Press BP1 to\ncapture and\ndisplay a picture",COLOR_BLACK);
      break;

      default:
         //do nothing
      break;
   }
}

/** Initialize a test pin for duration measurement*/
static void _TestPin_Init(void)
{
#ifdef STATE_STAYED
    PIO_Configure(&pinTest,1);
#endif
}

/**
 * \brief Set default master access for speed up.
 * Here assume code is put at flash, data is put at sram.
 * The PDC transfer target at SRAM
 */
static void _SetDefaultMaster( void )
{
    Matrix *pMatrix = MATRIX;

    /* Set default master: SRAM (slave 0)-> Cortex-M4 System (Master 1)*/
    pMatrix->MATRIX_SCFG[0] |= ((1 << 18) & MATRIX_SCFG_FIXED_DEFMSTR_Msk) | /* Master 1 */
                               ((2 << 16) & MATRIX_SCFG_DEFMSTR_TYPE_Msk);   /* Fixed Default Master */

    /* Set default master: Internal flash (slave 2) -> Cortex-M4 Instruction/Data (Master 0)*/
    pMatrix->MATRIX_SCFG[2] |= ((0 << 18) & MATRIX_SCFG_FIXED_DEFMSTR_Msk) | /* Master 0 */
                               ((2 << 16) & MATRIX_SCFG_DEFMSTR_TYPE_Msk);   /* Fixed Default Master */

    /* Set default master: EBI (slave 3) -> PDC (Master 2)*/
    pMatrix->MATRIX_SCFG[3] |= ((2 << 18) & MATRIX_SCFG_FIXED_DEFMSTR_Msk) | /* Master 2 */
                               ((2 << 16) & MATRIX_SCFG_DEFMSTR_TYPE_Msk);   /* Fixed Default Master */
}


/**
 * \brief Initialize the chip.
 */
static void _InitChip( void )
{
    /* Disable all the peripheral clocks */
  PMC_DisableAllPeripherals();

    /* Disable USB Clock */
    REG_PMC_SCDR = PMC_SCER_UDP;

    Pin pinsInitAllInput[]={ pinInitA, pinInitB, pinInitC };
    Pin pinsInitAllOutput[]={ pinInitAOut, pinInitBOut, pinInitCOut };

    /* Configure the following PIOs as input
          pa2 - USER_BP
          pa11 - SPI_NPCS0
          pa22 - NCS2_LCD
          pb3 - TSC_IRQ
          pb4 - ICE_TDI
          pb6 - ICE_TMS
          pb7 - ICE_TCK
          pc9 - SW_PSRAM
          pc12 - USER_LED
          pc13 - LCD_BL
          pc16 - SW_OVT
      */
    PIO_Configure(pinsInitAllInput,PIO_LISTSIZE(pinsInitAllInput));

    /* Configure the other PIOs as output */
    PIO_Configure(pinsInitAllOutput,PIO_LISTSIZE(pinsInitAllOutput));

    PMC_DisablePeripheral(ID_PIOA);
    PMC_DisablePeripheral(ID_PIOB);
    PMC_DisablePeripheral(ID_PIOC);
}

/**
 * \brief Enter Wait Mode.
 * Enter condition: (WAITMODE bit = 0) + (SLEEPDEEP bit = 0) + (LPM bit = 1)
 */
static void _EnterWaitMode( void )
{
     /* Disable Brownout Detector */
    SUPC->SUPC_MR |= (uint32_t)(0xA5 << 24) | (0x01 << 13) ;

    /* enable rtt alarm wakeup and low power mode*/
    PMC->PMC_FSMR |= (PMC_FSMR_LPM |PMC_FSMR_RTTAL) ;

    /* clear DEEPSLEEP bit*/
    SCB->SCR &= (uint32_t)~SCB_SCR_SLEEPDEEP_Msk ;

    /* set 200 alarm*/
#ifdef STATE_STAYED
    /* 2000ms is for normal time/power measurement. */
    /* Change to 2ms for sleep mode time measurement: (measured time - 1.953ms) */
    RTT_SetMsAlarm(2000);
#else
    RTT_SetMsAlarm(200);
#endif

    /* clear status*/
    RTT_ClearAlarm();

    /* restart rtt*/
    RTT_Restart();

    /* enter wait mode*/
    PMC->CKGR_MOR |= (CKGR_MOR_KEY(0x37) | CKGR_MOR_WAITMODE);
    while( !(PMC->PMC_SR & PMC_SR_MCKRDY) );

    while ( !(PMC->CKGR_MOR & CKGR_MOR_MOSCRCEN) ) ;
}

/** entery for capturing*/
static void _DoCapture(void)
{
    uint8_t *buf;

    SysTick->CTRL &= ~(SysTick_CTRL_ENABLE_Msk);

    /* set capturing destination address*/
    cap_dest_buf = (uint8_t*)CAP_DEST;

    buf = cap_dest_buf;

    cap_rows = IMAGE_HEIGHT;

    /* sync with flag */
    while(!vsync_flag)
    {
     /* enable vsync interrupt */
     PIO_EnableIt(&pinVSYNC);

     /* enter sleep */
     EnterSleepMode();

     /* disable first */
     PIO_DisableIt(&pinVSYNC);
    }

    PIO_Capture_Switch(PIOA,ON);

    /* only using Vsync*/
    PIO_CaptureToBuffer(PIOA,buf, (cap_line * cap_rows)>>2);

    /* enable interrupt to wakeup the core  */
    PIOA->PIO_PCIER |= PIO_PCIER_RXBUFF;

    /* enter sleep*/
    EnterSleepMode();
    while(!PIO_Capture_BUFF(PIOA));

    PIO_Capture_Switch(PIOA,OFF);
    /* clear vsync flag*/
    vsync_flag = false;
}

/** intialize LCD for debugging output*/
static void _Init_LCD(void)
{
    /* for lcd display, avoid interference from other ios*/
    /*
         Disable pull-up
         PC9 : SW_SRAM
         PC12: USER_LED
         PC16: SW_OVT
      */
    PIO_PullUp_Disable(PIOC, 9);
    PIO_PullUp_Disable(PIOC, 12);
    PIO_PullUp_Disable(PIOC, 16);

    /*
         Enable pad pull-down
         PC9 : SW_SRAM
         PC12: USER_LED
         PC16: SW_OVT
      */
    PIO_PullDown_Enable(PIOC, 9);
    PIO_PullDown_Enable(PIOC, 12);
    PIO_PullDown_Enable(PIOC, 16);

    #ifdef STATE_STAYED
   //do not configure PA5 pin because it is use for duration test in State_stayed
    PIO_Output_Disable_Mask(PIOA, 0xffffffdf);
    #else
    PIO_Output_Disable_Mask(PIOA, 0xffffffff);
    #endif


    PIO_Output_Disable_Mask(PIOB, 0xffff);
    PIO_Output_Disable_Mask(PIOC, 0xffffffff);

    /* Initialize LCD */
    LCDD_Initialize();
    LCD_On();
}

#ifdef LCD_DEBUG_OUT
static void _DrawFrame_YUV_BW8( void )
{
    volatile uint32_t dwCursor ;
    uint8_t *pucData ;

    pucData=(uint8_t*)cap_dest_buf ;

    LCD_SetDisplayLandscape(0);

    LCD_SetWindow(0,0,IMAGE_HEIGHT,IMAGE_WIDTH);
    LCD_SetCursor(0,0) ;
    LCD_WriteRAM_Prepare() ;
    for ( dwCursor=IMAGE_WIDTH*IMAGE_HEIGHT ; dwCursor != 0 ; dwCursor-- ,pucData++)
    {
        // Black and White using Y
        LCD_WriteRAMByte( *pucData ) ;
        LCD_WriteRAMByte( *pucData ) ;
        LCD_WriteRAMByte( *pucData ) ;

    }
}

/** normalization function*/
static inline uint8_t _clip( int32_t i )
{
    if ( i > 255 )
    {
        return 255 ;
    }
    if ( i < 0 )
    {
        return 0 ;
    }

    return (uint8_t)i ;
}

/** function for Draw color yuv data*/
static void _DrawFrame_YUV_ColorInt( void )
{
    uint32_t dwCursor ;
    int32_t C ;
    int32_t D ;
    int32_t E ;
    uint8_t* pucData ;

    pucData=(uint8_t*)cap_dest_buf ;

    LCD_SetDisplayLandscape(0);
    LCD_SetWindow( 0, 0,IMAGE_HEIGHT ,IMAGE_WIDTH  ) ;

    LCD_SetCursor( 0,0 ) ;
    LCD_WriteRAM_Prepare() ;
    for ( dwCursor=IMAGE_WIDTH*IMAGE_HEIGHT ; dwCursor != 0 ; dwCursor-=2, pucData+=4 )
    {
        C=pucData[0] ; // Y1
        C-=16 ;
        D=pucData[3] ; // U
        D-=128 ;
        E=pucData[1] ; // V
        E-=128 ;
        // BLUE
         LCD_WriteRAMByte( _clip(( 298 * C + 516 * D           + 128) >> 8) ) ;
        // GREEN
        LCD_WriteRAMByte( _clip(( 298 * C - 100 * D - 208 * E + 128) >> 8) ) ;
        // RED
        LCD_WriteRAMByte( _clip(( 298 * C           + 409 * E + 128) >> 8) ) ;

        C=pucData[2] ; // Y2
        C-=16 ;
        LCD_WriteRAMByte( _clip(( 298 * C + 516 * D           + 128) >> 8) ) ;
        LCD_WriteRAMByte( _clip(( 298 * C - 100 * D - 208 * E + 128) >> 8) ) ;
        LCD_WriteRAMByte( _clip(( 298 * C           + 409 * E + 128) >> 8) ) ;

    }
}

static void _Display(void)
{
    if(out_mode == PIO_OUT_COLOR)
    {
        _DrawFrame_YUV_ColorInt();
    }
    else
    {
        _DrawFrame_YUV_BW8();
    }
}
#endif

/** turn on or off image sensor power*/
static void _Turn_ImageSensor(bool on)
{
    Pin pinsPower[]={ PINS_OV };
    PIO_Configure(pinsPower,PIO_LISTSIZE(pinsPower));

    if(on)
    {
        PIO_Clear(&pinsPower[0]);
    }
    else
    {
        PIO_Set(&pinsPower[0]);
    }

}

/** Initialize Image Sensor*/
static void _Init_Image_Sensor(const EOV7740_Format eFormat)
{
     /* flag for intialization first time*/
    static bool bInitialized = false;
    /* backup setting for future use*/
    static uint32_t bk_setting[2]={0};
    /* intitializ Master Clock to xvclk1*/
    Pin pinPCK = PIN_PCK0;
    const Pin pinsTWI[] = {PINS_TWI0};
    Pmc *pmc = (Pmc*) PMC;
    /* Power on*/
    _Turn_ImageSensor(true);

    PIO_Configure(&pinPCK,1);

    /* PLLA is 96MHz so that PCK0 is 96MHz/4 = 24MHz*/
    pmc->PMC_PCK[0] = (PMC_PCK_PRES_DIV4 | PMC_PCK_CSS_PLLACLOCK);

    pmc->PMC_SCER = PMC_SCER_PCK0;

    while(!( pmc->PMC_SCSR & PMC_SCSR_PCK0));
    Wait(5);

    /* twi*/
    PIO_Configure(pinsTWI,PIO_LISTSIZE(pinsTWI));

    PMC_EnablePeripheral(ID_TWI0);

    TWI_ConfigureMaster(TWI0, TWCK, BOARD_MCK);

    TWID_Initialize(&twid, TWI0);	

    /* Configure TWI interrupts */
    NVIC_DisableIRQ(TWI0_IRQn);
    NVIC_ClearPendingIRQ(TWI0_IRQn);
    NVIC_SetPriority(TWI0_IRQn, 0);
    NVIC_EnableIRQ(TWI0_IRQn);

    while( ov_init(&twid) == 0 )
    {
        printf("-I- Retry init\n\r");
    }
    printf("-I- Init passed\n\r");

    /* OV7740 configuration */
    ov_configure(&twid, eFormat);

    /* first time*/
    if(!bInitialized)
    {
        Wait(3000);
        ov_sotre_manual(&twid,bk_setting,2);
        bInitialized = true;
    }
    else
    {
        ov_resotre_manual(bk_setting,2);
        ov_configure_manual(&twid);
    }
}


/**
 *  \brief Handler for Button 1 rising edge interrupt.
 *
 *  Handle state transition.
 */
static void _Button1_Handler( const Pin* pin )
{
  switch (cur_state)
  {
    case  STATE_INIT:
      cur_state = STATE_START;
      break;
    case  STATE_START:
      cur_state = STATE_WAIT;
      break;
    case  STATE_WAIT:
      cur_state = STATE_SLEEP_ENTER;
      break;
    case  STATE_SLEEP_ENTER:
      cur_state = STATE_SLEEP_EXIT;
      break;
    case  STATE_SLEEP_EXIT:
      cur_state = STATE_ACTIVE_PLL;
      break;
    case STATE_ACTIVE_PLL:
      cur_state = STATE_ACTIVE_TRANSFER_SETUP;
    break;
    case STATE_ACTIVE_TRANSFER_SETUP:
      cur_state = STATE_SLEEP_CAPTURE;
      break;
    case STATE_SLEEP_CAPTURE:
      cur_state = STATE_START;
      break;
    default:
      cur_state = STATE_END;
      break;
  }
}

/** interrupt handler for vsync*/
static void _Vsync_Handler(const Pin *pin)
{
    static bool bFirst = true;
    if(!bFirst)
    {
        vsync_flag = true;
        bFirst = true;
    }
    else
    {
        bFirst = false;
    }
}

/** intialize fram marker signal response*/
static void _Init_HVSync_Interrupts(void)
{
    static bool initialized = false;

    PIO_Configure(&pinVSYNC,1);

    if(!initialized)
    {
        /* this function can't be reentried, just call once*/
       PIO_ConfigureIt(&pinVSYNC, _Vsync_Handler);
       initialized = true;
    }
}

/** initialize pushbutton for state transition*/
static void _Init_Pushbutton_Trigger(void)
{
    static bool initialized = false;

    /* Configure pios as inputs. */
    PIO_Configure( &pinBP1, 1 ) ;

    /* Clearing prior input change statu*/
    pinBP1.pio->PIO_ISR;

    /* Adjust pio debounce filter patameters, uses 10 Hz filter. */
    PIO_SetDebounceFilter( &pinBP1, 10 ) ;

    /* Initialize pios interrupt handlers, see PIO definition in board.h. */
    if(!initialized)
    {
        PIO_ConfigureIt( &pinBP1, _Button1_Handler ) ; /* Interrupt on rising edge  */
        initialized = true;
    }

    /* Enable PIO controller IRQs. */
    NVIC_EnableIRQ( (IRQn_Type)pinBP1.id ) ;

    /* Enable PIO line interrupts. */
    PIO_EnableIt( &pinBP1 ) ;
}

/** Initialziation of SRAM, PIO capture, VSynch handler,Image sensor*/
static void _Capture_Init(void)
{
    /* SET ios to default input*/
    PIO_Output_Disable_Mask(PIOA, 0xffbff7db);

    /* OUTPUT color mode*/
#ifdef DEFAULT_MODE_COLORED
    out_mode = PIO_OUT_COLOR;
#else
    out_mode = PIO_OUT_MONO;
#endif
    /* configure SRAM*/
    BOARD_ConfigureSRAM(SMC) ;

    /* intialize Frame signal*/
    _Init_HVSync_Interrupts();

    /* initialize PIODC*/
    PIODC_Capture_Init(PIOA, ID_PIOA, out_mode);

    /** intialize capturing line*/
    if(out_mode == PIO_OUT_COLOR)
    {
        cap_line = IMAGE_LINE_COLORED;
    }
    else if(out_mode == PIO_OUT_MONO)
    {
        cap_line = IMAGE_LINE_MONO;
    }
    else
    {
        cap_line = IMAGE_LINE_COLORED;
    }

#ifdef DEFAULT_SENSOR_YUV422
    _Init_Image_Sensor(QVGA_YUV422);
#endif
}

/** cofigure ad0,ad4,ad5 as analog input*/
static void _PIR_Init(void)
{
    /* set AD0(pa17),AD4(pb0),AD5(pb1) as anaalog input*/
    ADC_EnableChannel(ADC, 0);
    ADC_EnableChannel(ADC, 4);
    ADC_EnableChannel(ADC, 5);
}

/** Capture Process, including clock switching, capture intialization*/
static void _Capture_Process(void)
{
   /* initialize systick for timing function*/
   TimeTick_Configure( (4000000) );

#ifdef LCD_DEBUG_OUT

    SysTick->CTRL &= ~(SysTick_CTRL_ENABLE_Msk);

#endif

    WAIT_FOR_STATE(STATE_ACTIVE_PLL);

    /* State: Enter Active Mode with PLL Enabled */
    /* switch MCK to PLL*/
    SwitchMck2PLL();

    /* reinitialize with mck*/
    TimeTick_Configure( (BOARD_MCK) );

    WAIT_FOR_STATE(STATE_ACTIVE_TRANSFER_SETUP);

    /* State: Capture */

    /*initizalize image sensor, piodc*/
   _Capture_Init();

    WAIT_FOR_STATE(STATE_SLEEP_CAPTURE);
    /* Enable PIO controller IRQs. */
    NVIC_EnableIRQ(PIOA_IRQn);
    /* doing capture*/
    _DoCapture();

    /* State: Display (Note: valid only when LCD_DEBUG_OUT is uncommented)*/

#ifdef LCD_DEBUG_OUT
    _Display();
#endif

    WAIT_FOR_STATE(STATE_START);

    NVIC_DisableIRQ(PIOA_IRQn);

    SwitchMck2FastRC();

}

/**
  * preparation for entering wait mode
  */
static void _Wait_Prepare(void)
{
    /* intialize chip to avoid leakage*/
    _InitChip();
    _PIR_Init();
}

/*----------------------------------------------------------------------------
 *        Exported functions
 *----------------------------------------------------------------------------*/


/** system systick handler*/
void SysTick_Handler( void )
{
    TimeTick_Increment() ;
}

/** PIO capture handler*/
extern void PIO_CaptureHandler( void )
{
    PIOA->PIO_PCIDR |= PIO_PCIDR_RXBUFF;
}

/** The main entry of the demo*/
extern int main( void )
{
    bool bIsTimeout = false;

    /* Disable watchdog */
    WDT_Disable( WDT ) ;

#ifdef STATE_STAYED
    volatile uint32_t timeout = 0;
    /* Select external slow clock for accuracy time measurement */
    if ((SUPC->SUPC_SR & SUPC_SR_OSCSEL) != SUPC_SR_OSCSEL_CRYST)
    {
        SUPC->SUPC_CR = SUPC_CR_XTALSEL_CRYSTAL_SEL | ((uint32_t)0xA5 << 24);
        timeout = 0;
        while (!(SUPC->SUPC_SR & SUPC_SR_OSCSEL_CRYST) );
    }
      #if defined(LCD_DEBUG_OUT) && !defined(LCD_DISPLAY_INFORMATION)
        TimeTick_Configure( (DEFAULT_MCK_FROM_RC) );
        Wait(1000); // wait for 1s to be sure LCD is ready for initialization
        _Init_LCD();
        LCDD_Fill(COLOR_TURQUOISE);
        LCDD_DrawString(0,10,"Periodic motion\ndetect demo",COLOR_BLACK);
        LCDD_DrawString( 0, 50, "Step by Step mode", COLOR_BLACK ) ;
      #endif
#else
      #ifdef LCD_DEBUG_OUT
        TimeTick_Configure( (DEFAULT_MCK_FROM_RC) );
        _Init_LCD();
        LCDD_Fill(COLOR_TURQUOISE);
        LCDD_DrawString(0,10,"Periodic motion\ndetect demo",COLOR_BLACK);
        LCDD_DrawString( 0, 60,"Please Wait during\ninitializations", COLOR_BLACK ) ;
      #endif
#endif

    _SetDefaultMaster();
    while(1)
    {
        /* prepare for entering wait mode*/
        _Wait_Prepare();

#ifdef STATE_STAYED
        /* for duration test*/
        _TestPin_Init();
       /* intialize pushbutton for state transition*/
        _Init_Pushbutton_Trigger();
        cur_state = STATE_START;
#endif

      WAIT_FOR_STATE(STATE_WAIT);

        /* State: Initialize M ms RTT alarm
                           Enter Wait Mode
             */

        _EnterWaitMode();

        /* State: Enter Active Mode without PLL
                           Initialize Motion Detect and a N ms Timeout Interval
            */
        /* initalize Timeout tick, ACC for motion detect*/

        WAIT_FOR_STATE(STATE_SLEEP_ENTER);

        MD_Init();
#ifdef STATE_STAYED
        RTT_AlarmTimeout_Init(2000);
#else
        RTT_AlarmTimeout_Init(500);
#endif
        /* enable motion detection and alarm*/
        ACC->ACC_ISR;
        MD_Enabled();
        RTT_AlarmTimeout_Enabled();
        SysTick->CTRL &= ~(SysTick_CTRL_ENABLE_Msk);
        /* State: Enter Sleep Mode */

        /* enter sleep mode*/
        EnterSleepMode();

        WAIT_FOR_STATE(STATE_SLEEP_EXIT);

        /* State: Motion Detected or Timeout Interval Expired */

        if( (bIsMotionDetected = MD_IsDetected()) || ( bIsTimeout = RTT_IsAlarmed()))
        {
            /* State: Enter Active Mode without PLL */
            /* disable motion detector and alarm*/
            MD_Disabled();
            RTT_AlarmTimeout_Disabled();
            ACC_DisableIt(ACC,ACC_IDR_CE);
            RTT_ResetAlarm();

            if(bIsMotionDetected)
            {
                /* State: Yes for IsMotionDetected and !(IsIntervalExpired) */

                /* capture then fall asleep*/
                SysTick->CTRL |= (SysTick_CTRL_ENABLE_Msk);
                _Capture_Process();
            }
            else
            {
                 WAIT_FOR_STATE(STATE_ACTIVE_PLL);
            }
        }
    }
}


