#include "core/tcm_device.hpp"

#include <algorithm>

namespace mag160c::core {
namespace {

constexpr uint8_t MAIN_TCM = 0x02;
constexpr uint8_t SUB_ROTATE = 0x77;
constexpr uint8_t SUB_LIGHT_STEADY = 0x31;
constexpr uint8_t SUB_LIGHT_BLINK = 0x32;
constexpr uint8_t SUB_LIGHT_BREATH = 0x33;

uint8_t light_subcommand(mag160c_tcm_light_mode_t mode) {
    switch (mode) {
    case MAG160C_TCM_LIGHT_STEADY:
        return SUB_LIGHT_STEADY;
    case MAG160C_TCM_LIGHT_BLINK:
        return SUB_LIGHT_BLINK;
    case MAG160C_TCM_LIGHT_BREATH:
        return SUB_LIGHT_BREATH;
    }
    return SUB_LIGHT_STEADY;
}

std::vector<uint8_t> light_payload(mag160c_tcm_light_color_t color) {
    switch (color) {
    case MAG160C_TCM_LIGHT_OFF:
        return {0x00, 0x00, 0x00, 0x00};
    case MAG160C_TCM_LIGHT_RED:
        return {0x01, 0xff, 0x00, 0x00};
    case MAG160C_TCM_LIGHT_GREEN:
        return {0x01, 0x00, 0xff, 0x00};
    case MAG160C_TCM_LIGHT_BLUE:
        return {0x01, 0x00, 0x00, 0xff};
    case MAG160C_TCM_LIGHT_YELLOW:
        return {0x01, 0xff, 0xff, 0x00};
    }
    return {0x00, 0x00, 0x00, 0x00};
}

} // namespace

TcmCommandBuilder::TcmCommandBuilder() : frame_id_(1) {}

uint16_t TcmCommandBuilder::next_frame_id() {
    const uint16_t current = frame_id_;
    ++frame_id_;
    if (frame_id_ >= 0x8000) {
        frame_id_ = 1;
    }
    return current;
}

std::vector<uint8_t> TcmCommandBuilder::build_rotate_frame(int angle) {
    angle = std::clamp(angle, -128, 128);

    const uint8_t direction = angle < 0 ? 0x01 : 0x00;
    const uint8_t magnitude = static_cast<uint8_t>(angle < 0 ? -angle : angle);
    return encode_tcm_frame(MAIN_TCM, SUB_ROTATE, next_frame_id(), {direction, magnitude});
}

std::vector<uint8_t> TcmCommandBuilder::build_light_frame(
    mag160c_tcm_light_color_t color,
    mag160c_tcm_light_mode_t mode
) {
    return encode_tcm_frame(MAIN_TCM, light_subcommand(mode), next_frame_id(), light_payload(color));
}

} // namespace mag160c::core