/* ----------------------------------------------------------------------------
*         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.
* ----------------------------------------------------------------------------
*/
/*----------------------------------------------------------------------------
*        Headers
*----------------------------------------------------------------------------*/

#include "board.h"

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


/*----------------------------------------------------------------------------
*        Local definitions
*----------------------------------------------------------------------------*/
/*macro to measure Frame rate, comment if it's not used*/
//#define FRAME_RATE_MEASURE

#define IMAGE_WIDTH     (320UL)
#define IMAGE_HEIGHT    (240UL)
#define IMAGE_COLOR_COMPONENTS     (3UL) /* YUV */
#define IMAGE_BPP            (2UL)       /* Each pixel is either YU or YV */
#define IMAGE_SIZE_QVGA (IMAGE_WIDTH*IMAGE_HEIGHT*IMAGE_BPP)

/** TWI clock frequency in Hz (100KHz) */
#define TWCK            100000

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

/* For Debug Purpose - use to measure time of a function - avoid to use the systick */
volatile float cyc[2];
unsigned int cnt;
volatile unsigned int *DWT_CYCCNT = (volatile unsigned int *)0xE0001004; //address of the register
volatile unsigned int *DWT_CONTROL = (volatile unsigned int *)0xE0001000; //address of the register
volatile unsigned int *SCB_DEMCR = (volatile unsigned int *)0xE000EDFC; //address of the register
  // Use macros around test code
#define STOPWATCH_START { cyc[0] = *DWT_CYCCNT; }
#define STOPWATCH_STOP { cyc[1] = *DWT_CYCCNT; cyc[1] = (float)(8e-9*(cyc[1] - cyc[0])); }
/* 8e-9 is for processor clock at 120MHz */
/*----------------------------------------------------------------------------
*        Local variables
*----------------------------------------------------------------------------*/

// Buffer for raw image capture from the image sensor in YUV422 format.
// Whole image capture (320x240x2 = 153600 bytes) is done in 3 capture

uint8_t CaptureBuffer_1[51200];     // First buffer for image capture from image Sensor
uint8_t CaptureBuffer_2[51200];     // Second buffer for image capture from image Sensor


// Misc variables for application

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

/** HSYNC flag RESERVED*/
bool hsync_flag = false;

/** TWI driver*/
static Twid twid;

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

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

/** Push button 1*/
const Pin pinBP1 = PIN_PUSHBUTTON_1;
volatile unsigned int Next = 0 ;
volatile unsigned int EndCapture = 0 ;


#define IMAGE_LINE_COLORED    (IMAGE_WIDTH * 2)
#define IMAGE_LINE_MONO  (IMAGE_WIDTH)
                             /** The output formats supported by the sensor*/
typedef enum {
    OUT_RGB,
    OUT_YUV
}OUT_FMT;


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

