#include "EEPROM.h"

/* ----------------------------------- EEPROM TWI ---------------------------------------------*/
/** TWI driver instance. */
static Twid twid;

void Twi_inst_init(void)
{
    TWID_Initialize(&twid, TWI0);
}

float EEPROM_Read_Param(uint8_t address)
{
    uint8_t RData[2];
    int16_t aux;
    float exit_value;

    TWID_Write(&twid, EEPROM_CONTROL_BYTE | EEPROM_ADDR, 0, 0x0, &address, 0x01, 0); /* set start address */
    TWID_Read(&twid, EEPROM_CONTROL_BYTE | EEPROM_ADDR, 0, 0x0, RData, 0x02, 0);
    aux = (RData[0] << 8) | RData[1];
    
    if (address != Ts_addr)
        exit_value = (float)aux/100;
    return exit_value;
}

void EEPROM_Write_Param(uint8_t address, float value)
{
    uint8_t Tdata[2];
    int16_t aux;

    if (address != Ts_addr)
        aux = value*100;
    else
        aux = value;

    Tdata[0] = aux >> 8; /* high byte */
    Tdata[1] = aux & 255; /* low byte*/
    TWID_Write(&twid, EEPROM_CONTROL_BYTE | EEPROM_ADDR, address, 0x01, Tdata, 0x02, 0);
}

void PSD_Param_Init(float *K, float *Ti, float *Td, float *N, uint16_t *Ts, float *w)
{
    uint8_t  Param_array[16];
    uint8_t start_addr = K_addr;

    TWID_Write(&twid, EEPROM_CONTROL_BYTE | EEPROM_ADDR, 0, 0x0, &start_addr, 0x01, 0); /* set start address */
    TWID_Read(&twid, EEPROM_CONTROL_BYTE | EEPROM_ADDR, 0, 0x0, Param_array, 0x0C, 0);

    *K = (float)((Param_array[0] << 8) | Param_array[1])/100;
    *Ti = (float)((Param_array[2] << 8) | Param_array[3])/100;
    *Td = (float)((Param_array[4] << 8) | Param_array[5])/100;
    *N = (float)((Param_array[6] << 8) | Param_array[7])/100;
    *Ts = (Param_array[8] << 8) | Param_array[9];   /* in ms */
    *w = (float)((Param_array[10] << 8) | Param_array[11])/100;
}