#include "sysmgr.h"
#include <nrfx.h>
#include <nrf_delay.h>
#include <drv_rtc.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include "nrf_spi.h"
#include "nrf_drv_spi.h"

#include "our_service.h"
#include "aes.h"



#define CBC 1
#define ECB 1


#define DATA_STORAGE_INSTANCE_ID        0x0022
#define DATA_STORAGE_TYPE_ID            0x0021
#define SENTINEL_VALUE                  0xEE


extern const nrf_drv_spi_t spi;  /**< SPI instance. */
extern volatile bool spi_xfer_done;  /**< Flag used to indicate that SPI instance completed the transfer. */
uint8_t       m_tx_buf[64];           /**< TX buffer. */
uint8_t       m_rx_buf[64];    /**< RX buffer. */
uint8_t m_length;        /**< Transfer length. */


RegMacPool_t RegMacPool;


const char *fds_err_str(ret_code_t ret)
{
    /* Array to map FDS return values to strings. */
    static char const * err_str[] =
    {
        "FDS_ERR_OPERATION_TIMEOUT",
        "FDS_ERR_NOT_INITIALIZED",
        "FDS_ERR_UNALIGNED_ADDR",
        "FDS_ERR_INVALID_ARG",
        "FDS_ERR_NULL_ARG",
        "FDS_ERR_NO_OPEN_RECORDS",
        "FDS_ERR_NO_SPACE_IN_FLASH",
        "FDS_ERR_NO_SPACE_IN_QUEUES",
        "FDS_ERR_RECORD_TOO_LARGE",
        "FDS_ERR_NOT_FOUND",
        "FDS_ERR_NO_PAGES",
        "FDS_ERR_USER_LIMIT_REACHED",
        "FDS_ERR_CRC_CHECK_FAILED",
        "FDS_ERR_BUSY",
        "FDS_ERR_INTERNAL",
    };

    return err_str[ret - NRF_ERROR_FDS_ERR_BASE];
}

void ParkSysInit()
{
  SystemTimer.TMR_SYS_OFF = 10;


}




void SC16IS750_FIFOEnable(unsigned char fifo_enable)
{
    unsigned char temp_fcr;

    temp_fcr = SC16IS750_ReadRegister(SC16IS750_REG_FCR);

    if (fifo_enable == 0){
        temp_fcr &= 0xFE;
    } else {
        temp_fcr |= 0x01;
    }
    SC16IS750_WriteRegister(SC16IS750_REG_FCR,temp_fcr);

    return;
}




void SC16IS750_WriteRegister(unsigned char reg_addr, unsigned char val)
{

  m_tx_buf[0] = reg_addr<<3;
  m_tx_buf[1] = val;

  //nrf_gpio_pin_write( SPI_SS_PIN,0);
  nrf_delay_us(10);
  nrf_drv_spi_transfer(&spi, m_tx_buf, 2, m_rx_buf, 0);
  nrf_delay_us(10);
  //nrf_gpio_pin_write( SPI_SS_PIN,1);

    return ;
}


unsigned char SC16IS750_ReadRegister(unsigned char reg_addr)
{
  unsigned char result;



  m_tx_buf[0] = 0x80|(reg_addr<<3);


 // nrf_gpio_pin_write( SPI_SS_PIN,0);
  nrf_delay_us(10);
  nrf_drv_spi_transfer(&spi, m_tx_buf, 1, m_rx_buf, 1);
  result = m_rx_buf[0];
  nrf_delay_us(10);
 // nrf_gpio_pin_write( SPI_SS_PIN,1);


  return result;

}



void SC16IS750_ResetDevice(void)
{
    unsigned char reg;

    reg = SC16IS750_ReadRegister(SC16IS750_REG_IOCONTROL);
    reg |= 0x08;
    SC16IS750_WriteRegister(SC16IS750_REG_IOCONTROL, reg);

    return;
}



int16_t SC16IS750_SetBaudrate(uint32_t baudrate) //return error of baudrate parts per thousand
{
    uint16_t divisor;
    uint8_t prescaler;
    uint32_t actual_baudrate;
    int16_t error;
    uint8_t temp_lcr;
    if ( (SC16IS750_ReadRegister(SC16IS750_REG_MCR)&0x80) == 0) { //if prescaler==1
        prescaler = 1;
    } else {
        prescaler = 4;
    }

    prescaler = 1;

    divisor = (SC16IS750_CRYSTCAL_FREQ/prescaler)/(baudrate*16);

    temp_lcr = SC16IS750_ReadRegister(SC16IS750_REG_LCR);
    temp_lcr |= 0x80;
    SC16IS750_WriteRegister(SC16IS750_REG_LCR,temp_lcr);
    //write to DLL
    SC16IS750_WriteRegister(SC16IS750_REG_DLL,(uint8_t)divisor);
    //write to DLH
    SC16IS750_WriteRegister(SC16IS750_REG_DLH,(uint8_t)(divisor>>8));
    temp_lcr &= 0x7F;
    SC16IS750_WriteRegister(SC16IS750_REG_LCR,temp_lcr);


    actual_baudrate = (SC16IS750_CRYSTCAL_FREQ/prescaler)/(16*divisor);
    error = ((float)actual_baudrate-baudrate)*1000/baudrate;

    return error;

}



