//
// Created by Administrator on 2025/12/24.
//

#include "erpc/service/include/servo_service_impl.h"
#include <string>
#include "common/type/common_type.h"

const std::vector<ServoDriver::Config> ServoServiceImpl::kServoCfg = {
    {"eye_l_up", "pwm1", 1, 20000000, 500000, 2500000, -0.5f * PI, 0.5f * PI, 0.0f},
    // {"eye_r_up", "pwm1", 2, 20000000, 500000, 2500000, 0.0f, kPi, 0.5f * kPi},
    // ...
};

ServoServiceImpl::ServoServiceImpl()
    : manager_(kServoCfg),
      init_err_(RT_EOK),
      mode_(MotionMode::Immediate),
      mutex_(RT_NULL)
{
    mutex_ = rt_mutex_create("sv_svc", RT_IPC_FLAG_PRIO);
    if (!mutex_) {
        init_err_ = -RT_ENOMEM;
        rt_kprintf("[ServoService][E] mutex create FAILED, err=%d\n", (int)init_err_);
        return;
    }

    init_err_ = manager_.init(true, true);
    if (init_err_ != RT_EOK) {
        rt_kprintf("[ServoService][E] init FAILED, err=%d\n", (int)init_err_);
        return;
    }

    manager_.setUpdatePeriodMs(1);

    for (const auto& cfg : kServoCfg) {
        ServoControl* servo = manager_.get(cfg.servo_id);
        if (!servo) {
            init_err_ = -RT_ENOSYS;
            rt_kprintf("[ServoService][E] missing servo control, id=%s\n", cfg.servo_id.c_str());
            return;
        }

        servo->setConstraints(
            3.14,
            31.4,
            31.4);
    }

    init_err_ = manager_.start();
    if (init_err_ != RT_EOK) {
        rt_kprintf("[ServoService][E] manager start FAILED, err=%d\n", (int)init_err_);
    }
}

ServoServiceImpl::~ServoServiceImpl()
{
    if (mutex_) {
        rt_mutex_delete(mutex_);
        mutex_ = RT_NULL;
    }
}

bool ServoServiceImpl::setConstraints(const char* id,
                                      float max_velocity_rad,
                                      float max_acceleration_rad,
                                      float max_jerk_rad)
{
    if (init_err_ != RT_EOK) {
        rt_kprintf("[ServoService][E] setConstraints: service not ready, init_err=%d\n",
                   (int)init_err_);
        return false;
    }

    if (!id) {
        rt_kprintf("[ServoService][E] setConstraints: id is null\n");
        return false;
    }

    if (max_velocity_rad <= 0.0f ||
        max_acceleration_rad <= 0.0f ||
        max_jerk_rad <= 0.0f) {
        rt_kprintf("[ServoService][E] setConstraints: invalid constraints, id=%s v=%.4f a=%.4f j=%.4f\n",
                   id,
                   (double)max_velocity_rad,
                   (double)max_acceleration_rad,
                   (double)max_jerk_rad);
        return false;
    }

    ServoControl* servo = manager_.get(std::string(id));
    if (!servo) {
        rt_kprintf("[ServoService][E] setConstraints: servo not found, id=%s\n", id);
        return false;
    }

    servo->setConstraints(max_velocity_rad, max_acceleration_rad, max_jerk_rad);
    rt_kprintf("[ServoService] setConstraints: id=%s v=%.4f a=%.4f j=%.4f\n",
               id,
               (double)max_velocity_rad,
               (double)max_acceleration_rad,
               (double)max_jerk_rad);
    return true;
}

