#include <stdio.h>
#include <string.h>
#include <ucos_ii.h> //ucos2
#include "os_c_lib_func.h"
#include "os_uart.h"
#include "flash.h"
#include "hal/usb.h"
#include "hal/led.h"

#define DEBUG
#ifdef DEBUG
#define USB_printf      OS_printf
#else
#define USB_printf(...)
#endif

OS_STK USB_Stk[TaskStkLength];

OS_EVENT   *usb_out_sem;
OS_EVENT   *usb_in_sem;

static unsigned char G_usb_out_buf[EP1_PKT_SIZE];
static unsigned char G_usb_out_len;


/* ---------- */


typedef struct usb_cmd_s
{
    unsigned int usb_command;
    unsigned int usb_seq_num;
    unsigned int usb_parameter[14];	// Let the size of structure == 64 byte
} usb_cmd_t;

#define USB_CMD_WRITE_W1            (0x01)
#define USB_CMD_WRITE_W2            (0x02)
#define USB_CMD_WRITE_W4            (0x03)
#define USB_CMD_WRITE_RANGE         (0x04)

#define USB_CMD_READ_R1             (0x11)
#define USB_CMD_READ_R2             (0x12)
#define USB_CMD_READ_R4             (0x13)
#define USB_CMD_READ_RANGE          (0x14)

#define USB_CMD_FLASH_ERASE         (0x21)
#define USB_CMD_FLASH_READ_PAGE     (0x22)
#define USB_CMD_FLASH_WRITE         (0x23)


/* ---------- */


typedef struct usb_ack_s
{
    unsigned int usb_command;
    unsigned int usb_seq_num;
    unsigned int usb_ack_status;
} usb_ack_t;


#define USB_ACK_SUCCESS         (0x00)
#define USB_ACK_FAIL            (0xFFFFFFFF)


/* ---------- */


#define ACK_RETRY_TIME          (10)
#define ACK_PEND_TIME           (100)

int usb_ack(unsigned int usb_command, unsigned int usb_seq_num, unsigned int usb_ack_status)
{
    usb_ack_t usb_ack;
    INT8U perr;
    
	usb_ack.usb_command = usb_command;
	usb_ack.usb_seq_num = usb_seq_num;
    usb_ack.usb_ack_status = usb_ack_status;
    
    OSSemPend(usb_in_sem, ACK_PEND_TIME, &perr);  // delay at most 100ms
    FillEP1Fifo((unsigned char*)&usb_ack, sizeof(usb_ack));
    return 0;
}

int usb_write(unsigned char *buf, unsigned int len)
{
    unsigned int start;
    INT8U perr;
    
    for(start=0; start<len; start+=EP3_PKT_SIZE)
    {
        unsigned int end;
        
        end = start + EP3_PKT_SIZE -1;
        if(end > len)
            end = len-1;
        
        OSSemPend(usb_in_sem, ACK_PEND_TIME, &perr);
        FillEP1Fifo(buf+start, end-start+1);
    }
    return 0;
}

int usb_read(unsigned char *buf, unsigned int len)
{
    unsigned int start, byte_recv=0;
    INT8U perr;
    
    for(start=0; start<len; )
    {
        OSSemPend(usb_out_sem, 300, &perr);
        if(perr != OS_ERR_NONE)
        {
            USB_printf("ERR!!\n");
            return -1;
        }
        memcpy((unsigned char*)(buf+start), G_usb_out_buf, G_usb_out_len);
        start += G_usb_out_len;
        byte_recv += G_usb_out_len;
    }
    return byte_recv;
}