void SC16IS750_SetLine(uint8_t data_length, uint8_t parity_select, uint8_t stop_length )
{
    uint8_t temp_lcr;
    temp_lcr = SC16IS750_ReadRegister(SC16IS750_REG_LCR);
    temp_lcr &= 0xC0; //Clear the lower six bit of LCR (LCR[0] to LCR[5]


    switch (data_length) {            //data length settings
        case 5:
            break;
        case 6:
            temp_lcr |= 0x01;
            break;
        case 7:
            temp_lcr |= 0x02;
            break;
        case 8:
            temp_lcr |= 0x03;
            break;
        default:
            temp_lcr |= 0x03;
            break;
    }

    if ( stop_length == 2 ) {
        temp_lcr |= 0x04;
    }

    switch (parity_select) {            //parity selection length settings
        case 0:                         //no parity
             break;
        case 1:                         //odd parity
            temp_lcr |= 0x08;
            break;
        case 2:                         //even parity
            temp_lcr |= 0x18;
            break;
        case 3:                         //force '1' parity
            temp_lcr |= 0x03;
            break;
        case 4:                         //force '0' parity
            break;
        default:
            break;
    }

    SC16IS750_WriteRegister(SC16IS750_REG_LCR,temp_lcr);
}



void SC16IS750_WriteByte(uint8_t val)
{
	uint8_t tmp_lsr;

	do {
		tmp_lsr = SC16IS750_ReadRegister(SC16IS750_REG_LSR);
	} while ((tmp_lsr&0x20) ==0);
//nrf_delay_ms(1);
	SC16IS750_WriteRegister(SC16IS750_REG_THR,val);
}



int DBGPrint(const char *fmt, ...)
{
    char buff[128];
    va_list args;
    int n;
    int i;

    va_start(args, fmt);
    n = vsnprintf(buff, 120, fmt, args);
    va_end(args);


    //HAL_UART_Transmit(CLIUart, (uint8_t*)buff, n, 500);

    for( i=0; i<n; i++)
    {
        
      SC16IS750_WriteByte(buff[i]);
    }

    

    return n;
}



/* Flash related functions */







/* Dummy configuration data. */
static configuration_t m_dummy_cfg =
{
    .config1_on  = false,
    .config2_on  = true,
    .boot_count  = 0x0,
    .device_name = "dummy",
};





static void record_write(uint32_t fid,
                         uint32_t key,
                         void const * p_data,
                         uint32_t len)
{
    fds_record_t const rec =
    {
        .file_id           = fid,
        .key               = key,
        .data.p_data       = p_data,
        .data.length_words = (len + 3) / sizeof(uint32_t)
    };

    NRF_LOG_INFO(
                    "writing record to flash...\n"
                    "file: 0x%x, key: 0x%x, \"%s\", len: %u bytes\n",
                    fid, key, p_data, len);

    ret_code_t rc = fds_record_write(NULL, &rec);
    if (rc != NRF_SUCCESS)
    {
        NRF_LOG_INFO(
                        "error: fds_record_write() returned %s.\n",
                        fds_err_str(rc));
    }
}





static void record_update( configuration_t const * p_cfg)
{
    fds_record_desc_t desc = {0};
    fds_find_token_t  ftok = {0};

    if (fds_record_find(CONFIG_FILE, CONFIG_REC_KEY, &desc, &ftok) == NRF_SUCCESS)
    {
        fds_record_t const rec =
        {
            .file_id           = CONFIG_FILE,
            .key               = CONFIG_REC_KEY,
            .data.p_data       = p_cfg,
            .data.length_words = (sizeof(configuration_t) + 3) / sizeof(uint32_t)
        };

        ret_code_t rc = fds_record_update(&desc, &rec);
        if (rc != NRF_SUCCESS)
        {
            NRF_LOG_INFO( "error: fds_record_update() returned %s.\n",
                            fds_err_str(rc));
        }
    }
    else
    {
        NRF_LOG_INFO( "error: could not find config file.\n");
    }
}


