#include "modbus_memory.h"
#include "modbus_points.h"

#include <string.h>

static modbus_point_type_t mem_to_point_type(modbus_mem_type_t type)
{
    switch (type)
    {
        case MB_MEM_COIL:
            return MB_POINT_COIL;

        case MB_MEM_DISCRETE_INPUT:
            return MB_POINT_DISCRETE_INPUT;

        case MB_MEM_INPUT_REGISTER:
            return MB_POINT_INPUT_REGISTER;

        case MB_MEM_HOLDING_REGISTER:
            return MB_POINT_HOLDING_REGISTER;

        default:
            return MB_POINT_INVALID;
    }
}

static modbus_point_t *find_point_rw(modbus_memory_t *mem,
                                     modbus_mem_type_t mem_type,
                                     uint16_t offset)
{
    size_t i;
    modbus_point_type_t point_type;

    if (mem == NULL || mem->db == NULL)
        return NULL;

    point_type = mem_to_point_type(mem_type);
    if (point_type == MB_POINT_INVALID)
        return NULL;

    for (i = 0; i < mem->db->count; i++)
    {
        if (mem->db->points[i].type == point_type &&
            mem->db->points[i].offset == offset)
        {
            return &mem->db->points[i];
        }
    }

    return NULL;
}

static const modbus_point_t *find_point_ro(modbus_memory_t *mem,
                                           modbus_mem_type_t mem_type,
                                           uint16_t offset)
{
    size_t i;
    modbus_point_type_t point_type;

    if (mem == NULL || mem->db == NULL)
        return NULL;

    point_type = mem_to_point_type(mem_type);
    if (point_type == MB_POINT_INVALID)
        return NULL;

    for (i = 0; i < mem->db->count; i++)
    {
        if (mem->db->points[i].type == point_type &&
            mem->db->points[i].offset == offset)
        {
            return &mem->db->points[i];
        }
    }

    return NULL;
}

static modbus_point_t *find_pics_point_rw(modbus_memory_t *mem,
                                          modbus_mem_type_t mem_type,
                                          uint16_t offset)
{
    modbus_point_type_t point_type;
    const modbus_point_t *pt;

    if (mem == NULL || mem->db == NULL)
        return NULL;

    point_type = mem_to_point_type(mem_type);
    if (point_type == MB_POINT_INVALID)
        return NULL;

    pt = modbus_points_find_by_pics_range(mem->db, point_type, offset);
    return (modbus_point_t *)pt;
}

static bool write_point_words(modbus_memory_t *mem,
                              modbus_point_t *pt,
                              const uint16_t *words,
                              uint8_t span)
{
    uint8_t i;
    modbus_mem_type_t mem_type;

    if (mem == NULL || pt == NULL || words == NULL)
        return false;

    if (modbus_points_is_bit_type(pt->type))
    {
        pt->bit_value = (words[0] != 0U) ? 1U : 0U;
        return true;
    }

    mem_type = modbus_points_type_to_mem(pt->type);

    for (i = 0; i < span; i++)
    {
        modbus_point_t *word_pt;

        word_pt = find_point_rw(mem, mem_type, (uint16_t)(pt->offset + i));
        if (word_pt == NULL)
            return false;

        word_pt->reg_value = words[i];
    }

    return true;
}

