#include "cfg.h"

#include <avr/io.h>
#include <avr/eeprom.h>

#include "constants.h"

#define SIZEOF_ARRAY(a) (sizeof(a)/sizeof(a[0]))
#define CFG_FILE_LENGTH 2048

cfg_loco EEMEM e_locomotive_list[CFG_LOCO_LIST_LENGTH];
cfg_address_type EEMEM e_accessory_list[CFG_ACCESSORY_LIST_LENGTH + 1];
cfg_address_type EEMEM e_accessory_on_list[CFG_ACCESSORY_LIST_LENGTH + 1];
cfg_address_type EEMEM e_accessory_hotlist[8];

// Bubble sort a list into ascending order.  The end of the list is denoted by
// CFG_NULL_ADDR.
void bubble_sort(cfg_address_type *list)
{
    size_t length = 0;
    while(list[length] != CFG_NULL_ADDR)
        ++length;
    // Bubble sort the list.
    for(cfg_loco_index_type i = 0; length && i < length - 1; ++i)
    {
        for(cfg_loco_index_type ii = 0; ii < length - i - 1; ++ii)
        {
            if(list[ii] > list[ii + 1])
            {
                cfg_address_type tmp = list[ii];
                list[ii] = list[ii + 1];
                list[ii + 1] = tmp;
            }
        }
    }
}

