/*
 *  for MB9BF618T on FRK-FM3
 * 
 *   Copyright (C) by Y.Ishioka.
 */
#include <basic.h>
#include <tk/tkernel.h>
#include <libstr.h>
#include <stdio.h>

#include <drivers/lcd_char.h>
#include <drivers/uart.h>
#include <drivers/sensor.h>
#include <drivers/led.h>
#include <drivers/io.h>
#include <drivers/adc.h>

#include <tk/sysdef.h>

#define  UART_PORT_CAM   0
#define  UART_PORT_PC    1


#define  DEBUG_ADC_COUNTER  0
#define  USE_CAM_PIX_SET    0


#define  SUB_TASK_STACK_SZ  1024
UB  demo_camera_stack[ SUB_TASK_STACK_SZ ] ;
UB  demo_sensor_stack[ SUB_TASK_STACK_SZ ] ;

UB  cam_response_data[ 0x20 ] ;

#define  CAM_READ_BUF_ADR  0x1fff0000
UB  *cam_read_data ;
int  cam_jpeg_size ;

ID  taskc_id ;
ID  tasks_id ;
ID  flag_id ;

#define  FLGPAT_REQ_CAM   0x00000001
#define  FLGPAT_REQ_LGH   0x00000010
#define  FLGPAT_END_CAM   0x00010000
#define  FLGPAT_STA_CAM   0x00100000

#define  CAM_REQ_RESET  4
#define  CAM_RES_RESET  4
#define  CAM_REQ_IMGSZ  9
#define  CAM_RES_IMGSZ  5
#define  CAM_REQ_TAKEPIC  5
#define  CAM_RES_TAKEPIC  5
#define  CAM_REQ_STPTAKE  5
#define  CAM_RES_STPTAKE  5
#define  CAM_REQ_JPEGSZ  5
#define  CAM_RES_JPEGSZ  9
#define  CAM_REQ_RDJPEG  16
#define  CAM_RES_RDJPEG  5

UB  cam_req_reset[CAM_REQ_RESET] = { 0x56, 0x00, 0x26, 0x00 } ;
UB  cam_res_reset[CAM_RES_RESET] = { 0x76, 0x00, 0x26, 0x00 } ;
UB  cam_req_takepic[CAM_REQ_TAKEPIC] = { 0x56, 0x00, 0x36, 0x01, 0x00 } ;
UB  cam_res_takepic[CAM_RES_TAKEPIC] = { 0x76, 0x00, 0x36, 0x00, 0x00 } ;
UB  cam_req_stptake[CAM_REQ_STPTAKE] = { 0x56, 0x00, 0x36, 0x01, 0x03 } ;
UB  cam_res_stptake[CAM_RES_STPTAKE] = { 0x76, 0x00, 0x36, 0x00, 0x00 } ;
UB  cam_req_jpegsz[CAM_REQ_JPEGSZ] = { 0x56, 0x00, 0x34, 0x01, 0x00 } ;
UB  cam_res_jpegsz[CAM_RES_JPEGSZ] = { 0x76, 0x00, 0x34, 0x00, 0x04, 0x00, 0x00, 0xff, 0xff } ;
UB  cam_req_rdjpeg[CAM_REQ_RDJPEG] = {
/*                                                   MH    ML                KH    KL    XX    XX */
   0x56, 0x00, 0x32, 0x0C, 0x00, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0A } ;
UB  cam_res_rdjpeg[CAM_RES_RDJPEG] = { 0x76, 0x00, 0x32, 0x00, 0x00 } ;

/* for 160x120 */
UB  cam_req_imgsz160[CAM_REQ_IMGSZ] = { 0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, 0x22 } ;
/* for 320x240 */
UB  cam_req_imgsz320[CAM_REQ_IMGSZ] = { 0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, 0x11 } ;
UB  cam_res_imgsz[CAM_RES_IMGSZ] = { 0x76, 0x00, 0x31, 0x00, 0x00 } ;