static void sync_points_controlled_by(modbus_memory_t *mem,
                                      const modbus_point_t *control_pt)
{
    size_t i;
    uint8_t j;
    modbus_mem_type_t control_mem_type;
    modbus_mem_type_t target_mem_type;

    if (mem == NULL || mem->db == NULL || control_pt == NULL)
        return;

    for (i = 0; i < mem->db->count; i++)
    {
        modbus_point_t *target_pt = &mem->db->points[i];

        if (!target_pt->has_control_address)
            continue;

        if (target_pt->control_type != control_pt->type)
            continue;

        if (target_pt->control_offset != control_pt->offset)
            continue;

        if (modbus_points_is_bit_type(target_pt->type))
        {
            target_pt->bit_value = control_pt->bit_value;
            continue;
        }

        control_mem_type = modbus_points_type_to_mem(control_pt->type);
        target_mem_type = modbus_points_type_to_mem(target_pt->type);

        for (j = 0; j < target_pt->reg_span; j++)
        {
            const modbus_point_t *src_pt;
            modbus_point_t *dst_pt;

            src_pt = find_point_ro(mem, control_mem_type,
                                   (uint16_t)(control_pt->offset + j));
            dst_pt = find_point_rw(mem, target_mem_type,
                                   (uint16_t)(target_pt->offset + j));

            if (src_pt == NULL || dst_pt == NULL)
                break;

            dst_pt->reg_value = src_pt->reg_value;
        }
    }
}

static bool apply_pics_to_official_point(modbus_memory_t *mem, modbus_point_t *pt)
{
    uint16_t out_words[4] = {0U, 0U, 0U, 0U};

    if (mem == NULL || pt == NULL || !pt->has_pics_address)
        return false;

    switch (pt->pics_data_type)
    {
        case PICS_DATA_BOOLEAN:
        {
            uint16_t v = (pt->pics_words[0] != 0U) ? 1U : 0U;

            if (modbus_points_is_bit_type(pt->type))
            {
                pt->bit_value = (v != 0U) ? 1U : 0U;
                sync_points_controlled_by(mem, pt);
                return true;
            }

            out_words[0] = v;
            if (!write_point_words(mem, pt, out_words, 1U))
                return false;

            sync_points_controlled_by(mem, pt);
            return true;
        }

        case PICS_DATA_INTEGER:
        {
            uint32_t raw_u32;
            int32_t pics_i32;

            raw_u32 = ((uint32_t)pt->pics_words[1] << 16) |
                      (uint32_t)pt->pics_words[0];
            pics_i32 = (int32_t)raw_u32;

            switch (pt->data_type)
            {
                case MB_DATA_UINT16:
                {
                    uint32_t v = (pics_i32 < 0) ? 0U : (uint32_t)pics_i32;
                    if (v > 65535U)
                        v = 65535U;

                    out_words[0] = (uint16_t)v;
                    if (!write_point_words(mem, pt, out_words, 1U))
                        return false;
                    break;
                }

                case MB_DATA_UINT32:
                {
                    uint32_t v = (pics_i32 < 0) ? 0U : (uint32_t)pics_i32;

                    out_words[0] = (uint16_t)((v >> 16) & 0xFFFFU);
                    out_words[1] = (uint16_t)(v & 0xFFFFU);

                    if (!write_point_words(mem, pt, out_words, 2U))
                        return false;
                    break;
                }

                case MB_DATA_FLOAT:
                {
                    float f = (float)pics_i32;
                    uint32_t raw_f32;

                    memcpy(&raw_f32, &f, sizeof(raw_f32));

                    out_words[0] = (uint16_t)((raw_f32 >> 16) & 0xFFFFU);
                    out_words[1] = (uint16_t)(raw_f32 & 0xFFFFU);

                    if (!write_point_words(mem, pt, out_words, 2U))
                        return false;
                    break;
                }

                case MB_DATA_DOUBLE:
                {
                    double d = (double)pics_i32;
                    uint64_t raw_f64;

                    memcpy(&raw_f64, &d, sizeof(raw_f64));

                    out_words[0] = (uint16_t)((raw_f64 >> 48) & 0xFFFFU);
                    out_words[1] = (uint16_t)((raw_f64 >> 32) & 0xFFFFU);
                    out_words[2] = (uint16_t)((raw_f64 >> 16) & 0xFFFFU);
                    out_words[3] = (uint16_t)(raw_f64 & 0xFFFFU);

                    if (!write_point_words(mem, pt, out_words, 4U))
                        return false;
                    break;
                }

                default:
                    return false;
            }

            sync_points_controlled_by(mem, pt);
            return true;
        }

        case PICS_DATA_FLOAT:
        {
            uint64_t raw_f64;
            double pics_d;

            raw_f64 = ((uint64_t)pt->pics_words[0] << 48) |
                      ((uint64_t)pt->pics_words[1] << 32) |
                      ((uint64_t)pt->pics_words[2] << 16) |
                      (uint64_t)pt->pics_words[3];

            memcpy(&pics_d, &raw_f64, sizeof(pics_d));

            switch (pt->data_type)
            {
                case MB_DATA_UINT16:
                {
                    double v = pics_d;
                    if (v < 0.0)
                        v = 0.0;
                    if (v > 65535.0)
                        v = 65535.0;

                    out_words[0] = (uint16_t)v;
                    if (!write_point_words(mem, pt, out_words, 1U))
                        return false;
                    break;
                }

                case MB_DATA_UINT32:
                {
                    double v = pics_d;
                    uint32_t u32;

                    if (v < 0.0)
                        v = 0.0;
                    if (v > 4294967295.0)
                        v = 4294967295.0;

                    u32 = (uint32_t)v;

                    out_words[0] = (uint16_t)((u32 >> 16) & 0xFFFFU);
                    out_words[1] = (uint16_t)(u32 & 0xFFFFU);

                    if (!write_point_words(mem, pt, out_words, 2U))
                        return false;
                    break;
                }

                case MB_DATA_FLOAT:
                {
                    float f = (float)pics_d;
                    uint32_t raw_f32;

                    memcpy(&raw_f32, &f, sizeof(raw_f32));

                    out_words[0] = (uint16_t)((raw_f32 >> 16) & 0xFFFFU);
                    out_words[1] = (uint16_t)(raw_f32 & 0xFFFFU);

                    if (!write_point_words(mem, pt, out_words, 2U))
                        return false;
                    break;
                }

                case MB_DATA_DOUBLE:
                {
                    uint64_t raw_out_f64;

                    memcpy(&raw_out_f64, &pics_d, sizeof(raw_out_f64));

                    out_words[0] = (uint16_t)((raw_out_f64 >> 48) & 0xFFFFU);
                    out_words[1] = (uint16_t)((raw_out_f64 >> 32) & 0xFFFFU);
                    out_words[2] = (uint16_t)((raw_out_f64 >> 16) & 0xFFFFU);
                    out_words[3] = (uint16_t)(raw_out_f64 & 0xFFFFU);

                    if (!write_point_words(mem, pt, out_words, 4U))
                        return false;
                    break;
                }

                default:
                    return false;
            }

            sync_points_controlled_by(mem, pt);
            return true;
        }

        default:
            return false;
    }
}