static void record_delete( uint32_t fid, uint32_t key)
{
    fds_find_token_t tok   = {0};
    fds_record_desc_t desc = {0};

    NRF_LOG_INFO( 
                    "deleting record...\n"
                    "file: 0x%x, key: 0x%x\n",
                    fid,
                    key);

    if (fds_record_find(fid, key, &desc, &tok) == NRF_SUCCESS)
    {
        ret_code_t rc = fds_record_delete(&desc);
        if (rc != NRF_SUCCESS)
        {
            NRF_LOG_INFO( 
                            "error: fds_record_delete() returned %s.\n", fds_err_str(rc));

            return;
        }

        NRF_LOG_INFO(  "record id: 0x%x\n", desc.record_id);
    }
    else
    {
        NRF_LOG_INFO(  "error: record not found!\n");
    }
}


bool record_delete_next(void)
{
    fds_find_token_t  tok   = {0};
    fds_record_desc_t desc  = {0};

    if (fds_record_iterate(&desc, &tok) == NRF_SUCCESS)
    {
        ret_code_t rc = fds_record_delete(&desc);
        if (rc != NRF_SUCCESS)
        {
            return false;
        }

        return true;
    }
    else
    {
        /* No records left to delete. */
        return false;
    }
}




static void print_cfg_cmd( size_t argc, char ** argv)
{
    fds_record_desc_t desc = {0};
    fds_find_token_t  tok  = {0};

    while (fds_record_find(CONFIG_FILE, CONFIG_REC_KEY, &desc, &tok) == NRF_SUCCESS)
    {
        ret_code_t rc;
        fds_flash_record_t frec = {0};

        rc = fds_record_open(&desc, &frec);
        switch (rc)
        {
            case NRF_SUCCESS:
                break;

            case FDS_ERR_CRC_CHECK_FAILED:
                NRF_LOG_INFO( "error: CRC check failed!\n");
                continue;

            case FDS_ERR_NOT_FOUND:
                NRF_LOG_INFO( "error: record not found!\n");
                continue;

            default:
            {
                NRF_LOG_INFO(
                                "error: unexpecte error %s.\n",
                                fds_err_str(rc));

                continue;
            }
        }

        configuration_t * p_cfg = (configuration_t *)(frec.p_data);

        NRF_LOG_INFO(
                        "config1:\t%s\n"
                        "config2:\t%s\n"
                        "boot count:\t%u\n"
                        "device name:\t%s\n",
                        p_cfg->config1_on ? "on" : "off",
                        p_cfg->config2_on ? "on" : "off",
                        p_cfg->boot_count,
                        p_cfg->device_name);

        rc = fds_record_close(&desc);
        APP_ERROR_CHECK(rc);
    }
}





static void print_all_cmd( )
{
    fds_find_token_t tok   = {0};
    fds_record_desc_t desc = {0};
    uint8_t *data;

    NRF_LOG_INFO( 
                    "rec. id\t"
                    "\tfile id\t"
                    "\trec. key"
                    "\tlength\n");

    while (fds_record_iterate(&desc, &tok) != FDS_ERR_NOT_FOUND)
    {
        ret_code_t rc;
        fds_flash_record_t frec = {0};

        rc = fds_record_open(&desc, &frec);
        switch (rc)
        {
            case NRF_SUCCESS:
                break;

            case FDS_ERR_CRC_CHECK_FAILED:
                NRF_LOG_INFO(  "error: CRC check failed!\n");
                continue;

            case FDS_ERR_NOT_FOUND:
                NRF_LOG_INFO(  "error: record not found!\n");
                continue;

            default:
            {
                NRF_LOG_INFO( 
                                "error: unexpecte error %s.\n",
                                fds_err_str(rc));

                continue;
            }
        }

        uint32_t const len = frec.p_header->length_words * sizeof(uint32_t);

        NRF_LOG_INFO( 
                        " 0x%04x\t"
                        "\t 0x%04x\t"
                        "\t 0x%04x\t"
                        "\t %4u bytes\t",
                        frec.p_header->record_id,
                        frec.p_header->file_id,
                        frec.p_header->record_key,
                        len);

        data = (uint8_t *) frec.p_data;
        for (uint8_t i=0;i<len;i++)
        {
                NRF_LOG_RAW_INFO( "%c",data[i]);
        }
        NRF_LOG_INFO("\n");



        rc = fds_record_close(&desc);
        APP_ERROR_CHECK(rc);
    }
}



void RegMac(char* str)
{

  print_all_cmd();
  NRF_LOG_RAW_INFO("Registration MAC : %s\n", str);
  
  RegMacPool.Count++;
  record_write(1, RegMacPool.Count, str, 12);

  print_all_cmd();

}



void RegMacDelteAll()
{
    bool next;

  while(1)
  {  
       next = record_delete_next();
        if (!next)
        {
            NRF_LOG_INFO("No records left to delete.");
            break;
        } 
  }



  memset( &RegMacPool, 0, sizeof(RegMacPool));


}




