/*******************************************************************************
 * @file	cat_interface.c
 * @brief   
 ******************************************************************************/

/* board driver */
#include <stdio.h>
#include <string.h>
#include <stdarg.h>

#include <stdbool.h>
#include "nrf.h"
#include "app_error.h"
#include "boards.h"
#include "nrfx_gpiote.h"
#include "nrfx_twi.h"
#include "nrf_crypto.h"
#include "nrf_crypto_aes.h"

#include "nrf_drv_twi.h"
#include "system_interface.h"
#include "nrf_delay.h"
#include "cat_interface.h"
#include "debug_print.h"
/* I2C number and slave address for INV device */
#define ICM_I2C_ADDR     0x68
#define INV_MAX_SERIAL_WRITE 16


#define EEPROM_I2C_ADDRESS  0x50
#define EEPROM_PAGE_SIZE    64
#define EEPROM_INSTANCE 0



#define AES_BLOCK_SIZE 16

static uint8_t aes_key[16] = {
    0x60, 0x3d, 0xeb, 0x10, 0x15, 0xca, 0x71, 0xbe,
    0x2b, 0x73, 0xae, 0xf0, 0x85, 0x7d, 0x77, 0x81
};

static uint8_t aes_iv[16] = { 0 };  // Fixed IV for simplicity




const nrfx_twi_t m_eeprom = NRFX_TWI_INSTANCE(EEPROM_INSTANCE);





ret_code_t encrypt_data(const uint8_t *input, size_t length, uint8_t *output, size_t *output_len) {
    nrf_crypto_aes_context_t aes_ctx;
    size_t padded_len = ((length + AES_BLOCK_SIZE - 1) / AES_BLOCK_SIZE) * AES_BLOCK_SIZE;

    uint8_t buffer[padded_len];
    memcpy(buffer, input, length);
    memset(&buffer[length], 0, padded_len - length); // Zero padding

    ret_code_t err = nrf_crypto_aes_init(&aes_ctx, &g_nrf_crypto_aes_cbc_128_info, NRF_CRYPTO_ENCRYPT);
    VERIFY_SUCCESS(err);

    err = nrf_crypto_aes_key_set(&aes_ctx, aes_key);
    VERIFY_SUCCESS(err);

    err = nrf_crypto_aes_iv_set(&aes_ctx, aes_iv);
    VERIFY_SUCCESS(err);

    *output_len = padded_len;
    err = nrf_crypto_aes_finalize(&aes_ctx, buffer, padded_len, output, output_len);
    return err;
}

ret_code_t decrypt_data(const uint8_t *input, size_t length, uint8_t *output) {
    nrf_crypto_aes_context_t aes_ctx;
		uint8_t input_copy[length];
		memcpy(input_copy, input, length);

    ret_code_t err = nrf_crypto_aes_init(&aes_ctx, &g_nrf_crypto_aes_cbc_128_info, NRF_CRYPTO_DECRYPT);
    VERIFY_SUCCESS(err);

    err = nrf_crypto_aes_key_set(&aes_ctx, aes_key);
    VERIFY_SUCCESS(err);

    err = nrf_crypto_aes_iv_set(&aes_ctx, aes_iv);
    VERIFY_SUCCESS(err);

    size_t output_len = length;
    err = nrf_crypto_aes_finalize(&aes_ctx, input_copy, length, output, &output_len);
    return err;
}

ret_code_t eeprom_write_encrypted(uint16_t mem_address, const uint8_t *plaintext, size_t length) {
    uint8_t encrypted_buf[256];  // Adjust if needed
    size_t encrypted_len = 0;

    ret_code_t err = encrypt_data(plaintext, length, encrypted_buf, &encrypted_len);
    VERIFY_SUCCESS(err);

    return eeprom_write_bytes(mem_address, encrypted_buf, encrypted_len);
}


ret_code_t eeprom_read_decrypted(uint16_t mem_address, uint8_t *plaintext, size_t original_length) {
    size_t encrypted_len = ((original_length + AES_BLOCK_SIZE - 1) / AES_BLOCK_SIZE) * AES_BLOCK_SIZE;
    uint8_t encrypted_buf[256];  // Adjust as needed

    ret_code_t err = eeprom_read_bytes(mem_address, encrypted_buf, encrypted_len);
    VERIFY_SUCCESS(err);

    return decrypt_data(encrypted_buf, encrypted_len, plaintext);
}




