zephyr/rims_app/src/operation.cpp

#include "operation.hpp"
#include "common.hpp"
#include "error.hpp"
#include "inplace_vector.hpp"
#include "log.hpp"
#include "messenger.hpp"
#include "pid.hpp"
#include "zephyr.hpp"
#include "ctrl.hpp"

#include <algorithm>
#include <chrono>
#include <cstdint>
#include <optional>
#include <span>
#include <thread>
#include <variant>

namespace rims {

template <typename T>
concept HasSetTargetTemperature = requires(T t, const op_TargetTemperature &temp) {
    { t.setTargetTemperature(temp) } -> std::same_as<void>;
};
static_assert(HasSetTargetTemperature<ConstantTemperatureMode>);
static_assert(HasSetTargetTemperature<TemperatureSlopeMode>);
static_assert(not HasSetTargetTemperature<DisabledMode>);

template <typename T>
concept HasTargetTemperature = requires(T t) {
    { t.targetTemperature() } -> std::convertible_to<op_TargetTemperature>;
};
static_assert(HasTargetTemperature<ConstantTemperatureMode>);
static_assert(HasTargetTemperature<TemperatureSlopeMode>);
static_assert(not HasTargetTemperature<DisabledMode>);

K_FIFO_DEFINE(operationIngress);
K_FIFO_DEFINE(operationEggress);

fifo_queue<op_IngressMessage, 2> operationIngressQueue{operationIngress};
fifo_queue<op_EgressMessage, 2>  operationEgressQueue{operationEggress};

constexpr auto op_factory = handler_factory<op_IngressMessage, op_EgressMessage, op::error>{};

constexpr std::array<handler, 5> op_handlers = {
    op_factory.make_handler_expected<op_ActiveChannelsRequest, &OperationOrchestrator::handle_activeChannelsRequest>(),
    op_factory.make_handler_expected<op_InitializeChannelRequest, &OperationOrchestrator::handle_initializeChannelRequest>(),
    op_factory.make_handler_expected<op_DeinitializeChannelRequest, &OperationOrchestrator::handle_deinitializeChannelRequest>(),
    op_factory.make_handler_expected<op_ModeRequest, &OperationOrchestrator::handle_modeRequest>(),
    op_factory.make_handler_expected<op_TargetTemperatureRequest, &OperationOrchestrator::handle_targetTemperatureRequest>(),
};

OperationOrchestrator::OperationOrchestrator() {
    ULOG_DEBUG("OperationOrchestrator start");

    zephyr::event_pool::k_init(_events[0], _tickSem);
    operationIngressQueue.k_event_init(_events[1]);

    _tickTimer.start();
}

void OperationOrchestrator::loop() {
    while (1) {
        auto ret = zephyr::event_pool::k_poll_forever(_events);
        if (ret == 0) {
            zephyr::event_pool::k_poll_handle(_events[0], [&]() { event_tick(); });
            zephyr::event_pool::k_poll_handle(_events[1], [&]() { event_operationMessageArrived(); });
        }
    }
}

void OperationOrchestrator::event_tick() {
    zephyr::semaphore::k_sem_take_now(_tickSem);
    ULOG_INFO("PID update thread");
    for (auto &channel : _channels) {
        channel.update();
    }
}

void OperationOrchestrator::event_operationMessageArrived() {
    operationIngressQueue.try_consume([&](const op_IngressMessage &req) {
        ULOG_INFO("operation request message handler");
        operationEgressQueue.try_produce([&](op_EgressMessage &resp) {
            ULOG_INFO("operation response message handler");
            for (const auto &handler : op_handlers) {
                if (handler.execute(this, req, resp)) break;
            }
            return true;
        });
    });
}

std::expected<void, op::error> OperationOrchestrator::handle_activeChannelsRequest(const ActiveChannelsRequest &req, ActiveChannelsResponse &resp) const {
    ULOG_INFO("active channels request handler");
    resp.data_count = _channels.size();
    for (auto i = 0; i < resp.data_count; i++) {
        resp.data[i].channel                = _channels.at(i).id();
        resp.data[i].ctrl_channel_ids_count = _channels.at(i).copyControlChannels(resp.data[i].ctrl_channel_ids);
    }
    return {};
}

std::expected<void, op::error> OperationOrchestrator::handle_initializeChannelRequest(const InitializeChannelRequest &req, InitializeChannelResponse &resp) {
    ULOG_INFO("channel init request handler");

    CHECK(freeSlot());

    // check if any of needed channels are already controlld by somebody else
    for (auto i = 0; i < req.ctrl_channels_count; i++) {
        const auto ch = req.ctrl_channels[i];
        for (const auto &opchannel : _channels) {
            if (opchannel.usesControlChannel(ch)) {
                return std::unexpected{op::channel_used_multiple_times{}};
            }
        }
    }

    beman::inplace_vector<uint8_t, sizeof(InitializeChannelRequest::ctrl_channels)> channels;
    for (auto i = 0; i < req.ctrl_channels_count; i++) {
        channels.push_back(req.ctrl_channels[i]);
    }

    auto find_id = [&]() -> std::expected<uint8_t, op::error> {
        for (uint8_t id = 0; id < 255; ++id) {
            bool used = std::any_of(_channels.begin(), _channels.end(), [id](const OperationChannel &ch) { return ch.id() == id; });
            if (!used) return id;
        }
        return std::unexpected{op::max_number_of_channels{}};
    };

    // create OperationChannel for channels and id
    auto        activatedChannels = TRY(OperationChannel::check_channels(std::span{channels}));
    auto        id                = TRY(find_id());
    const auto &newChannel        = _channels.emplace_back(activatedChannels, id);

    resp.has_data                    = true;
    resp.data.channel                = newChannel.id();
    resp.data.ctrl_channel_ids_count = newChannel.copyControlChannels(resp.data.ctrl_channel_ids);
    return {};
}

std::expected<void, op::error> OperationOrchestrator::handle_deinitializeChannelRequest(const DeinitializeChannelRequest &req, DeinitializeChannelResponse &resp) {
    auto found = std::find_if(_channels.begin(), _channels.end(), [&](const OperationChannel &ch) { return ch.id() == req.channel_id; });

    if (found == _channels.end()) {
        ULOG_WARNING("channel %d not found", req.channel_id);
        return std::unexpected{op::channel_not_found{}};
    }

    _channels.erase(found);
    return {};
}

std::expected<void, op::error> OperationOrchestrator::handle_modeRequest(const ModeRequest &request, ModeResponse &resp) {
    ULOG_INFO("mode request handler");
    CHECK(channelValid(request.channel_id));

    if (request.has_data) {
        setMode(request.channel_id, request.data);
    }

    resp.data.mode = mode(request.channel_id);
    return {};
}

TemperatureSlopeMode::TemperatureSlopeMode(PowerControl &pc, PIDController &pid, const op_TemperatureSlopeConfig &config) : _powerControl{pc}, _pid{pid}, _tempChannel{static_cast<uint8_t>(config.temperature_channel_id)}, _slope{config.slope} {
}

void TemperatureSlopeMode::update() {
    float currentTemperature = temp(_tempChannel).temperature_c;

    // Use simple hysteresis to reduce mode-switch jitter
    constexpr float hysteresis      = 0.25f;
    const bool      closeToSetpoint = std::abs(targetTemperature().temperature_c - currentTemperature) <= hysteresis;

    auto useConstantTemperatureAlg = [&]() {
        if (_heating) {
            _pid.get().reset();
            _heating = false;
        }
        const float outputPower = _pid.get().update(targetTemperature().temperature_c, currentTemperature);
        _powerControl.get().setPower(outputPower);
    };

    auto useConstantSlope = [&]() {
        if (!_heating) {
            _pid.get().reset();
            _heating = true;
        }

        const float dt            = 1.0f; // 1s guaranteed by RTOS
        const float dT            = currentTemperature - _lastTemperature;
        const float measuredSlope = (dT / dt) * 60.0f; // °C/min

        const float outputPower = _pid.get().update(_slope, measuredSlope);
        _powerControl.get().setPower(outputPower);
    };

    if (closeToSetpoint) {
        useConstantTemperatureAlg();
    } else {
        useConstantSlope();
    }

    _lastTemperature = currentTemperature;
}

ConstantTemperatureMode::ConstantTemperatureMode(PowerControl &pc, PIDController &pid, const op_ConstantTemperatureConfig &config) : _powerControl{pc}, _pid{pid}, _tempChannel{static_cast<uint8_t>(config.temperature_channel_id)} {

    gpio_GpioRequest gpio = gpio_GpioRequest_init_zero;
    gpio.gpio             = gpio_GPIO_GPIO_pin_2;
    gpio.has_state        = false;
    auto resp             = MessengerThread::ipc_request(gpio);

    if (resp->state != true) {
        gpio.has_state = true;
        gpio.state     = true;
        MessengerThread::ipc_request(gpio);
        ULOG_INFO("Pump turned on");

        std::this_thread::sleep_for(std::chrono::seconds{1});
    }
}

void ConstantTemperatureMode::update() {
    auto  currentTemperature = temp(_tempChannel).temperature_c;
    float outputPower        = _pid.get().update(targetTemperature().temperature_c, currentTemperature);
    _powerControl.get().setPower(outputPower);
}

std::expected<void, op::error> OperationOrchestrator::handle_targetTemperatureRequest(const TargetTemperatureRequest &req, TargetTemperatureResponse &resp) {
    ULOG_INFO("target temperature request handler");
    CHECK(channelValid(req.channel_id));
    CHECK(TRY(find(req.channel_id)).get().handle_targetTemperatureRequest(req, resp));
    return {};
}

std::expected<void, op::error> OperationChannel::handle_targetTemperatureRequest(const TargetTemperatureRequest &req, TargetTemperatureResponse &resp) {
    ULOG_INFO("target temperature request handler");
    if (req.has_target_temperature) {
        CHECK(std::visit(
            [&](auto &obj) -> std::expected<void, op::error> {
                if constexpr (HasSetTargetTemperature<decltype(obj)>) {
                    obj.setTargetTemperature(req.target_temperature);
                    return {};
                } else {
                    return std::unexpected{op::mode_does_not_accept_target_temperature{}};
                }
            },
            _mode
        ));
    }

    std::visit(
        [&](auto &obj) {
            if constexpr (HasTargetTemperature<decltype(obj)>) {
                resp.has_data = true;
                resp.data     = obj.targetTemperature();
            } else {
                resp.has_data = false;
            }
        },
        _mode
    );
    return {};
}

void OperationOrchestrator::setMode(uint8_t ch, const op_ModeData &data) {
    CHECK_ASSERT(channelValid(ch));

    ULOG_INFO("channel %d mode change to %d", ch, data.mode);
    _channels.at(ch).setMode(data.mode, optcref(data.has_mode_config, data.mode_config));
}

void OperationChannel::setMode(Mode mode, std::optional<cref<op_ModeConfig>> config) {
    switch (mode) {
    case op_Mode_Disabled: {
        _mode.emplace<DisabledMode>();
        break;
    }
    case op_Mode_ConstantTemperature: {
        constexpr auto defaultConfig = op_ModeConfig{
            .which_config = op_ModeConfig_constant_temperature_tag, //
            .config       = {.constant_temperature = {op_ConstantTemperatureConfig{.temperature_channel_id = 0}}}
        };

        _mode.emplace<ConstantTemperatureMode>(_pc, _pid, config.value_or(defaultConfig).get().config.constant_temperature);
        break;
    }
    case op_Mode_TemperatureSlope: {
        constexpr auto defaultConfig = op_ModeConfig{
            .which_config = op_ModeConfig_temperature_slope_tag, //
            .config       = {.temperature_slope = {op_TemperatureSlopeConfig{.temperature_channel_id = 0, .slope = 1.0f}}}
        };

        _mode.emplace<TemperatureSlopeMode>(_pc, _pid, config.value_or(defaultConfig).get().config.temperature_slope);
        break;
    }
    default:
        break;
    }
}

op_Mode OperationOrchestrator::mode(uint8_t ch) const {
    /// TODO should return some other status on disabled channel
    if (not channelValid(ch)) return op_Mode_Disabled;
    else return find(ch).value().get().mode();
}

Mode OperationChannel::mode() const {
    if (std::holds_alternative<ConstantTemperatureMode>(_mode)) {
        return op_Mode_ConstantTemperature;
    }
    if (std::holds_alternative<TemperatureSlopeMode>(_mode)) {
        return op_Mode_TemperatureSlope;
    }

    return op_Mode_Disabled;
}

void OperationThread::threadMain() {
    ULOG_DEBUG("OperationThread start");
    OperationOrchestrator thread{};
    thread.loop();
}

std::expected<void, op::error> PowerControl::check_channels(std::span<uint8_t> _control_channels) {
    for (const auto ch : _control_channels) {
        ctrl_PowerControlAlgorithmRequest plar = ctrl_PowerControlAlgorithmRequest_init_zero;
        plar.channel_id                        = ch;
        auto resp                              = MessengerThread::ipc_request(plar);

        assert(resp.has_value()); // internal error, should not occour ever

        if (resp->data.alg == ctrl_PowerControlAlgorithm_NoModulation) {
            ULOG_WARNING("ctrl channel %d disabled", ch);
            return std::unexpected{op::control_channel_disabled{}};
        }
    }
    return {};
}

PowerControl::PowerControl(std::span<uint8_t> channels) : _control_channels{channels.begin(), channels.end()} {
    ULOG_DEBUG("PowerControl start");
}

void PowerControl::setPower(float percent) {
    ULOG_INFO("Set power %f", (double)percent);

    for (const auto ch : _control_channels) {
        ctrl_PowerLevelRequest plr;
        plr.has_level  = true;
        plr.level      = percent;
        plr.channel_id = ch;

        MessengerThread::ipc_request(plr);
    }
}

} // namespace rims