void LoadRegMac()
{
    fds_find_token_t tok   = {0};
    fds_record_desc_t desc = {0};
    uint8_t *data;
    char tmpBuf[15];

    NRF_LOG_INFO( 
                    "\nrec. id\t"
                    "\tfile id\t"
                    "\trec. key"
                    "\tlength\tmac\n");

    while (fds_record_iterate(&desc, &tok) != FDS_ERR_NOT_FOUND)
    {
        ret_code_t rc;
        fds_flash_record_t frec = {0};

        rc = fds_record_open(&desc, &frec);
		
        switch (rc)
        {
            case NRF_SUCCESS:
                break;

            case FDS_ERR_CRC_CHECK_FAILED:
                NRF_LOG_INFO("error: CRC check failed!\n");
                continue;

            case FDS_ERR_NOT_FOUND:
                NRF_LOG_INFO("error: record not found!\n");
                continue;

            default:
            {
                NRF_LOG_INFO("error: unexpecte error %s.\n", fds_err_str(rc));
                continue;
            }
        }

        uint32_t const len = frec.p_header->length_words * sizeof(uint32_t);

        NRF_LOG_RAW_INFO( 
                        " 0x%04x\t"
                        "\t 0x%04x\t"
                        "\t 0x%04x\t"
                        "\t %4u bytes\t",
                        frec.p_header->record_id,
                        frec.p_header->file_id,
                        frec.p_header->record_key,
                        len);

        data = (uint8_t *) frec.p_data;


        if( RegMacPool.Count >= 5 )
        {
            NRF_LOG_RAW_INFO( "Mac Pool Over\n");

            return;
        }
        
        RegMacPool.Mac[RegMacPool.Count][12] = 0;
        strncpy( RegMacPool.Mac[RegMacPool.Count++], data, 12);
        //for (uint8_t i=0;i<len;i++)
        {
        //        NRF_LOG_RAW_INFO( "%c",data[i]);
        }
        //NRF_LOG_RAW_INFO( "\n");
         NRF_LOG_RAW_INFO( "%s\n",RegMacPool.Mac[RegMacPool.Count-1]);

        rc = fds_record_close(&desc);
        APP_ERROR_CHECK(rc);
    }
}



// 수신기 Flash에 등록할 비상벨 Mac 저장
void SaveRegMac(char* mac)
{
  int i;

  if( RegMacPool.Count>= 5 )
      return;

  for( i=0; i<RegMacPool.Count; i++)
  {
      if( strncmp( RegMacPool.Mac[i], mac, 12) == 0 )
      {
           NRF_LOG_RAW_INFO( "Allready Registrated\n");
          return;
      }
  }
  
  RegMacPool.Mac[RegMacPool.Count][12] = 0;
  strncpy( RegMacPool.Mac[RegMacPool.Count], mac, 12);

   record_write(1, RegMacPool.Count+1, mac, 12);

    RegMacPool.Count++;
	
}




// 송신기가 수신기에 등록된 MAC인지 확인
int FindRegMac(char* mac)
{

  int i;

  for( i=0; i<RegMacPool.Count; i++)
  {
      if( strncmp( RegMacPool.Mac[i], mac, 12) == 0 )
      {
           NRF_LOG_RAW_INFO( "Valid Mac\n");

          return 1;
      }  
  }

  return 0;

}

void DisplayRegMac()
{

  print_all_cmd( );

}




void FlashTest()
{


    static uint8_t m_data[256];
    int len;

    sprintf(m_data, "hello-%d", 1);

    len = strlen(m_data);
    

   // record_write( 1, 1, m_data, len);
    sprintf(m_data, "hello-%d", 2);
    //record_write( 1, 2, m_data, len);

    print_all_cmd();
}


void SetMacAddress(uint8_t* addr)
{

	sd_ble_gap_addr_get(&old_ble_addr);
	ble_gap_addr_t dd;
	dd.addr_id_peer = 0;
	dd.addr_type = BLE_GAP_ADDR_TYPE_PUBLIC;
	dd.addr[0] = addr[0];
	dd.addr[1] = addr[1];
	//dd.addr[2] = addr[2];
        dd.addr[2] = 0x57;
	dd.addr[3] = addr[3];
	dd.addr[4] = addr[4];
	dd.addr[5] = addr[5];	
    sd_ble_gap_addr_set(&dd);

	
	sd_ble_gap_addr_get(&new_ble_addr);
	
}





// prints string as hex
static void phex(uint8_t* str)
{
    unsigned char i;
    for(i = 0; i < 16; ++i)
        NRF_LOG_RAW_INFO("%.2x", str[i]);
    NRF_LOG_RAW_INFO("\n");
}

