#include "controller.hpp"
#include "codec.hpp"

#include <vector>

// ------------------------------------------------------------
// lifecycle
// ------------------------------------------------------------

Controller::Controller(std::unique_ptr<ITransport> transport)
    : transport_(std::move(transport))
{
}

bool Controller::connect(const char* host, std::uint16_t port)
{
    return transport_ && transport_->connect(host, port);
}

void Controller::disconnect()
{
    if (transport_)
        transport_->disconnect();
}

bool Controller::is_connected() const
{
    return transport_ && transport_->is_connected();
}

void Controller::poll()
{
    if (transport_)
        transport_->poll();
}


// ------------------------------------------------------------
// raw register
// ------------------------------------------------------------

bool Controller::read_register(std::uint16_t addr, std::uint16_t& value)
{
    if (!transport_ || !transport_->can_io())
        return false;

    std::vector<std::uint16_t> regs;
    if (!transport_->read_registers(addr, 1, regs) || regs.size() != 1)
        return false;

    value = regs[0];
    return true;
}

bool Controller::read_raw(std::uint16_t addr, std::uint16_t count, std::vector<std::uint16_t>& out)
{
    if (!transport_ || !transport_->can_io()) return false;
    return transport_->read_registers(addr, count, out);
}

bool Controller::write_register(std::uint16_t addr, std::uint16_t value)
{
    if (!is_connected()) return false;
    return transport_->write_registers(addr, { value });
}

// ------------------------------------------------------------
// 32-bit
// ------------------------------------------------------------

bool Controller::read_int32(std::uint16_t addr,
                            std::int32_t& value,
                            const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    std::vector<std::uint16_t> regs;
    if (!transport_->read_registers(addr, 2, regs) || regs.size() != 2)
        return false;

    value = decode_int32(regs[0], regs[1], fmt);
    return true;
}

bool Controller::write_int32(std::uint16_t addr,
                             std::int32_t value,
                             const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    auto r = encode_int32(value, fmt);
    return transport_->write_registers(addr, { r[0], r[1] });
}

bool Controller::read_uint32(std::uint16_t addr,
                             std::uint32_t& value,
                             const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    std::vector<std::uint16_t> regs;
    if (!transport_->read_registers(addr, 2, regs) || regs.size() != 2)
        return false;

    value = decode_uint32(regs[0], regs[1], fmt);
    return true;
}

bool Controller::write_uint32(std::uint16_t addr,
                              std::uint32_t value,
                              const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    auto r = encode_uint32(value, fmt);
    return transport_->write_registers(addr, { r[0], r[1] });
}

// ------------------------------------------------------------
// float / double
// ------------------------------------------------------------

bool Controller::read_float(std::uint16_t addr,
                            float& value,
                            const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    std::vector<std::uint16_t> regs;
    if (!transport_->read_registers(addr, 2, regs) || regs.size() != 2)
        return false;

    value = decode_float(regs[0], regs[1], fmt);
    return true;
}

bool Controller::write_float(std::uint16_t addr,
                             float value,
                             const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;
    auto r = encode_float(value, fmt);
    return transport_->write_registers(addr, { r[0], r[1] });
}

bool Controller::read_double(std::uint16_t addr,
                             double& value,
                             const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    std::vector<std::uint16_t> regs;
    if (!transport_->read_registers(addr, 4, regs) || regs.size() != 4)
        return false;

    value = decode_double(regs[0], regs[1], regs[2], regs[3], fmt);
    return true;
}

bool Controller::write_double(std::uint16_t addr,
                              double value,
                              const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    auto r = encode_double(value, fmt);
    return transport_->write_registers(addr, { r[0], r[1], r[2], r[3] });
}

// ------------------------------------------------------------
// 64-bit
// ------------------------------------------------------------

bool Controller::read_int64(std::uint16_t addr,
                            std::int64_t& value,
                            const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    std::vector<std::uint16_t> regs;
    if (!transport_->read_registers(addr, 4, regs) || regs.size() != 4)
        return false;

    value = decode_int64(regs[0], regs[1], regs[2], regs[3], fmt);
    return true;
}

bool Controller::write_int64(std::uint16_t addr,
                             std::int64_t value,
                             const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    auto r = encode_int64(value, fmt);
    return transport_->write_registers(addr, { r[0], r[1], r[2], r[3] });
}

bool Controller::read_uint64(std::uint16_t addr,
                             std::uint64_t& value,
                             const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    std::vector<std::uint16_t> regs;
    if (!transport_->read_registers(addr, 4, regs) || regs.size() != 4)
        return false;

    value = decode_uint64(regs[0], regs[1], regs[2], regs[3], fmt);
    return true;
}

bool Controller::write_uint64(std::uint16_t addr,
                              std::uint64_t value,
                              const DataFormat& fmt)
{
    if (!transport_ || !transport_->can_io())
        return false;

    auto r = encode_uint64(value, fmt);
    return transport_->write_registers(addr, { r[0], r[1], r[2], r[3] });
}