static bool write_pics_staging_word(modbus_memory_t *mem,
                                    modbus_mem_type_t mem_type,
                                    uint16_t offset,
                                    uint16_t value)
{
    modbus_point_t *pt;
    uint16_t word_index;

    pt = find_pics_point_rw(mem, mem_type, offset);
    if (pt == NULL)
        return false;

    if (!pt->has_pics_address || pt->pics_reg_span == 0U)
        return false;

    if (offset < pt->pics_offset)
        return false;

    word_index = (uint16_t)(offset - pt->pics_offset);
    if (word_index >= pt->pics_reg_span || word_index >= 4U)
        return false;

    pt->pics_words[word_index] = value;
    return true;
}

bool modbus_memory_init(modbus_memory_t *mem, modbus_db_t *db)
{
    if (mem == NULL || db == NULL)
        return false;

    mem->db = db;
    modbus_memory_reset(mem);
    return true;
}

void modbus_memory_deinit(modbus_memory_t *mem)
{
    if (mem == NULL)
        return;

    mem->db = NULL;
}

void modbus_memory_reset(modbus_memory_t *mem)
{
    size_t i;

    if (mem == NULL || mem->db == NULL)
        return;

    for (i = 0; i < mem->db->count; i++)
    {
        mem->db->points[i].bit_value = 0U;
        mem->db->points[i].reg_value = 0U;
        memset(mem->db->points[i].pics_words, 0, sizeof(mem->db->points[i].pics_words));
    }
}