void eeprom_uninitialize(void){
    nrfx_twi_disable(&m_eeprom);
    nrfx_twi_uninit(&m_eeprom);
}

void eeprom_initialize(void){
    ret_code_t err_code;

    const nrfx_twi_config_t eeprom_config = {
       .scl                = ICM42670_I2C_SCL_PIN,
       .sda                = ICM42670_I2C_SDA_PIN,
       .frequency          = NRF_TWI_FREQ_400K,
       .interrupt_priority = APP_IRQ_PRIORITY_HIGH,
    };

    err_code = nrfx_twi_init(&m_eeprom, &eeprom_config, NULL, NULL);
    APP_ERROR_CHECK(err_code);

    nrfx_twi_enable(&m_eeprom);
}

ret_code_t eeprom_write_page(uint16_t mem_address, const uint8_t *data)
{
	  
    uint8_t buffer[2 + EEPROM_PAGE_SIZE];  // 2 bytes for address + 64 bytes data
    ret_code_t ret;

    buffer[0] = (uint8_t)(mem_address >> 8);     // MSB
    buffer[1] = (uint8_t)(mem_address & 0xFF);   // LSB
    memcpy(&buffer[2], data, EEPROM_PAGE_SIZE);

    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, buffer, 2 + EEPROM_PAGE_SIZE, false);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM write failed (code: %d)\n", ret);
        return ret;
    }

     // Wait for internal EEPROM write cycle (typically ~5ms)
    for (int i = 0; i < 100; i++) {
    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, NULL, 0, false);
    if (ret == NRF_SUCCESS)
        break;
    nrf_delay_us(100); // Wait 100us before retry
}

    return NRF_SUCCESS;
}


ret_code_t eeprom_write_uint16_array(uint16_t start_address, const uint16_t *data, size_t count)
{
	  
    if (count != 48) {
        DBG_PRINTF("Error: This function is only for writing exactly 48 uint16_t values.\n");
        return NRF_ERROR_INVALID_PARAM;
    }
		if (start_address < 128) {
        DBG_PRINTF("Error: This function is only for after 192....\n");
        return NRF_ERROR_INVALID_PARAM;
    }

    uint8_t buffer[EEPROM_PAGE_SIZE];
    ret_code_t ret;

    // Write first 32 uint16_t values (64 bytes)
    for (size_t i = 0; i < 32; i++) {
        buffer[i * 2]     = (uint8_t)(data[i] >> 8);     // MSB
        buffer[i * 2 + 1] = (uint8_t)(data[i] & 0xFF);   // LSB
//			  buffer[i * 2 +1]     = (uint8_t)(data[i] >> 8);     // MSB
//        buffer[i * 2 ] = (uint8_t)(data[i] & 0xFF);   // LSB
    }

    ret = eeprom_write_page(start_address, buffer);
    if (ret != NRF_SUCCESS) {
        return ret;
    }

    // Write remaining 16 uint16_t values (32 bytes)
    for (size_t i = 0; i < 16; i++) {
        buffer[i * 2]     = (uint8_t)(data[i + 32] >> 8);
        buffer[i * 2 + 1] = (uint8_t)(data[i + 32] & 0xFF);
    }

    ret = eeprom_write_page(start_address + EEPROM_PAGE_SIZE, buffer); // next page
    return ret;
}



ret_code_t eeprom_read_uint16_array(uint16_t start_address, uint16_t *data, size_t count)
{
	
	  
    if (count != 48) {
        DBG_PRINTF("Error: This function is only for reading exactly 48 uint16_t values.\n");
        return NRF_ERROR_INVALID_PARAM;
    }

    uint8_t buffer[EEPROM_PAGE_SIZE];
    ret_code_t ret;

    // Read first 64 bytes (32 uint16_t)
    ret = eeprom_read_bytes(start_address, buffer, EEPROM_PAGE_SIZE);
    if (ret != NRF_SUCCESS) {
        return ret;
    }

    for (size_t i = 0; i < 32; i++) {
        data[i] = ((uint16_t)buffer[i * 2] << 8) | buffer[i * 2 + 1];
    }

    // Read next 32 bytes (16 uint16_t)
    ret = eeprom_read_bytes(start_address + EEPROM_PAGE_SIZE, buffer, 32);
    if (ret != NRF_SUCCESS) {
        return ret;
    }

    for (size_t i = 0; i < 16; i++) {
        data[i + 32] = ((uint16_t)buffer[i * 2] << 8) | buffer[i * 2 + 1];
    }

    return NRF_SUCCESS;
}