static void test_encrypt_ecb_verbose(void)
{
    // Example of more verbose verification

    uint8_t i, buf[64], buf2[64];

    // 128bit key
    uint8_t key[16] =        { (uint8_t) 0x2b, (uint8_t) 0x7e, (uint8_t) 0x15, (uint8_t) 0x16, (uint8_t) 0x28, (uint8_t) 0xae, (uint8_t) 0xd2, (uint8_t) 0xa6, (uint8_t) 0xab, (uint8_t) 0xf7, (uint8_t) 0x15, (uint8_t) 0x88, (uint8_t) 0x09, (uint8_t) 0xcf, (uint8_t) 0x4f, (uint8_t) 0x3c };
    // 512bit text
    uint8_t plain_text[64] = { (uint8_t) 0x6b, (uint8_t) 0xc1, (uint8_t) 0xbe, (uint8_t) 0xe2, (uint8_t) 0x2e, (uint8_t) 0x40, (uint8_t) 0x9f, (uint8_t) 0x96, (uint8_t) 0xe9, (uint8_t) 0x3d, (uint8_t) 0x7e, (uint8_t) 0x11, (uint8_t) 0x73, (uint8_t) 0x93, (uint8_t) 0x17, (uint8_t) 0x2a,
                               (uint8_t) 0xae, (uint8_t) 0x2d, (uint8_t) 0x8a, (uint8_t) 0x57, (uint8_t) 0x1e, (uint8_t) 0x03, (uint8_t) 0xac, (uint8_t) 0x9c, (uint8_t) 0x9e, (uint8_t) 0xb7, (uint8_t) 0x6f, (uint8_t) 0xac, (uint8_t) 0x45, (uint8_t) 0xaf, (uint8_t) 0x8e, (uint8_t) 0x51,
                               (uint8_t) 0x30, (uint8_t) 0xc8, (uint8_t) 0x1c, (uint8_t) 0x46, (uint8_t) 0xa3, (uint8_t) 0x5c, (uint8_t) 0xe4, (uint8_t) 0x11, (uint8_t) 0xe5, (uint8_t) 0xfb, (uint8_t) 0xc1, (uint8_t) 0x19, (uint8_t) 0x1a, (uint8_t) 0x0a, (uint8_t) 0x52, (uint8_t) 0xef,
                               (uint8_t) 0xf6, (uint8_t) 0x9f, (uint8_t) 0x24, (uint8_t) 0x45, (uint8_t) 0xdf, (uint8_t) 0x4f, (uint8_t) 0x9b, (uint8_t) 0x17, (uint8_t) 0xad, (uint8_t) 0x2b, (uint8_t) 0x41, (uint8_t) 0x7b, (uint8_t) 0xe6, (uint8_t) 0x6c, (uint8_t) 0x37, (uint8_t) 0x10 };

    memset(buf, 0, 64);
    memset(buf2, 0, 64);

    // print text to encrypt, key and IV
    NRF_LOG_RAW_INFO("ECB encrypt verbose:\n\n");
    NRF_LOG_RAW_INFO("plain text:\n");
    for(i = (uint8_t) 0; i < (uint8_t) 4; ++i)
    {
        phex(plain_text + i * (uint8_t) 16);
    }
    NRF_LOG_RAW_INFO("\n");

    NRF_LOG_RAW_INFO("key:\n");
    phex(key);
    NRF_LOG_RAW_INFO("\n");

    // print the resulting cipher as 4 x 16 byte strings
    NRF_LOG_RAW_INFO("ciphertext:\n");
    for(i = 0; i < 4; ++i)
    {
        AES128_ECB_encrypt(plain_text + (i*16), key, buf+(i*16));
        phex(buf + (i*16));
    }
    NRF_LOG_RAW_INFO("\n");
}


static void test_encrypt_ecb(void)
{
  uint8_t key[] = {0x2b, 0x7e, 0x15, 0x16, 0x28, 0xae, 0xd2, 0xa6, 0xab, 0xf7, 0x15, 0x88, 0x09, 0xcf, 0x4f, 0x3c};
  uint8_t in[]  = {0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96, 0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a};
  uint8_t out[] = {0x3a, 0xd7, 0x7b, 0xb4, 0x0d, 0x7a, 0x36, 0x60, 0xa8, 0x9e, 0xca, 0xf3, 0x24, 0x66, 0xef, 0x97};
  uint8_t buffer[16];

  AES128_ECB_encrypt(in, key, buffer);

  NRF_LOG_RAW_INFO("ECB encrypt: ");

  if(0 == strncmp((char*) out, (char*) buffer, 16))
  {
    NRF_LOG_RAW_INFO("SUCCESS!\n");
  }
  else
  {
    NRF_LOG_RAW_INFO("FAILURE!\n");
  }
}