bool modbus_memory_read_coil(modbus_memory_t *mem, uint16_t offset, uint8_t *value)
{
    const modbus_point_t *pt;

    if (mem == NULL || value == NULL)
        return false;

    pt = find_point_ro(mem, MB_MEM_COIL, offset);
    if (pt == NULL)
        return false;

    *value = (pt->bit_value != 0U) ? 1U : 0U;
    return true;
}

bool modbus_memory_write_coil(modbus_memory_t *mem, uint16_t offset, uint8_t value)
{
    modbus_point_t *pt;

    if (mem == NULL)
        return false;

    pt = find_point_rw(mem, MB_MEM_COIL, offset);
    if (pt != NULL)
    {
        pt->bit_value = (value != 0U) ? 1U : 0U;
        sync_points_controlled_by(mem, pt);
        return true;
    }

    pt = find_pics_point_rw(mem, MB_MEM_COIL, offset);
    if (pt != NULL)
    {
        uint16_t staged = (value != 0U) ? 1U : 0U;
        return write_pics_staging_word(mem, MB_MEM_COIL, offset, staged);
    }

    return false;
}

bool modbus_memory_read_discrete_input(modbus_memory_t *mem, uint16_t offset, uint8_t *value)
{
    const modbus_point_t *pt;

    if (mem == NULL || value == NULL)
        return false;

    pt = find_point_ro(mem, MB_MEM_DISCRETE_INPUT, offset);
    if (pt == NULL)
        return false;

    *value = (pt->bit_value != 0U) ? 1U : 0U;
    return true;
}

bool modbus_memory_write_discrete_input(modbus_memory_t *mem, uint16_t offset, uint8_t value)
{
    modbus_point_t *pt;

    if (mem == NULL)
        return false;

    pt = find_point_rw(mem, MB_MEM_DISCRETE_INPUT, offset);
    if (pt == NULL)
        return false;

    pt->bit_value = (value != 0U) ? 1U : 0U;
    return true;
}

bool modbus_memory_read_input_register(modbus_memory_t *mem, uint16_t offset, uint16_t *value)
{
    const modbus_point_t *pt;

    if (mem == NULL || value == NULL)
        return false;

    pt = find_point_ro(mem, MB_MEM_INPUT_REGISTER, offset);
    if (pt == NULL)
        return false;

    *value = pt->reg_value;
    return true;
}

bool modbus_memory_write_input_register(modbus_memory_t *mem, uint16_t offset, uint16_t value)
{
    modbus_point_t *pt;

    if (mem == NULL)
        return false;

    pt = find_point_rw(mem, MB_MEM_INPUT_REGISTER, offset);
    if (pt == NULL)
        return false;

    pt->reg_value = value;
    return true;
}

bool modbus_memory_read_holding_register(modbus_memory_t *mem, uint16_t offset, uint16_t *value)
{
    const modbus_point_t *pt;

    if (mem == NULL || value == NULL)
        return false;

    pt = find_point_ro(mem, MB_MEM_HOLDING_REGISTER, offset);
    if (pt == NULL)
        return false;

    *value = pt->reg_value;
    return true;
}

bool modbus_memory_write_holding_register(modbus_memory_t *mem, uint16_t offset, uint16_t value)
{
    modbus_point_t *pt;

    if (mem == NULL)
        return false;

    pt = find_point_rw(mem, MB_MEM_HOLDING_REGISTER, offset);
    if (pt != NULL)
    {
        pt->reg_value = value;
        sync_points_controlled_by(mem, pt);
        return true;
    }

    if (write_pics_staging_word(mem, MB_MEM_HOLDING_REGISTER, offset, value))
        return true;

    return false;
}