UB  cam_trans_header[] = { "YI" } ;
UB  cam_trans_footer[] = { "Studio" } ;


void  camera_data_trans( int size, UB *data )
{
    UB  *pnt ;
    int  loop ;

    pnt = cam_trans_header ;
    while( *pnt != 0x00 ) {
        uart_send( UART_PORT_PC, *pnt++ ) ;
    }

    uart_send( UART_PORT_PC, (UB)((size>>8)&0xff) ) ; /* size H */
    uart_send( UART_PORT_PC, (UB)(size&0xff) ) ; /* size L */

    for( loop=0 ; loop<size ; loop++ ) {
         uart_send( UART_PORT_PC, *data++ ) ;
//         if( (loop % 80) == 0 ) {
//             tk_slp_tsk( 20 ) ;
//         }
    }
    pnt = cam_trans_footer ;
    while( *pnt != 0x00 ) {
        uart_send( UART_PORT_PC, *pnt++ ) ;
    }
}


void  camera_cmd( int size, UB *pattern )
{
    int  cnt ;

    for( cnt=0 ; cnt<size ; cnt++ ) {
        uart_send( UART_PORT_CAM, *pattern ) ;
        pattern++ ;
    }
}


void  camera_response( int size, UB *pattern )
{
    int  cnt ;
    int  data ;

    for( cnt=0 ; cnt<size ; cnt++ ) {
        while( (data = uart_recv( UART_PORT_CAM )) == -1 ) ;
        if( cnt == 0 && data != (int)0x76 ) {
            while( (data = uart_recv( UART_PORT_CAM )) != (int)0x76 ) ;
        }

        *pattern++ = (UB)data ;
    }

    while( uart_recv( UART_PORT_CAM ) != -1 ) ;
}


void  camera_readjpeg( int size, UB *buff )
{
    int  cnt ;
    int  data ;
    UB  header[10], footer[10], *pnt ;

    pnt = header ;
    for( cnt=0 ; cnt<CAM_RES_RDJPEG ; cnt++ ) {
        while( (data = uart_recv( UART_PORT_CAM )) == -1 ) ;
        if( cnt == 0 && data != (int)0x76 ) {
            while( (data = uart_recv( UART_PORT_CAM )) != (int)0x76 ) ;
        }
        *pnt++ = (UB)data ;
    }

    for( cnt=0 ; cnt<size ; cnt++ ) {
        while( (data = uart_recv( UART_PORT_CAM )) == -1 ) ;
        *buff++ = (UB)data ;
    }

    pnt = footer ;
    for( cnt=0 ; cnt<CAM_RES_RDJPEG ; cnt++ ) {
        while( (data = uart_recv( UART_PORT_CAM )) == -1 ) ;
        if( cnt == 0 && data != (int)0x76 ) {
            while( (data = uart_recv( UART_PORT_CAM )) != (int)0x76 ) ;
        }
        *pnt++ = (UB)data ;
    }
}


/*
 *  CAMERA Initialize
 */
void  camera_init( void )
{
    cam_read_data = (UB *)CAM_READ_BUF_ADR ;

    tk_dly_tsk( 500 ) ;

#if USE_CAM_PIX_SET  /* use pix setting */
#if 1
    /* set 160x120 pix */
    camera_cmd(      CAM_REQ_IMGSZ, cam_req_imgsz160 ) ;
#else
    /* set 320x240 pix */
    camera_cmd(      CAM_REQ_IMGSZ, cam_req_imgsz320 ) ;
#endif
    camera_response( CAM_RES_IMGSZ, cam_response_data ) ;
    tk_dly_tsk( 500 ) ;
#endif

    camera_cmd(      CAM_REQ_RESET, cam_req_reset ) ;
    camera_response( CAM_RES_RESET, cam_response_data ) ;
    tk_dly_tsk( 200 ) ;
}