/** cap line length*/
uint16_t cap_line = IMAGE_LINE_COLORED;
uint32_t first_display_finished = 0;
/*----------------------------------------------------------------------------
*        Local functions
*----------------------------------------------------------------------------*/
/** intialize LCD for debugging output*/
static void _Init_LCD(void)
{

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

static void _DrawFrame_YUV_ColorInt_by_chunk( uint8_t* CapDataBuffer )
{
   uint32_t dwCursor ;
    int32_t C ;
    int32_t D ;
    int32_t E ;

    int32_t dw1_1 ;
    int32_t dw1_1C1 =516, dw1_1C2=128;

    int32_t dw1_2 ;
    int32_t dw1_2C1 =-100, dw1_2C2=208;

    int32_t dw1_3 ;
    int32_t dw1_3C1 =409 ;
    int32_t clip_c1 =298 ;
    uint8_t* pucData=CapDataBuffer;
    uint8_t*  LcdData = (unsigned char*)0x62000002 ;

    if (first_display_finished==0)
    {
      LCD_SetDisplayLandscape(0);
      LCD_SetWindow( 0, 0, IMAGE_HEIGHT ,320 ) ;
      LCD_SetCursor( 0,0 ) ;
      LCD_WriteRAM_Prepare() ;

    }


    for ( dwCursor=(IMAGE_WIDTH*IMAGE_HEIGHT/3 ) ; dwCursor != 0 ; dwCursor-=2, pucData+=4 )
    {
        C=pucData[0] ; // Y
        C-=16 ;
        D=pucData[3] ; // U
        D-=128 ;
        E=pucData[1] ; // V
        E-=128 ;


        // 32 ?(32 x 32) = 32 MLA, MLS
        dw1_1=dw1_1C1*D+dw1_1C2 ;

        dw1_2= dw1_2C1*D-dw1_2C2*E+dw1_1C2;

        // 32 ?(32 x 32) = 32 MLA, MLS
        dw1_3=dw1_3C1*E+dw1_1C2 ;

        // BLUE
        *LcdData = __USAT( ( clip_c1 * C + dw1_1 ) >> 8, 8);

        // GREEN
        *LcdData = __USAT( ( clip_c1 * C + dw1_2 ) >> 8, 8);

        // RED
        *LcdData = __USAT( ( clip_c1 * C + dw1_3 ) >> 8, 8);

        C=pucData[2] ; // Y2
        C-=16 ;

        *LcdData = __USAT( ( clip_c1 * C + dw1_1 ) >> 8, 8);

        *LcdData = __USAT( ( clip_c1 * C + dw1_2 ) >> 8, 8);

        *LcdData = __USAT( ( clip_c1 * C + dw1_3 ) >> 8, 8);

    }

}


/** switch pio capturing on or off*/
static void _PIO_Capture_Switch(Pio *pio,bool on_off)
{
    if(on_off)
    {
        pio->PIO_PCMR |= PIO_PCMR_PCEN;
    }
    else
    {
        pio->PIO_PCMR &= (~(uint32_t)PIO_PCMR_PCEN);
    }
}

/** initialize PIO parallel capture function*/
static void _Init_PIODC_Capture(Pio *pio)
{
    /* enable periphral clock*/
    PMC_EnablePeripheral(ID_PIOA);

    /* disable pio capture*/
    pio->PIO_PCMR &= ~((uint32_t)PIO_PCMR_PCEN);

    /* Set the PIO Capture data width to concantenate 4 bytes then xfer to mem */
    pio->PIO_PCMR &= ~((uint32_t)PIO_PCMR_DSIZE_Msk);
    // 32-bit data transfer from PDC to Memory
    pio->PIO_PCMR |= PIO_PCMR_DSIZE(2);

    /* only HSYNC and VSYNC enabled*/
    pio->PIO_PCMR &= ~((uint32_t)PIO_PCMR_ALWYS);
    pio->PIO_PCMR &= ~((uint32_t)PIO_PCMR_HALFS);


}

/** Init PDC Receive pointer/counter and Next Received pointer/counter */
static uint8_t _PIO_CaptureToBuffer(Pio *pio, uint8_t *buf, uint8_t *nextbuf , uint32_t size)
{
        pio->PIO_RPR = (uint32_t)buf;
        pio->PIO_RCR = size/4;

        pio->PIO_RNPR = (uint32_t)nextbuf;
        pio->PIO_RNCR = size/4;

        // Enable channel
        pio->PIO_PTCR = PIO_PTCR_RXTEN;

        return(0);
}

/** turn on or off image sensor power*/
static void _ImageSensor_Switch(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*/
    _ImageSensor_Switch(ON);

    /* configure Master Clock*/
    PIO_Configure(&pinPCK,1);

    /* Use PLLA @ 96MHz / 4 for 24MHz on image sensor */
    /* The sensor frame rate is then initialized @ 15 fps in OV7740_QVGA_YUV422_15FPS Structure */
    pmc->PMC_PCK[0] = (PMC_PCK_PRES_DIV4 | PMC_PCK_CSS_PLLACLOCK);

    /* enable output clock on PIO pin */
    pmc->PMC_SCER = PMC_SCER_PCK0;

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

    /* Configure the Two wire interface (I2C) to set the sensor */
    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);

    /* Read OV Sensor ID for communication cheking */
    while (ov_init(&twid) == 0) {};

      /* Configure Sensor in QVGA YUV422 and Automatic Exposure and Gain Control
      / enabled (AEC and GC mode) */
      ov_configure(&twid, QVGA_YUV422_30FPS);

      /* first time initialization after AEC */
      if(!bInitialized)
      {
        /* Wait 3 seconds to let the image sensor to adapt to environment */
        Wait(3000);
        ov_sotre_manual(&twid,bk_setting,2);
        bInitialized = true;
      }
      else
      {
        /* Use the previously saved Automatic Exposure and Gain Control value
        // from first startup */
        ov_resotre_manual(bk_setting,2);
        ov_configure_manual(&twid);
        Wait(100);
      }
}


