#include "controller.hpp"
#include "codec.hpp"
#include "modbus_tcp.hpp"
#include <iostream>

Controller::Controller()
    : connected_(false)
{
}

// ---- connection management ------------------------------------
bool Controller::connect(const std::string& address)
{
    transport_ = std::make_unique<ModbusTCP>();

    if (!transport_->connect(address.c_str(), 502)) {
        return false;
    }

    connected_ = true;
    address_ = address;
    return true;
}

void Controller::disconnect()
{
    if (connected_) {
        std::cout << "Disconnected from controller at " << address_ << std::endl;
    }

    connected_ = false;
    address_.clear();
}

bool Controller::is_connected() const
{
    return connected_;
}


// ---- PLC-style raw register API ----------------------------------

bool Controller::read_register(std::uint16_t address,
                               std::uint16_t& value)
{
    std::vector<std::uint16_t> regs;
    if (!transport_->read_registers(address, 1, regs)) {
        return false;
    }

    value = regs[0];
    return true;
}


bool Controller::write_register(std::uint16_t address,
                                std::uint16_t value)
{
    return transport_->write_registers(address, { value });
}

// ---- PLC-style float API ------------------------⭐

bool Controller::read_float(std::uint16_t address,
                            float& value,
                            const DataFormat& fmt)
{
    if (!connected_) {
        return false;
    }

    std::vector<std::uint16_t> regs;
    if (!transport_->read_registers(address, 2, regs)) {
        return false;
    }

    if (regs.size() != 2) {
        return false;
    }

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

bool Controller::write_float(std::uint16_t address,
                             float value,
                             const DataFormat& fmt)
{
    if (!connected_) {
        return false;
    }

    // float → 2 registers
    auto regs = encode_float(value, fmt);

    std::vector<std::uint16_t> values = {
        regs[0], regs[1]
    };

    return transport_->write_registers(address, values);
}

// ---- PLC-style double API ----------------------------------

bool Controller::read_double(std::uint16_t address,
                             double& value,
                             const DataFormat& fmt)
{
    if (!connected_) {
        return false;
    }

    // read 4 consecutive registers atomically
    std::vector<std::uint16_t> regs;
    if (!transport_->read_registers(address, 4, regs)) {
        return false;
    }

    if (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 address,
                              double value,
                              const DataFormat& fmt)
{
    if (!connected_) {
        return false;
    }

    // double → 4 registers
    auto regs = encode_double(value, fmt);

    std::vector<std::uint16_t> values = {
        regs[0], regs[1], regs[2], regs[3]
    };

    return transport_->write_registers(address, values);
}