ret_code_t eeprom_read_page(uint16_t mem_address, uint8_t *data)
{
	  
    uint8_t addr_buf[2];
    ret_code_t ret;

    addr_buf[0] = (uint8_t)(mem_address >> 8);
    addr_buf[1] = (uint8_t)(mem_address & 0xFF);

    // Send memory address first
    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, addr_buf, 2, true);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM set read address failed (code: %d)\n", ret);
        return ret;
    }

    // Read the page
    ret = icm42670_twi_rx(EEPROM_I2C_ADDRESS, data, EEPROM_PAGE_SIZE);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM read failed (code: %d)\n", ret);
        return ret;
    }
		DBG_PRINTF("EEPROM read \n");
		for (int i = 0; i < EEPROM_PAGE_SIZE; i++) {
       
            DBG_PRINTF("%02X\n", data[i]);
		}
		DBG_PRINTF("\r\n");
    return NRF_SUCCESS;
}




ret_code_t eeprom_write_byte(uint16_t mem_address, uint8_t data)
{
	  
    uint8_t buffer[3]; // 2 bytes for address + 1 byte data
    ret_code_t ret;

    buffer[0] = (uint8_t)(mem_address >> 8);     // MSB of address
    buffer[1] = (uint8_t)(mem_address & 0xFF);   // LSB of address
    buffer[2] = data;
		 DBG_PRINTF("EEPROM write byte %02X,%02X,%02X\n",  buffer[0], buffer[1], buffer[2]);
    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, buffer, 3, false);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM write byte failed (code: %d)\n", ret);
        return ret;
    }

    // Wait for internal EEPROM write cycle (typically ~5ms)
    for (int i = 0; i < 100; i++) {
    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, NULL, 0, false);
    if (ret == NRF_SUCCESS)
        break;
    nrf_delay_us(100); // Wait 100us before retry
}

    return NRF_SUCCESS;
}

ret_code_t eeprom_read_byte(uint16_t mem_address, uint8_t *data)
{
	  
    uint8_t addr_buf[2];
    ret_code_t ret;

    addr_buf[0] = (uint8_t)(mem_address >> 8);     // MSB of address
    addr_buf[1] = (uint8_t)(mem_address & 0xFF);   // LSB of address

    // Send memory address
    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, addr_buf, 2, true);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM set address failed (code: %d)\n", ret);
        return ret;
    }

    // Read 1 byte
    ret = icm42670_twi_rx(EEPROM_I2C_ADDRESS, data, 1);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM read byte failed (code: %d)\n", ret);
        return ret;
    }

    return NRF_SUCCESS;
}


ret_code_t eeprom_write_bytes(uint16_t mem_address, const uint8_t *data, size_t length)
{
	  
    ret_code_t ret;
    size_t bytes_written = 0;

    while (bytes_written < length) {
        size_t page_offset = mem_address % EEPROM_PAGE_SIZE;
        size_t bytes_to_write = EEPROM_PAGE_SIZE - page_offset;
        if (bytes_to_write > (length - bytes_written)) {
            bytes_to_write = length - bytes_written;
        }

        uint8_t buffer[2 + EEPROM_PAGE_SIZE];  // 2-byte addr + up to 64 data bytes
        buffer[0] = (uint8_t)(mem_address >> 8);
        buffer[1] = (uint8_t)(mem_address & 0xFF);
        memcpy(&buffer[2], &data[bytes_written], bytes_to_write);

        ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, buffer, bytes_to_write + 2, false);
        if (ret != NRF_SUCCESS) {
            DBG_PRINTF("EEPROM write error at addr 0x%04X\n", mem_address);
            return ret;
        }
  // Wait for internal EEPROM write cycle (typically ~5ms)
    for (int i = 0; i < 100; i++) {
    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, NULL, 0, false);
    if (ret == NRF_SUCCESS)
        break;
    nrf_delay_us(100); // Wait 100us before retry
}

        mem_address += bytes_to_write;
        bytes_written += bytes_to_write;
    }

    return NRF_SUCCESS;
}

ret_code_t eeprom_read_bytes(uint16_t mem_address, uint8_t *data, size_t length)
{
	  
    ret_code_t ret;
    uint8_t addr_buf[2];

    addr_buf[0] = (uint8_t)(mem_address >> 8);
    addr_buf[1] = (uint8_t)(mem_address & 0xFF);

    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, addr_buf, 2, true);  // send addr, no stop
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM read set address failed\n");
        return ret;
    }

    ret = icm42670_twi_rx(EEPROM_I2C_ADDRESS, data, length);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM read failed\n");
        return ret;
    }

    return NRF_SUCCESS;
}