bool modbus_memory_read_bit(modbus_memory_t *mem,
                            modbus_mem_type_t type,
                            uint16_t offset,
                            uint8_t *value)
{
    switch (type)
    {
        case MB_MEM_COIL:
            return modbus_memory_read_coil(mem, offset, value);

        case MB_MEM_DISCRETE_INPUT:
            return modbus_memory_read_discrete_input(mem, offset, value);

        default:
            return false;
    }
}

bool modbus_memory_write_bit(modbus_memory_t *mem,
                             modbus_mem_type_t type,
                             uint16_t offset,
                             uint8_t value)
{
    switch (type)
    {
        case MB_MEM_COIL:
            return modbus_memory_write_coil(mem, offset, value);

        case MB_MEM_DISCRETE_INPUT:
            return modbus_memory_write_discrete_input(mem, offset, value);

        default:
            return false;
    }
}

bool modbus_memory_read_reg(modbus_memory_t *mem,
                            modbus_mem_type_t type,
                            uint16_t offset,
                            uint16_t *value)
{
    switch (type)
    {
        case MB_MEM_INPUT_REGISTER:
            return modbus_memory_read_input_register(mem, offset, value);

        case MB_MEM_HOLDING_REGISTER:
            return modbus_memory_read_holding_register(mem, offset, value);

        default:
            return false;
    }
}

bool modbus_memory_write_reg(modbus_memory_t *mem,
                             modbus_mem_type_t type,
                             uint16_t offset,
                             uint16_t value)
{
    switch (type)
    {
        case MB_MEM_INPUT_REGISTER:
            return modbus_memory_write_input_register(mem, offset, value);

        case MB_MEM_HOLDING_REGISTER:
            return modbus_memory_write_holding_register(mem, offset, value);

        default:
            return false;
    }
}

bool modbus_memory_valid_bit_range(modbus_memory_t *mem,
                                   modbus_mem_type_t type,
                                   uint16_t start_offset,
                                   uint16_t quantity)
{
    uint32_t end_offset;

    (void)type;

    if (mem == NULL || mem->db == NULL || quantity == 0U)
        return false;

    end_offset = (uint32_t)start_offset + (uint32_t)quantity - 1U;
    if (end_offset > 0xFFFFU)
        return false;

    return true;
}

bool modbus_memory_valid_reg_range(modbus_memory_t *mem,
                                   modbus_mem_type_t type,
                                   uint16_t start_offset,
                                   uint16_t quantity)
{
    uint32_t end_offset;

    (void)type;

    if (mem == NULL || mem->db == NULL || quantity == 0U)
        return false;

    end_offset = (uint32_t)start_offset + (uint32_t)quantity - 1U;
    if (end_offset > 0xFFFFU)
        return false;

    return true;
}

bool modbus_memory_read_coils(modbus_memory_t *mem,
                              uint16_t start_offset,
                              uint16_t quantity,
                              uint8_t *dest)
{
    uint16_t i;

    if (dest == NULL || !modbus_memory_valid_bit_range(mem, MB_MEM_COIL, start_offset, quantity))
        return false;

    for (i = 0; i < quantity; i++)
    {
        uint8_t value = 0U;

        if (!modbus_memory_read_coil(mem, (uint16_t)(start_offset + i), &value))
            value = 0U;

        dest[i] = value;
    }

    return true;
}

bool modbus_memory_write_coils(modbus_memory_t *mem,
                               uint16_t start_offset,
                               uint16_t quantity,
                               const uint8_t *src)
{
    uint16_t i;

    if (src == NULL || !modbus_memory_valid_bit_range(mem, MB_MEM_COIL, start_offset, quantity))
        return false;

    for (i = 0; i < quantity; i++)
    {
        if (!modbus_memory_write_coil(mem, (uint16_t)(start_offset + i), src[i]))
            return false;
    }

    return true;
}