static void test_decrypt_ecb(void)
{
  uint8_t key[] = {0x2b, 0x7e, 0x15, 0x16, 0x28, 0xae, 0xd2, 0xa6, 0xab, 0xf7, 0x15, 0x88, 0x09, 0xcf, 0x4f, 0x3c};
  uint8_t in[]  = {0x3a, 0xd7, 0x7b, 0xb4, 0x0d, 0x7a, 0x36, 0x60, 0xa8, 0x9e, 0xca, 0xf3, 0x24, 0x66, 0xef, 0x97};
  uint8_t out[] = {0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96, 0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a};
 uint8_t buffer[16];

  AES128_ECB_decrypt(in, key, buffer);

  NRF_LOG_RAW_INFO("ECB decrypt: ");

  if(0 == strncmp((char*) out, (char*) buffer, 16))
  {
    NRF_LOG_RAW_INFO("SUCCESS!\n");
  }
  else
  {
    NRF_LOG_RAW_INFO("FAILURE!\n");
  }
}



uint8_t AesBuffer[16];

void CarEncryptEcb(int mode)
{
  uint8_t key[] = {0x53, 0x6D, 0x61, 0x72, 0x74, 0x42, 0x61, 0x6E, 0x64, 0x50, 0x41, 0x52, 0x54, 0x52, 0x4F, 0x4E}; // SmartBandPARTRON
  uint8_t in[]  = {'C', 'A', 'R', 1,2,3,4,5,6,7,8,9,10,11,12,13 };

  uint16_t tempVal;

#if 1
	switch(mode){
		case AES_ENC_CAR:
			in[0] = 'C';
			in[1] = 'A';
			in[2] = 'R';
		break;

		case AES_ENC_EMG:
			in[0] = 'E';
			in[1] = 'M';
			in[2] = 'G';
		break;

		case AES_ENC_PCA:
			in[0] = 'P';
			in[1] = 'C';
			in[2] = 'A';
		break;
	}
	
#else

  if( mode == AES_ENC_CAR )
  {
  	in[0] = 'C';
	in[1] = 'A';
	in[2] = 'R';
  }else if( mode == AES_ENC_EMG )
  {
  	in[0] = 'E';
	in[1] = 'M';
	in[2] = 'G';
  }
#endif

  tempVal = rand();

  in[3] = tempVal&0xff;
  in[4] = (tempVal>>8)&0xff;

  AES128_ECB_encrypt(in, key, SystemManager.AesEncData);

  NRF_LOG_RAW_INFO("ECB encrypt: %x\n", tempVal);

}



// 0x73:parking  0x72:emg
void CarEncryptEcbACK(char type)
{

	uint8_t key[] = {0x53, 0x6D, 0x61, 0x72, 0x74, 0x42, 0x61, 0x6E, 0x64, 0x50, 0x41, 0x52, 0x54, 0x52, 0x4F, 0x4E}; // SmartBandPARTRON
	uint8_t in[]  = {'C', 'A', 'R', 'A','C','K',1,2,3,4,5,6,7,8,9,0x0A };

	switch(type){

		case TRANSFER_UNIT_BAND_PARKING:
			in[0] = 'A';
			in[1] = 'C';
			in[2] = 'K';	
		break;

		case TRANSFER_UNIT_BAND_EMG:
			in[0] = 'E';
			in[1] = 'M';
			in[2] = 'G';	
		break;

		case TRANSFER_UNIT_NEW_IOS_PARKING:
			in[0] = 'P';
			in[1] = 'C';
			in[2] = 'A';	
		break;
	}


	AES128_ECB_encrypt(in, key, SystemManager.AesEncData);

	NRF_LOG_RAW_INFO("ECB ACK\n");
}






void CarDecryptEcb(char* in, char* out)
{
	uint8_t key[] = {0x53, 0x6D, 0x61, 0x72, 0x74, 0x42, 0x61, 0x6E, 0x64, 0x50, 0x41, 0x52, 0x54, 0x52, 0x4F, 0x4E}; // SmartBandPARTRON
//	uint8_t out[] = {'C', 'A', 'R', 1,2,3,4,5,6,7,8,9,10,11,12,13 };
//	uint8_t buffer[16];

	AES128_ECB_decrypt(in, key, out);

//	NRF_LOG_RAW_INFO("ECB decrypt: ");


}




void AppDecryptCbc(char* in, char* out)
{
	uint8_t key[] = {'1', '9', '-', 'H', 'D', 'C', '-', 'I', 'C', 'O', 'N', 'T', 'R', 'O', 'L', 'S'}; // App 
	uint8_t iv[]  = {'H', 'D', 'C', '-', 'I', 'V', '-', 'R', 'N', 'D', 'C', 'E', 'N', 'T', 'E', 'R' };

	//uint8_t key[] = "19-HDC-ICONTROLS";
	//uint8_t iv[]  = "HDC-IV-RNDCENTER";
	   

	AES128_CBC_decrypt_buffer(out+0, in+0,	16, key, iv);
	//AES128_CBC_decrypt_buffer(out+16, in+16, 8, 0, 0);

}