/*
 *  CAMERA main task
 */
void  demo_camera_task( void )
{
    SYSTIM  pk_tim ;
    UINT  flgptn ;
    UB  rdcmd[CAM_REQ_RDJPEG] ;
    int  loop ;
    char  buff[20] ;

    camera_init() ;

    /* Light OFF */
    io_port_c9(0) ;

    lcd_putstr( (char *)"Start CAM TASK", 0 ) ;

    /* send camera start */
    tk_set_flg ( flag_id, FLGPAT_STA_CAM ) ;

    while( 1 ) {
        /* LED OFF */
        led_set( 0x00000000 ) ;

        tk_wai_flg( flag_id, FLGPAT_REQ_CAM, TWF_ANDW, 
                    &flgptn, TMO_FEVR ) ;

        if( (flgptn & FLGPAT_REQ_LGH) != 0 ) {
            /* Light ON */
            io_port_c9(1) ;
            tk_slp_tsk( 100 ) ;
        }

        /* LED ON */
        led_set( 0x00000001 ) ;

        /* take picture */
        camera_cmd(      CAM_REQ_TAKEPIC, cam_req_takepic ) ;
        camera_response( CAM_RES_TAKEPIC, cam_response_data ) ;

        /* Light OFF */
        io_port_c9(0) ;

        /* read jpag size */
        camera_cmd(      CAM_REQ_JPEGSZ, cam_req_jpegsz ) ;
        camera_response( CAM_RES_JPEGSZ, cam_response_data ) ;
        for( loop=0 ; loop<CAM_REQ_RDJPEG ; loop++ ) {
            rdcmd[loop] = cam_req_rdjpeg[loop] ;
        }
        rdcmd[12] = cam_response_data[7] ;
        rdcmd[13] = cam_response_data[8] ;
        cam_jpeg_size = (int)rdcmd[12]*0x100 + (int)rdcmd[13] ;

        lcd_putstr( (char *)"                ", 0 ) ;
        tk_get_otm( &pk_tim ) ;
        sprintf( buff, "SysTime:%dsec", pk_tim.lo/1000 ) ;
        lcd_putstr( (char *)buff, 0 ) ;
#if DEBUG_ADC_COUNTER == 0
        lcd_putstr( (char *)"                ", 1 ) ;
        sprintf( buff, "send:%d Byte", cam_jpeg_size ) ;
        lcd_putstr( (char *)buff, 1 ) ;
#endif

        camera_cmd(      CAM_REQ_RDJPEG, rdcmd ) ;
        camera_readjpeg( cam_jpeg_size, cam_read_data ) ;

        /* stop take picture */
        camera_cmd(      CAM_REQ_STPTAKE, cam_req_stptake ) ;
        camera_response( CAM_RES_STPTAKE, cam_response_data ) ;

        /* send CAM data */
        camera_data_trans( cam_jpeg_size, cam_read_data ) ;

#if DEBUG_ADC_COUNTER == 0
        buff[0]='C' ;
        buff[1]='O' ;
        buff[2]='M' ;
        buff[3]='P' ;
        lcd_putstr( (char *)buff, 1 ) ;
#endif

        for( loop=0 ; loop < 80 ; loop++ ) {
            /* LED ON */
            led_set( 0x00000001 ) ;
            tk_slp_tsk( 2 ) ;
            /* LED OFF */
            led_set( 0x00000000 ) ;
            tk_slp_tsk( 23 ) ;
        }

        /* clear request flag */
        tk_clr_flg ( flag_id, ~(FLGPAT_REQ_CAM | FLGPAT_REQ_LGH) ) ;

        /* sensor restart */
        tk_set_flg ( flag_id, FLGPAT_END_CAM ) ;
    }
}


/*
 *  SENSOR task
 */