static void _Capture_Init(void)
{

    PIO_Configure(&pinVSYNC, 1);
    /* initialize PIODC*/
    _Init_PIODC_Capture(PIOA);
    _Init_Image_Sensor(QVGA_YUV422_30FPS);
}

/** check for capture done*/
static bool Wait_PIO_Capture_ENDRX(Pio *pio)
{
    return ((pio->PIO_PCISR & PIO_PCIMR_ENDRX) == PIO_PCIMR_ENDRX)?true:false;
}



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

/** system systick handler*/
#ifdef __cplusplus
 extern "C" {
#endif
void SysTick_Handler( void )
{
    TimeTick_Increment() ;
}
#ifdef __cplusplus
}
#endif
/**
*  \brief Continuous_Capture_Mode Application entry point.
*
*  \return Unused (ANSI-C compatibility).
*  \callgraph
*/
extern int main( void )
{
    /* Disable watchdkog */
    WDT_Disable( WDT ) ;
		
	/* initialize 1ms systick*/
    TimeTick_Configure(BOARD_MCK);
    Wait(1000); // wait for 1s to be sure LCD is ready for initialization

    /*  For Measuring time for Debug - Set up once */
    *SCB_DEMCR = *SCB_DEMCR | 0x01000000;
    *DWT_CYCCNT = 0;                    // reset the counter
    *DWT_CONTROL = *DWT_CONTROL | 1 ;   // enable the counter

    /* LCD for display*/
    _Init_LCD();

    LCDD_Fill( COLOR_TURQUOISE ) ;

    /* Display some information on the LCD */
    LCDD_DrawString(0,10,"Continuous_Capture\ndemo\n(without Ext SRAM)",COLOR_BLACK);	
    LCDD_DrawString( 0, 80, (uint8_t *)"Please Wait during\ninitializations", COLOR_BLACK ) ;

#ifdef FRAME_RATE_MEASURE
    PIO_Configure(&pinTest,1);
#endif

    /* initialize image sensor and PIO parallel capture  */
    _Capture_Init();

    while (1)
    {

    /* Wait the VSync signal to start capture */
    while (1 == PIO_Get(&pinVSYNC));

// For debug purpose to start the stopwatch
STOPWATCH_START
	
    /* Configure the PDC Buffer and Next Buffer for image capture */
    _PIO_CaptureToBuffer(PIOA,CaptureBuffer_1, CaptureBuffer_2, sizeof(CaptureBuffer_1) );

    /* Enable PIO Capture */
    _PIO_Capture_Switch(PIOA,ON);

    /***************************************************/
    /* Wait CaptureBuffer_1  received on ENDRX PDC IRQ */
    /***************************************************/
    while(!Wait_PIO_Capture_ENDRX(PIOA));

    // Reload Next Buffer
    PIOA->PIO_RNPR = (uint32_t)CaptureBuffer_1;
    // Size of buffer is /4 due to 32-bit word transfer
    PIOA->PIO_RNCR = sizeof(CaptureBuffer_1)/4 ;

    /**********************************************************************/
    /* Display the first Captured Buffer while receiving CaptureBuffer_2  */
    /*                                                                    */
    /**********************************************************************/
#ifdef FRAME_RATE_MEASURE
    PIO_Set(&pinTest);
#endif

    _DrawFrame_YUV_ColorInt_by_chunk( CaptureBuffer_1 );

    first_display_finished = 1 ;

    /************************************************************/
    /* Wait one more capture of CaptureBuffer_1 to be  received */
    /************************************************************/
    while(!Wait_PIO_Capture_ENDRX(PIOA));

    /***********************************/
    /* Display the 2 remaining buffers */
    /***********************************/
    _DrawFrame_YUV_ColorInt_by_chunk( CaptureBuffer_2 );
    _DrawFrame_YUV_ColorInt_by_chunk( CaptureBuffer_1 );

    // For debug purpose to stop the stopwatch the value of cyc[1] will give
    // the number of ms for capturing one frame and display it
STOPWATCH_STOP

    first_display_finished = 0 ;

#ifdef FRAME_RATE_MEASURE
    PIO_Clear(&pinTest);
#endif
    }


}