bool ServoServiceImpl::setMode(RpcMotionMode mode)
{
    if (init_err_ != RT_EOK) {
        rt_kprintf("[ServoService][E] setMode: service not ready, init_err=%d\n",
                   (int)init_err_);
        return false;
    }

    if (!mutex_) {
        rt_kprintf("[ServoService][E] setMode: mutex is null\n");
        return false;
    }

    MotionMode motion_mode;
    switch (mode) {
    case RpcMotionModeImmediate:
        motion_mode = MotionMode::Immediate;
        break;
    case RpcMotionModeSCurve:
        motion_mode = MotionMode::SCurve;
        break;
    default:
        rt_kprintf("[ServoService][E] setMode: invalid mode=%d\n", (int)mode);
        return false;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
    mode_ = motion_mode;
    rt_mutex_release(mutex_);

    rt_kprintf("[ServoService] setMode: mode=%d\n", (int)mode);
    return true;
}

bool ServoServiceImpl::setUpdatePeriodMs(uint32_t ms)
{
    if (init_err_ != RT_EOK) {
        rt_kprintf("[ServoService][E] setUpdatePeriodMs: service not ready, init_err=%d\n",
                   (int)init_err_);
        return false;
    }

    manager_.setUpdatePeriodMs(ms);
    manager_.wake();

    rt_kprintf("[ServoService] setUpdatePeriodMs: ms=%u\n", (unsigned)ms);
    return true;
}

int32_t ServoServiceImpl::move(const list_ServoCmd_1_t* cmds)
{
    if (init_err_ != RT_EOK) {
        rt_kprintf("[ServoService][E] move: service not ready, init_err=%d\n",
                   (int)init_err_);
        return init_err_;
    }

    if (!cmds) {
        rt_kprintf("[ServoService][E] move: cmds is null\n");
        return -RT_EINVAL;
    }

    if (!cmds->elements || cmds->elementsCount == 0) {
        rt_kprintf("[ServoService][E] move: empty list (elements=%p, count=%d)\n",
                   cmds->elements,
                   (int)cmds->elementsCount);
        return -RT_EINVAL;
    }

    if (!mutex_) {
        rt_kprintf("[ServoService][E] move: mutex is null\n");
        return -RT_ENOMEM;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
    const MotionMode motion_mode = mode_;
    rt_mutex_release(mutex_);

    for (uint32_t i = 0; i < cmds->elementsCount; ++i) {
        const char* id = cmds->elements[i].id;
        if (!id) {
            rt_kprintf("[ServoService][E] move: cmds[%d].id is null\n", (int)i);
            return -RT_EINVAL;
        }

        ServoControl* servo = manager_.get(std::string(id));
        if (!servo) {
            rt_kprintf("[ServoService][E] move: servo not found, id=%s\n", id);
            return -RT_ENOSYS;
        }

        const rt_err_t err = servo->move(cmds->elements[i].angle_rad, motion_mode);
        if (err != RT_EOK) {
            rt_kprintf("[ServoService][E] move: move failed, id=%s angle=%.4f mode=%d err=%d\n",
                       id,
                       (double)cmds->elements[i].angle_rad,
                       (int)motion_mode,
                       (int)err);
            return err;
        }
    }

    if (motion_mode == MotionMode::SCurve) {
        manager_.wake();
    }

    return RT_EOK;
}

int32_t ServoServiceImpl::moveJ(const list_float_1_t* angles_rad)
{
    if (init_err_ != RT_EOK) {
        rt_kprintf("[ServoService][E] moveJ: service not ready, init_err=%d\n",
                   (int)init_err_);
        return init_err_;
    }

    if (!angles_rad) {
        rt_kprintf("[ServoService][E] moveJ: angles_rad is null\n");
        return -RT_EINVAL;
    }

    if (!angles_rad->elements || angles_rad->elementsCount == 0) {
        rt_kprintf("[ServoService][E] moveJ: empty list (elements=%p, count=%d)\n",
                   angles_rad->elements,
                   (int)angles_rad->elementsCount);
        return -RT_EINVAL;
    }

    const size_t need = kServoCfg.size();
    const size_t got = (size_t)angles_rad->elementsCount;
    if (got != need) {
        rt_kprintf("[ServoService][E] moveJ: invalid angles count, need=%d got=%d\n",
                   (int)need,
                   (int)got);
        return -RT_EINVAL;
    }

    if (!mutex_) {
        rt_kprintf("[ServoService][E] moveJ: mutex is null\n");
        return -RT_ENOMEM;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
    const MotionMode motion_mode = mode_;
    rt_mutex_release(mutex_);

    for (size_t i = 0; i < need; ++i) {
        ServoControl* servo = manager_.get(kServoCfg[i].servo_id);
        if (!servo) {
            rt_kprintf("[ServoService][E] moveJ: servo not found, id=%s\n",
                       kServoCfg[i].servo_id.c_str());
            return -RT_ENOSYS;
        }

        const rt_err_t err = servo->move(angles_rad->elements[i], motion_mode);
        if (err != RT_EOK) {
            rt_kprintf("[ServoService][E] moveJ: move failed, id=%s angle=%.4f mode=%d err=%d\n",
                       kServoCfg[i].servo_id.c_str(),
                       (double)angles_rad->elements[i],
                       (int)motion_mode,
                       (int)err);
            return err;
        }
    }

    if (motion_mode == MotionMode::SCurve) {
        manager_.wake();
    }

    return RT_EOK;
}