void  demo_sensor_task( void )
{
    int  now, before ;
    int  flag ;
    UINT  flgptn ;
    int  adc_flag ;
    unsigned int  adc_data ;
    ER  ercd ;
#if DEBUG_ADC_COUNTER
    char  buff[20] ;
#endif

    flag = 0 ;
    flgptn = FLGPAT_REQ_CAM ;

    /** check CAM Start **/
    ercd = tk_wai_flg( flag_id, FLGPAT_STA_CAM, TWF_ANDW|TWF_BITCLR,
                       &flgptn, TMO_FEVR ) ;

    lcd_putstr( (char *)"Start SENS TASK", 1 ) ;

    adc_flag = 0 ;
    before = sensport_counter() ; ;
    while( 1 ) {
        if( adc_flag == 0 ) {
            (void)adc_start(16) ;
            adc_flag = 1 ;
        } else {
            adc_data = adc_read( 16 ) ;
            if( adc_data != 0xffffffff ) {
#if DEBUG_ADC_COUNTER
                lcd_putstr( (char *)"                ", 1 ) ;
                sprintf( buff, "ADC : 0x%04x", adc_data ) ;
                lcd_putstr( (char *)buff, 1 ) ;
#endif

                if( adc_data > 0x800 ) {
                    flgptn = FLGPAT_REQ_CAM|FLGPAT_REQ_LGH ;
                } else {
                    flgptn = FLGPAT_REQ_CAM ;
                }
            }
            adc_flag = 0 ;
        }

        now = sensport_counter() ;
        if( flag < 1 ) {
            if( now != before ) {
                flag++ ;     /* increment pending flag */
            } else {
                flag = 0 ;   /* clear pending flag */
            }
        }
        before = now ;

        if( flag == 1 ) {
            /** request camera **/
            tk_set_flg ( flag_id, flgptn ) ;
            flag++ ;     /* increment pending flag */
        }

        /** check CAM END **/
        ercd = tk_wai_flg( flag_id, FLGPAT_END_CAM, TWF_ANDW|TWF_BITCLR,
                        &flgptn, TMO_POL ) ;
        if( ercd == E_OK ) {
            flag = 0 ;
        }

        /* sleep TASK */
        tk_dly_tsk( 500 ) ;
    }
}


void  demo_main( void )
{
    T_CTSK  ctsk ;
    T_CFLG  cflg;

    led_init();

    /* LED ON */
    led_set( 0x00000001 ) ;

    lcd_init() ;
    lcd_clear() ;
    uart_init() ;

    lcd_putstr( "YI Studio.", 0 ) ;
    lcd_putstr( " uT-Kernel", 1 ) ;

    sensport_init() ;
    io_port_init() ;
    adc_init() ;

    cflg.exinf = (VP)0x11110000;
    cflg.flgatr = (ATR)TA_TPRI|TA_WMUL;
    cflg.iflgptn = 0x00000000;
    flag_id = tk_cre_flg(&cflg) ;

    ctsk.exinf = (VP)0x0000abcd ;
    ctsk.tskatr = TA_HLNG|TA_RNG0|TA_USERBUF;
    ctsk.task = demo_camera_task ;
    ctsk.itskpri = 10 ;
    ctsk.bufptr = demo_camera_stack ;
    ctsk.stksz = SUB_TASK_STACK_SZ ;
    taskc_id = tk_cre_tsk( &ctsk ) ;
    tk_sta_tsk( taskc_id, 0 ) ;

    ctsk.exinf = (VP)0x00001234 ;
    ctsk.tskatr = TA_HLNG|TA_RNG0|TA_USERBUF;
    ctsk.task = demo_sensor_task ;
    ctsk.itskpri = 12 ;
    ctsk.bufptr = demo_sensor_stack ;
    ctsk.stksz = SUB_TASK_STACK_SZ ;
    tasks_id = tk_cre_tsk( &ctsk ) ;
    tk_sta_tsk( tasks_id, 0 ) ;

    while( 1 ) ;
}