void Cfg_Reset()
{
    // EEPROM is written in 8-byte pages.  Check if each page is clear; if not,
    // write the default value (0xFF) to each byte in the page.
    const uint8_t clear[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
    uint8_t data[8];
    for(unsigned int addr = 0; addr < CFG_FILE_LENGTH; addr += 8)
    {
        eeprom_read_block((void *)&data[0], (const void *)addr, 8);
        for(unsigned char i = 0; i < 8; ++i)
        {
            if(data[i] != 0xFF)
            {
                eeprom_write_block((const void*)&clear[0], (void*)addr, 8);
                break;
            }
        }
    }

    // Add one locomotive with address 3.
    cfg_loco default_loco = Cfg_DefaultLoco();
    default_loco.address = 3;
    Cfg_WriteLocoToIndex(0, default_loco);
}

bool Cfg_LocoAddressValid(cfg_address_type address)
{
    return (address > 0 && address <= HERMES_MAX_LOCO_ADDRESS);
}

unsigned char Cfg_LocoFunctions(const cfg_loco *loco)
{
    unsigned char count = 0;
    functions_type f = loco->f_enable;
    for(unsigned char i = 0; i < 32; ++i)
    {
        if(f & 1)
            ++count;
        f >>= 1;
    }
    return count;
}

bool Cfg_AccessoryAddressValid(cfg_address_type address)
{
    return (address > 0 && address <= HERMES_MAX_ACC_ADDRESS);
}

cfg_loco_index_type Cfg_AddressCount(
        const cfg_address_type *list
        )
{
    cfg_loco_index_type count = 0;
    while(list[count] != CFG_NULL_ADDR)
        ++count;
    return count;
}

cfg_loco_index_type Cfg_IndexInList(
        cfg_address_type addr,
        const cfg_address_type *list
        )
{
    for(cfg_loco_index_type index = 0; list[index] != CFG_NULL_ADDR; ++index)
    {
        if(list[index] == addr)
            return index;
    }
    return CFG_NULL_INDEX;
}

void Cfg_AddToList(
        cfg_address_type addr,
        cfg_address_type *list
        )
{
    while(*list != CFG_NULL_ADDR && *list < addr)
        ++list;

    if(*list == addr)
        return;

    // *list > addr
    // Move every element in the list back by one space.

    cfg_address_type tmp = *list;
    *list = addr;
    ++list;

    do
    {
        const cfg_address_type tmp2 = *list;
        *list = tmp;
        tmp = tmp2;
        ++list;
    }
    while(tmp != CFG_NULL_ADDR);

    *list = CFG_NULL_ADDR;
}

void Cfg_RemoveFromList(
        cfg_address_type addr,
        cfg_address_type *list
        )
{
    while(*list != CFG_NULL_ADDR && *list != addr)
        ++list;

    if(*list == CFG_NULL_ADDR)
        // 'addr' was not found.
        return;

    // Move each item one place towards the start of the list.
    do
    {
        *list = *(list + 1);
        ++list;
    }
    while(*list != CFG_NULL_ADDR);
}

cfg_address_type Cfg_NextInList(
        cfg_address_type *const list,
        cfg_address_type current
        )
{
    for(int index = 0; list[index] != CFG_NULL_ADDR; ++index)
    {
        if(list[index] == current)
        {
            // Return the next item in the list (wrapping if required).
            return list[(index + 1) % Cfg_AddressCount(list)];
        }
    }
    // The item was not found in the list.
    return CFG_NULL_ADDR;
}

cfg_address_type Cfg_PrevInList(
        cfg_address_type *const list,
        cfg_address_type current
        )
{
    for(int index = 0; list[index] != CFG_NULL_ADDR; ++index)
    {
        if(list[index] == current)
        {
            // Return the previous item in the list (wrapping if required).
            return list[(Cfg_AddressCount(list) + index - 1) % Cfg_AddressCount(list)];
        }
    }
    // The item was not found in the list.
    return CFG_NULL_ADDR;
}

cfg_loco Cfg_DefaultLoco()
{
    cfg_loco out = {
        .address = CFG_NULL_ADDR,
        .speed_steps = SPEED_STEPS_128,
        .stop_mode = STOP_MODE_ON_ENTERING_STATION,
        .f_default = 0,
        .f_enable = 0,
        .f_momentary = 0,
        .f_list = {
            CFG_NULL_FN, CFG_NULL_FN, CFG_NULL_FN, CFG_NULL_FN,
            CFG_NULL_FN, CFG_NULL_FN, CFG_NULL_FN, CFG_NULL_FN
        }
    };
    return out;
}

cfg_loco Cfg_ReadLoco(cfg_address_type address)
{
    cfg_loco_index_type index = Cfg_AddrToIndex(address);
    if(index == CFG_NULL_INDEX)
        return Cfg_DefaultLoco();
    return Cfg_ReadLocoFromIndex(index);
}

cfg_loco Cfg_ReadLocoFromIndex(cfg_loco_index_type index)
{
    cfg_loco out;
    if(index == CFG_NULL_INDEX)
        out = Cfg_DefaultLoco();
    else
        eeprom_read_block(&out, &e_locomotive_list[index], sizeof(cfg_loco));
    return out;
}

bool Cfg_WriteLoco(cfg_loco loco)
{
    cfg_loco_index_type index = Cfg_AddrToIndex(loco.address);
    if(index == CFG_NULL_INDEX)
        index = Cfg_NextFreeLoco();
    if(index == CFG_NULL_INDEX)
        return false;
    Cfg_WriteLocoToIndex(index, loco);
    return true;
}

void Cfg_WriteLocoToIndex(
        cfg_loco_index_type index,
        cfg_loco loco
        )
{
    eeprom_update_block(&loco, &e_locomotive_list[index], sizeof(cfg_loco));
}

void Cfg_Addresses(cfg_address_type *list)
{
    cfg_loco_index_type i_out = 0;
    for(cfg_loco_index_type i = 0; i < CFG_LOCO_LIST_LENGTH; ++i)
    {
        cfg_loco loco = Cfg_ReadLocoFromIndex(i);
        if(loco.address != CFG_NULL_ADDR)
            list[i_out++] = loco.address;
    }
    list[i_out] = CFG_NULL_ADDR;

    bubble_sort(list);
}

cfg_loco_index_type Cfg_NextFreeLoco()
{
    for(cfg_loco_index_type i = 0; i < CFG_LOCO_LIST_LENGTH; ++i)
    {
        cfg_loco loco = Cfg_ReadLocoFromIndex(i);
        if(loco.address == CFG_NULL_ADDR)
            return i;
    }
    return CFG_NULL_INDEX;
}

cfg_loco_index_type Cfg_AddrToIndex(cfg_address_type address)
{
    for(cfg_loco_index_type i = 0; i < CFG_LOCO_LIST_LENGTH; ++i)
    {
        cfg_loco loco = Cfg_ReadLocoFromIndex(i);
        if(loco.address == address)
            return i;
    }
    return CFG_NULL_INDEX;
}

void Cfg_Accessories(cfg_address_type *list)
{
    cfg_loco_index_type i_out = 0;
    for(cfg_loco_index_type i = 0; i < CFG_ACCESSORY_LIST_LENGTH; ++i)
    {
        cfg_address_type address = eeprom_read_word(&e_accessory_list[i]);
        if(address != CFG_NULL_ADDR)
            list[i_out++] = address;
    }
    list[i_out] = CFG_NULL_ADDR;

    bubble_sort(list);
}

void Cfg_SaveAccessory(cfg_address_type accessory_address)
{
    for(size_t i = 0; i < CFG_ACCESSORY_LIST_LENGTH; ++i)
    {
        cfg_address_type address = eeprom_read_word(&e_accessory_list[i]);
        if(address == accessory_address)
            return;
    }
    for(size_t i = 0; i < CFG_ACCESSORY_LIST_LENGTH; ++i)
    {
        cfg_address_type address = eeprom_read_word(&e_accessory_list[i]);
        if(address == CFG_NULL_ADDR)
        {
            eeprom_write_word(
                    &e_accessory_list[i],
                    (unsigned int)accessory_address
                    );
            return;
        }
    }
}

void Cfg_DeleteAccessory(cfg_address_type accessory_address)
{
    for(size_t i = 0; i < CFG_ACCESSORY_LIST_LENGTH; ++i)
    {
        cfg_address_type address = eeprom_read_word(&e_accessory_list[i]);
        if(address == accessory_address)
            eeprom_write_word(&e_accessory_list[i], CFG_NULL_ADDR);
    }
}

void Cfg_ReadAccessoryOn(cfg_address_type *accessories)
{
    for(unsigned char i = 0; i <= CFG_ACCESSORY_LIST_LENGTH; ++i)
    {
        accessories[i] = eeprom_read_word(&e_accessory_on_list[i]);

        if(accessories[i] == CFG_NULL_ADDR)
            break;
    }
}

void Cfg_SaveAccessoryOn(cfg_address_type *accessories)
{
    for(unsigned char i = 0; i <= CFG_ACCESSORY_LIST_LENGTH; ++i)
    {
        // Write the accessory address
        eeprom_write_word(&e_accessory_on_list[i], accessories[i]);

        if(accessories[i] == CFG_NULL_ADDR)
            break;
    }
}

void Cfg_ReadAccessoryHotlist(cfg_address_type *accessories)
{
    eeprom_read_block(
            (void *)accessories,
            (const void *)e_accessory_hotlist,
            sizeof(cfg_address_type) * 8
            );
}

void Cfg_SaveAccessoryHotlist(cfg_address_type *accessories)
{
    eeprom_update_block(
            (void *)accessories,
            (void *)e_accessory_hotlist,
            sizeof(cfg_address_type) * 8
            );
}