ret_code_t eeprom_write_word(uint16_t mem_address, uint16_t data)
{
	
    uint8_t buffer[4]; // 2 bytes for address + 2 bytes for data
    ret_code_t ret;

    buffer[0] = (uint8_t)(mem_address >> 8);       // MSB of address
    buffer[1] = (uint8_t)(mem_address & 0xFF);     // LSB of address
    buffer[2] = (uint8_t)(data & 0xFF);            // LSB of data
    buffer[3] = (uint8_t)(data >> 8);              // MSB of data

    DBG_PRINTF("EEPROM write word %02X,%02X,%02X,%02X\n", buffer[0], buffer[1], buffer[2], buffer[3]);

    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, buffer, 4, false);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM write word failed (code: %d)\n", ret);
        return ret;
    }

    // Wait for internal write cycle (~5ms typical)
    for (int i = 0; i < 100; i++) {
        ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, NULL, 0, false);
        if (ret == NRF_SUCCESS)
            break;
        nrf_delay_us(100); // 100us delay between polling
    }

    return NRF_SUCCESS;
}


ret_code_t eeprom_read_word(uint16_t mem_address, uint16_t *data)
{
	  
    uint8_t addr_buf[2];
    uint8_t read_buf[2];
    ret_code_t ret;

    addr_buf[0] = (uint8_t)(mem_address >> 8);
    addr_buf[1] = (uint8_t)(mem_address & 0xFF);

    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, addr_buf, 2, true);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM set address for word read failed (code: %d)\n", ret);
        return ret;
    }

    ret = icm42670_twi_rx(EEPROM_I2C_ADDRESS, read_buf, 2);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM read word failed (code: %d)\n", ret);
        return ret;
    }

    *data = ((uint16_t)read_buf[1] << 8) | read_buf[0]; // Little-endian

    return NRF_SUCCESS;
}

ret_code_t eeprom_write_uint32(uint16_t mem_address, uint32_t data)
{
	  
    uint8_t buffer[6]; // 2 bytes address + 4 bytes data
    ret_code_t ret;

    buffer[0] = (uint8_t)(mem_address >> 8);     // MSB of address
    buffer[1] = (uint8_t)(mem_address & 0xFF);   // LSB of address
    buffer[2] = (uint8_t)(data >> 24);
    buffer[3] = (uint8_t)(data >> 16);
    buffer[4] = (uint8_t)(data >> 8);
    buffer[5] = (uint8_t)(data);

    DBG_PRINTF("EEPROM write uint32: %02X %02X %02X %02X %02X %02X\n",
           buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);

    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, buffer, 6, false);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM write uint32 failed (code: %d)\n", ret);
        return ret;
    }

    // Wait for internal EEPROM write cycle
    for (int i = 0; i < 100; i++) {
        ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, NULL, 0, false);
        if (ret == NRF_SUCCESS)
            break;
        nrf_delay_us(100);
    }

    return NRF_SUCCESS;
}




ret_code_t eeprom_read_uint32(uint16_t mem_address, uint32_t *data)
{
    uint8_t addr_buf[2];
    uint8_t data_buf[4];
    ret_code_t ret;
  
	
    addr_buf[0] = (uint8_t)(mem_address >> 8);
    addr_buf[1] = (uint8_t)(mem_address & 0xFF);
	
    DBG_PRINTF("EEPROM address:%02X,%02X \n",addr_buf[0],addr_buf[1]);
    // Send memory address to read from
    ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, addr_buf, 2, true);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM read (addr phase) failed (code: %d)\n", ret);
        return ret;
    }

    // Read 4 bytes of data
    ret = icm42670_twi_rx(EEPROM_I2C_ADDRESS, data_buf, 4);
    if (ret != NRF_SUCCESS) {
        DBG_PRINTF("EEPROM read (data phase) failed (code: %d)\n", ret);
        return ret;
    }

    *data = ((uint32_t)data_buf[0] << 24) |
            ((uint32_t)data_buf[1] << 16) |
            ((uint32_t)data_buf[2] << 8) |
            ((uint32_t)data_buf[3]);

    DBG_PRINTF("EEPROM read uint32: %02X %02X %02X %02X -> %08X\n",
           data_buf[0], data_buf[1], data_buf[2], data_buf[3], *data);

    return NRF_SUCCESS;
}