bool modbus_memory_read_discrete_inputs(modbus_memory_t *mem,
                                        uint16_t start_offset,
                                        uint16_t quantity,
                                        uint8_t *dest)
{
    uint16_t i;

    if (dest == NULL || !modbus_memory_valid_bit_range(mem, MB_MEM_DISCRETE_INPUT, start_offset, quantity))
        return false;

    for (i = 0; i < quantity; i++)
    {
        uint8_t value = 0U;

        if (!modbus_memory_read_discrete_input(mem, (uint16_t)(start_offset + i), &value))
            value = 0U;

        dest[i] = value;
    }

    return true;
}

bool modbus_memory_write_discrete_inputs(modbus_memory_t *mem,
                                         uint16_t start_offset,
                                         uint16_t quantity,
                                         const uint8_t *src)
{
    uint16_t i;

    if (src == NULL || !modbus_memory_valid_bit_range(mem, MB_MEM_DISCRETE_INPUT, start_offset, quantity))
        return false;

    for (i = 0; i < quantity; i++)
    {
        if (!modbus_memory_write_discrete_input(mem, (uint16_t)(start_offset + i), src[i]))
            return false;
    }

    return true;
}

bool modbus_memory_read_input_registers(modbus_memory_t *mem,
                                        uint16_t start_offset,
                                        uint16_t quantity,
                                        uint16_t *dest)
{
    uint16_t i;

    if (dest == NULL || !modbus_memory_valid_reg_range(mem, MB_MEM_INPUT_REGISTER, start_offset, quantity))
        return false;

    for (i = 0; i < quantity; i++)
    {
        uint16_t value = 0U;

        if (!modbus_memory_read_input_register(mem, (uint16_t)(start_offset + i), &value))
            value = 0U;

        dest[i] = value;
    }

    return true;
}

bool modbus_memory_write_input_registers(modbus_memory_t *mem,
                                         uint16_t start_offset,
                                         uint16_t quantity,
                                         const uint16_t *src)
{
    uint16_t i;

    if (src == NULL || !modbus_memory_valid_reg_range(mem, MB_MEM_INPUT_REGISTER, start_offset, quantity))
        return false;

    for (i = 0; i < quantity; i++)
    {
        if (!modbus_memory_write_input_register(mem, (uint16_t)(start_offset + i), src[i]))
            return false;
    }

    return true;
}

bool modbus_memory_read_holding_registers(modbus_memory_t *mem,
                                          uint16_t start_offset,
                                          uint16_t quantity,
                                          uint16_t *dest)
{
    uint16_t i;

    if (dest == NULL || !modbus_memory_valid_reg_range(mem, MB_MEM_HOLDING_REGISTER, start_offset, quantity))
        return false;

    for (i = 0; i < quantity; i++)
    {
        uint16_t value = 0U;

        if (!modbus_memory_read_holding_register(mem, (uint16_t)(start_offset + i), &value))
            value = 0U;

        dest[i] = value;
    }

    return true;
}

bool modbus_memory_write_holding_registers(modbus_memory_t *mem,
                                           uint16_t start_offset,
                                           uint16_t quantity,
                                           const uint16_t *src)
{
    uint16_t i;

    if (src == NULL || quantity == 0U || mem == NULL || mem->db == NULL)
        return false;

    for (i = 0; i < quantity; i++)
    {
        uint16_t off = (uint16_t)(start_offset + i);

        if (find_point_rw(mem, MB_MEM_HOLDING_REGISTER, off) == NULL)
        {
            if (modbus_points_find_by_pics_range(mem->db,
                                                 MB_POINT_HOLDING_REGISTER,
                                                 off) == NULL)
            {
                return false;
            }
        }
    }

    for (i = 0; i < quantity; i++)
    {
        if (!modbus_memory_write_holding_register(mem,
                                                  (uint16_t)(start_offset + i),
                                                  src[i]))
        {
            return false;
        }
    }

    {
        modbus_point_t *pics_pt;

        pics_pt = find_pics_point_rw(mem, MB_MEM_HOLDING_REGISTER, start_offset);
        if (pics_pt != NULL)
        {
            if (!apply_pics_to_official_point(mem, pics_pt))
                return false;
        }
    }

    return true;
}