void AppEnryptCbc(char* in, char* out)
{
	uint8_t key[] = {'1', '9', '-', 'H', 'D', 'C', '-', 'T', 'C', 'O', 'N', 'T', 'R', 'O', 'L', 'S'}; // APP
	uint8_t iv[]  = {'H', 'D', 'C', '-', 'T', 'V', '-', 'R', 'N', 'D', 'C', 'E', 'N', 'T', 'E', 'R' };
	// uint8_t in[]  = { 0x31, 0x01, 0x02, 0xEE, 0x16, 0x00, 0x00, 0xA5, 0xE4, 0x8B, 0x01, 0x16, 0x6C, 0x38, 0xFA, 0x95, 0xDB, 0x5E, 0xFA, 0xF3, 0x82, 0x09, 0x5C, 0x7B };

	// uint8_t buffer[30];

	AES128_CBC_encrypt_buffer(out, in, 24, key, iv);

	NRF_LOG_RAW_INFO("CBC encrypt: ");


}



static void test_decrypt_cbc(void)
{
  // Example "simulating" a smaller buffer...

  uint8_t key[] = { 0x2b, 0x7e, 0x15, 0x16, 0x28, 0xae, 0xd2, 0xa6, 0xab, 0xf7, 0x15, 0x88, 0x09, 0xcf, 0x4f, 0x3c };
  uint8_t iv[]  = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f };
  uint8_t in[]  = { 0x76, 0x49, 0xab, 0xac, 0x81, 0x19, 0xb2, 0x46, 0xce, 0xe9, 0x8e, 0x9b, 0x12, 0xe9, 0x19, 0x7d,
                    0x50, 0x86, 0xcb, 0x9b, 0x50, 0x72, 0x19, 0xee, 0x95, 0xdb, 0x11, 0x3a, 0x91, 0x76, 0x78, 0xb2,
                    0x73, 0xbe, 0xd6, 0xb8, 0xe3, 0xc1, 0x74, 0x3b, 0x71, 0x16, 0xe6, 0x9e, 0x22, 0x22, 0x95, 0x16, 
                    0x3f, 0xf1, 0xca, 0xa1, 0x68, 0x1f, 0xac, 0x09, 0x12, 0x0e, 0xca, 0x30, 0x75, 0x86, 0xe1, 0xa7 };
  uint8_t out[] = { 0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96, 0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a,
                    0xae, 0x2d, 0x8a, 0x57, 0x1e, 0x03, 0xac, 0x9c, 0x9e, 0xb7, 0x6f, 0xac, 0x45, 0xaf, 0x8e, 0x51,
                    0x30, 0xc8, 0x1c, 0x46, 0xa3, 0x5c, 0xe4, 0x11, 0xe5, 0xfb, 0xc1, 0x19, 0x1a, 0x0a, 0x52, 0xef,
                    0xf6, 0x9f, 0x24, 0x45, 0xdf, 0x4f, 0x9b, 0x17, 0xad, 0x2b, 0x41, 0x7b, 0xe6, 0x6c, 0x37, 0x10 };
  uint8_t buffer[64];

  AES128_CBC_decrypt_buffer(buffer+0, in+0,  16, key, iv);
  AES128_CBC_decrypt_buffer(buffer+16, in+16, 16, 0, 0);
  AES128_CBC_decrypt_buffer(buffer+32, in+32, 16, 0, 0);
  AES128_CBC_decrypt_buffer(buffer+48, in+48, 16, 0, 0);

  NRF_LOG_RAW_INFO("CBC decrypt: ");

  if(0 == strncmp((char*) out, (char*) buffer, 64))
  {
    NRF_LOG_RAW_INFO("SUCCESS!\n");
  }
  else
  {
    NRF_LOG_RAW_INFO("FAILURE!\n");
  }
}

