//
// Created by Administrator on 2026/4/22.
//

#include "servo_manager/include/servo_manager.h"

ServoManager::ServoManager(const std::vector<ServoConfig>& cfg)
    : controls_(),
      mutex_(RT_NULL),
      wake_sem_(RT_NULL),
      thread_(RT_NULL),
      running_(false)
{
    controls_.reserve(cfg.size());
    for (const auto& c : cfg) {
        controls_.emplace_back(c);
    }

    mutex_ = rt_mutex_create("sv_mgr", RT_IPC_FLAG_PRIO);
    wake_sem_ = rt_sem_create("sv_wk", 0, RT_IPC_FLAG_PRIO);
}


ServoManager::ServoManager(const std::string& json_file_path)
    : ServoManager(loadServoConfigsFromJsonFile(json_file_path))
{
}

ServoManager::~ServoManager()
{
    stop();

    if (mutex_) {
        rt_mutex_delete(mutex_);
        mutex_ = RT_NULL;
    }

    if (wake_sem_) {
        rt_sem_delete(wake_sem_);
        wake_sem_ = RT_NULL;
    }
}

rt_err_t ServoManager::init(bool enable_after_init, bool go_home)
{
    if (!mutex_) {
        return -RT_ENOMEM;
    }

    if (controls_.empty()) {
        rt_kprintf("[ServoManager][E] init: no servo config\n");
        return -RT_EINVAL;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);

    for (size_t i = 0; i < controls_.size(); ++i) {
        const rt_err_t err = controls_[i].init(enable_after_init, go_home);
        if (err != RT_EOK) {
            rt_mutex_release(mutex_);
            return err;
        }
    }

    rt_mutex_release(mutex_);
    return RT_EOK;
}

ServoControl* ServoManager::get(size_t index)
{
    if (!mutex_) {
        return RT_NULL;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
    ServoControl* control = (index < controls_.size()) ? &controls_[index] : RT_NULL;
    rt_mutex_release(mutex_);
    return control;
}

const ServoControl* ServoManager::get(size_t index) const
{
    if (!mutex_) {
        return RT_NULL;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
    const ServoControl* control = (index < controls_.size()) ? &controls_[index] : RT_NULL;
    rt_mutex_release(mutex_);
    return control;
}

ServoControl* ServoManager::get(const std::string& servo_id)
{
    if (!mutex_) {
        return RT_NULL;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
    const int index = findIndexByIdNoLock(servo_id);
    ServoControl* control = (index >= 0) ? &controls_[(size_t)index] : RT_NULL;
    rt_mutex_release(mutex_);
    return control;
}

const ServoControl* ServoManager::get(const std::string& servo_id) const
{
    if (!mutex_) {
        return RT_NULL;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
    const int index = findIndexByIdNoLock(servo_id);
    const ServoControl* control = (index >= 0) ? &controls_[(size_t)index] : RT_NULL;
    rt_mutex_release(mutex_);
    return control;
}

size_t ServoManager::count() const
{
    return controls_.size();
}

rt_err_t ServoManager::start()
{
    if (!mutex_ || !wake_sem_) {
        return -RT_ENOMEM;
    }

    if (running_) {
        return RT_EOK;
    }

    running_ = true;

    thread_ = rt_thread_create("sv_mgr",
                               ServoManager::threadEntry,
                               this,
                               4096,
                               20,
                               1);
    if (!thread_) {
        running_ = false;
        return -RT_ENOMEM;
    }

    const rt_err_t err = rt_thread_startup(thread_);
    if (err != RT_EOK) {
        running_ = false;
        rt_thread_delete(thread_);
        thread_ = RT_NULL;
        return err;
    }

    return RT_EOK;
}

void ServoManager::stop()
{
    if (!running_) {
        return;
    }

    running_ = false;
    wake();

    if (thread_) {
        rt_thread_mdelay((rt_int32_t)(update_period_ms_ + 1));
        rt_thread_delete(thread_);
        thread_ = RT_NULL;
    }
}

void ServoManager::setUpdatePeriodMs(rt_uint32_t update_period_ms)
{
    const rt_uint32_t clamped = (update_period_ms == 0) ? 1 : update_period_ms;

    if (!mutex_) {
        update_period_ms_ = clamped;
        return;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
    update_period_ms_ = clamped;
    rt_mutex_release(mutex_);
}

void ServoManager::wake()
{
    if (thread_ && wake_sem_) {
        rt_sem_release(wake_sem_);
    }
}

void ServoManager::threadEntry(void* parameter)
{
    ServoManager* self = static_cast<ServoManager*>(parameter);
    if (!self) {
        return;
    }

    self->run();
}

void ServoManager::run()
{
    while (running_) {
        rt_tick_t wait_ticks = rt_tick_from_millisecond(update_period_ms_);
        if (wait_ticks == 0) {
            wait_ticks = 1;
        }

        if (wake_sem_) {
            rt_sem_take(wake_sem_, wait_ticks);
        } else {
            rt_thread_mdelay((rt_int32_t)update_period_ms_);
        }

        if (!running_) {
            break;
        }

        const rt_tick_t now_tick = rt_tick_get();

        rt_mutex_take(mutex_, RT_WAITING_FOREVER);
        for (size_t i = 0; i < controls_.size(); ++i) {
            controls_[i].update(now_tick);
        }
        rt_mutex_release(mutex_);
    }

    thread_ = RT_NULL;
}

int ServoManager::findIndexByIdNoLock(const std::string& servo_id) const
{
    if (servo_id.empty()) {
        return -1;
    }

    for (size_t i = 0; i < controls_.size(); ++i) {
        if (controls_[i].id() == servo_id) {
            return (int)i;
        }
    }

    return -1;
}