void usb_out_process_cmd(void)
{
    INT8U perr;
    usb_cmd_t usb_cmd;
    unsigned int usb_seq_num, error = 0;
    
    OSSemPend(usb_out_sem, 0, &perr); // wait for data coming in    
    memcpy(&usb_cmd, G_usb_out_buf, EP1_PKT_SIZE);
    usb_seq_num = usb_cmd.usb_seq_num;
    
    // Parse command & data transmission
    switch(usb_cmd.usb_command)
    {
    case USB_CMD_WRITE_W1:
        {
            unsigned int addr = usb_cmd.usb_parameter[0];
            unsigned char value = usb_cmd.usb_parameter[1];
            
            USB_printf("W1: seq = %d, addr = 0x%08X, value = 0x%02X\n", usb_seq_num, addr, value);
            *(unsigned char*)addr = value;
        }
        break;
    case USB_CMD_WRITE_W2:
        {
            unsigned int addr = usb_cmd.usb_parameter[0];
            unsigned short value = usb_cmd.usb_parameter[1];
            
            USB_printf("W2: seq = %d, addr = 0x%08X, value = 0x%04X\n", usb_seq_num, addr, value);
            *(unsigned short*)addr = value;
        }
        break;
    case USB_CMD_WRITE_W4:
        {
            unsigned int addr = usb_cmd.usb_parameter[0];
            unsigned int value = usb_cmd.usb_parameter[1];
            
            USB_printf("W4: seq = %d, addr = 0x%08X, value = 0x%08X\n", usb_seq_num, addr, value);
            *(unsigned int*)addr = value;
        }
        break;
    case USB_CMD_WRITE_RANGE:
        {
            unsigned int addr = usb_cmd.usb_parameter[0];
            unsigned int length = usb_cmd.usb_parameter[1];
            USB_printf("WR: seq = %d, addr = 0x%08X, length = 0x%08X\n", usb_seq_num, addr, length);
            USB_printf("%d bytes received\n", usb_read((unsigned char *)addr, length));
        }
        break;
        
    /* ---------- */
    
    case USB_CMD_READ_R1:
        {
            unsigned int addr = usb_cmd.usb_parameter[0];
            unsigned char value = *(unsigned char*)addr;
            USB_printf("R1: seq = %d, addr = 0x%08X, value = 0x%02X\n", usb_seq_num, addr, value);
            usb_write((unsigned char*)&value, sizeof(value));
        }
        break;
    case USB_CMD_READ_R2:
        {
            unsigned int addr = usb_cmd.usb_parameter[0];
            unsigned short value = *(unsigned short*)addr;
            USB_printf("R2: seq = %d, addr = 0x%08X, value = 0x%04X\n", usb_seq_num, addr, value);
            usb_write((unsigned char*)&value, sizeof(value));
        }
        break;
    case USB_CMD_READ_R4:
        {
            unsigned int addr = usb_cmd.usb_parameter[0];
            unsigned int value = *(unsigned int*)addr;
            USB_printf("R4: seq = %d, addr = 0x%08X, value = 0x%08X\n", usb_seq_num, addr, value);
            usb_write((unsigned char*)&value, sizeof(value));
        }
        break;
    case USB_CMD_READ_RANGE:
        {
            unsigned int addr = usb_cmd.usb_parameter[0];
            unsigned int length = usb_cmd.usb_parameter[1];
            
            if(length > 0)
            {
                USB_printf("Read Range: seq = %d, addr = 0x%08X, length = %d\n", usb_seq_num, addr, length);
                usb_write((unsigned char*)addr, length);
            }
            else
            {
                OSTimeDly(2000);    // delay to let PC-end program timeout
                error = 1;
            }
        }
        break;
        
    /* ---------- */
    
    case USB_CMD_FLASH_ERASE:
        {
            unsigned int flash_block_num = usb_cmd.usb_parameter[0];
            unsigned int flash_linear_addr = flash_block_num*16384;
            flash_mem f_addr;
            
            USB_printf("Flash Erase: seq = %d, flash_block_num = %d\n", usb_seq_num, flash_block_num);
            Flash_fill_mem_addr(flash_linear_addr, &f_addr);
            Flash_EraseBlock(&f_addr);
        }
        break;
    case USB_CMD_FLASH_READ_PAGE:
        {
            unsigned int flash_start_page = usb_cmd.usb_parameter[0];
            unsigned int num_flash_page = usb_cmd.usb_parameter[1];
            unsigned int flash_linear_addr;
            unsigned char buf[512];
            flash_mem f_addr;
            unsigned int i;
            
            USB_printf("Flash Read: seq = %d, flash_start_page = %d, num_flash_page = %d\n", usb_seq_num, flash_start_page, num_flash_page);
            
            flash_linear_addr = flash_start_page * 512;
            for(i=0; i<num_flash_page; i++)
            {
                Flash_fill_mem_addr(flash_linear_addr, &f_addr);
                Flash_ReadPage(&f_addr, buf);
                usb_write(buf, 512);
                flash_linear_addr += 512;
            }            
        }
        break;
    case USB_CMD_FLASH_WRITE:
        {
            unsigned int flash_start_page = usb_cmd.usb_parameter[0];
            unsigned int num_flash_page = usb_cmd.usb_parameter[1];
            unsigned int flash_linear_addr;
            flash_mem f_addr;
            unsigned int i;
            
            USB_printf("Flash Write: seq = %d, flash_start_page = %d, num_flash_page = %d\n", usb_seq_num, flash_start_page, num_flash_page);
            
            flash_linear_addr = flash_start_page * 512;
            for(i=0; i<num_flash_page; i++)
            {
                unsigned char flash_buf[512];
                unsigned char buf[] = "OK";
                
                if(usb_read((unsigned char *)flash_buf, 512) != 512)
                {
                    error = 1;
                    USB_printf("USB receive data error\n");
                    break;
                }
                Flash_fill_mem_addr(flash_linear_addr, &f_addr);
                Flash_WritePage(&f_addr, flash_buf);
                flash_linear_addr += 512;
                usb_write(buf, 2);  // inform host that flash page wrote done
            }
        }
        break;
    default:
        break;
    }
    
    // ACK Phase
    if(error == 0)
        usb_ack(usb_cmd.usb_command, usb_seq_num, USB_ACK_SUCCESS);
    else
        usb_ack(usb_cmd.usb_command, usb_seq_num, USB_ACK_FAIL);
}

/* ----------- */

void usb_out_callback_fun(unsigned char *buf, int packet_size)
{
    USB_printf("buffer addr: 0x%X, length: 0x%X\n", (unsigned int)buf, packet_size);
    
    memcpy(G_usb_out_buf, buf, packet_size);
    G_usb_out_len = packet_size;
    
    OSSemPost(usb_out_sem);
}

void usb_in_callback_fun()
{
    OSSemPost(usb_in_sem);
}

void USB_Task(void *pdata)
{
	pdata = pdata;
    
    usb_out_sem = OSSemCreate(0);
    usb_out_register_callback_func(usb_out_callback_fun);

    usb_in_sem = OSSemCreate(1);
    usb_in_register_callback_func(usb_in_callback_fun);
    
    while (1)
    {
        usb_out_process_cmd();
    }
}