static void test_encrypt_cbc(void)
{
  uint8_t key[] = { 0x2b, 0x7e, 0x15, 0x16, 0x28, 0xae, 0xd2, 0xa6, 0xab, 0xf7, 0x15, 0x88, 0x09, 0xcf, 0x4f, 0x3c };
  uint8_t iv[]  = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f };
  uint8_t in[]  = { 0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96, 0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a,
                    0xae, 0x2d, 0x8a, 0x57, 0x1e, 0x03, 0xac, 0x9c, 0x9e, 0xb7, 0x6f, 0xac, 0x45, 0xaf, 0x8e, 0x51,
                    0x30, 0xc8, 0x1c, 0x46, 0xa3, 0x5c, 0xe4, 0x11, 0xe5, 0xfb, 0xc1, 0x19, 0x1a, 0x0a, 0x52, 0xef,
                    0xf6, 0x9f, 0x24, 0x45, 0xdf, 0x4f, 0x9b, 0x17, 0xad, 0x2b, 0x41, 0x7b, 0xe6, 0x6c, 0x37, 0x10 };
  uint8_t out[] = { 0x76, 0x49, 0xab, 0xac, 0x81, 0x19, 0xb2, 0x46, 0xce, 0xe9, 0x8e, 0x9b, 0x12, 0xe9, 0x19, 0x7d,
                    0x50, 0x86, 0xcb, 0x9b, 0x50, 0x72, 0x19, 0xee, 0x95, 0xdb, 0x11, 0x3a, 0x91, 0x76, 0x78, 0xb2,
                    0x73, 0xbe, 0xd6, 0xb8, 0xe3, 0xc1, 0x74, 0x3b, 0x71, 0x16, 0xe6, 0x9e, 0x22, 0x22, 0x95, 0x16, 
                    0x3f, 0xf1, 0xca, 0xa1, 0x68, 0x1f, 0xac, 0x09, 0x12, 0x0e, 0xca, 0x30, 0x75, 0x86, 0xe1, 0xa7 };
  uint8_t buffer[64];

  AES128_CBC_encrypt_buffer(buffer, in, 64, key, iv);

  NRF_LOG_RAW_INFO("CBC encrypt: ");

  if(0 == strncmp((char*) out, (char*) buffer, 64))
  {
    NRF_LOG_RAW_INFO("SUCCESS!\n");
  }
  else
  {
    NRF_LOG_RAW_INFO("FAILURE!\n");
  }
}




void TestAES()
{

    //test_encrypt_cbc();
    //test_decrypt_cbc();
    //test_encrypt_ecb();
    //test_decrypt_ecb();

	//CarEncryptEcb();
	//CarDecryptEcb();

#if 0
	sprintf(SystemManager.TmpBuff2, "%s", "123456789012345678901234");
	AppEnryptCbc( SystemManager.TmpBuff2, SystemManager.TmpBuff);
	memset(SystemManager.TmpBuff2, 0, 40);
	AppDecryptCbc(SystemManager.TmpBuff, SystemManager.TmpBuff2);
#endif
	
    
    //test_encrypt_ecb_verbose();

}




int AppParkingCheck(char* data)
{
	unsigned char i;
	char* pData = data;

	if( *data != 0x02  )
		return 0;

	
	data++;
	if( *data != 0x01  )
		return 0;

	data++;
	if( *data != 0x1A  )
		return 0;

	data++;
	if( *data != 0x19  )
		return 0;

	data++;
	if( *data != 0xff  )
		return 0;


	AppDecryptCbc(&pData[12], SystemManager.TmpBuff);
	
    NRF_LOG_RAW_INFO("App Park Info ==> ");
    for(i = 0; i < 16; ++i)
        NRF_LOG_RAW_INFO("%02x ", SystemManager.TmpBuff[i] );
    NRF_LOG_RAW_INFO("\n");


	SystemManager.Site[0] = SystemManager.TmpBuff[0];
	SystemManager.Site[1] = SystemManager.TmpBuff[1];
	SystemManager.Site[2] = SystemManager.TmpBuff[2];
	SystemManager.Site[3] = SystemManager.TmpBuff[3];

	SystemManager.Dong[0] = SystemManager.TmpBuff[4];
	SystemManager.Dong[1] = SystemManager.TmpBuff[5];

	SystemManager.Ho[0] = SystemManager.TmpBuff[6];
	SystemManager.Ho[1] = SystemManager.TmpBuff[7];

	if( SystemTimer.SVR_SEND_TIMER > 1000 )
	{		
		SendParkingApp();
	}

	SystemTimer.SVR_SEND_TIMER = 0;

	return 1;

}


int AppParkingCheck_IOS(char* data)
{
	unsigned char i;
	char* pData = data;

	AppDecryptCbc(&pData[8], SystemManager.TmpBuff);
	
    NRF_LOG_RAW_INFO("App Park Info ==> ");
    for(i = 0; i < 16; ++i)
        NRF_LOG_RAW_INFO("%02x ", SystemManager.TmpBuff[i] );
    NRF_LOG_RAW_INFO("\n");

	SystemManager.Site[0] = SystemManager.TmpBuff[0];
	SystemManager.Site[1] = SystemManager.TmpBuff[1];
	SystemManager.Site[2] = SystemManager.TmpBuff[2];
	SystemManager.Site[3] = SystemManager.TmpBuff[3];

	SystemManager.Dong[0] = SystemManager.TmpBuff[4];
	SystemManager.Dong[1] = SystemManager.TmpBuff[5];

	SystemManager.Ho[0] = SystemManager.TmpBuff[6];
	SystemManager.Ho[1] = SystemManager.TmpBuff[7];

	
	if( SystemTimer.SVR_SEND_TIMER > 1000 )
	{		
		SendParkingApp();
	}
	
	SystemTimer.SVR_SEND_TIMER = 0;

	return 1;

}




