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

#include "servo_manager/include/servo_config.h"

#include <ArduinoJson.h>
#include <dfs.h>
#include <sys/stat.h>

#include "sys/unistd.h"

std::vector<ServoConfig> loadServoConfigsFromJsonFile(const std::string& json_file_path)
{
    std::vector<ServoConfig> configs;

    if (json_file_path.empty()) {
        rt_kprintf("json file path is empty\n");
        return configs;
    }

    struct stat st;
    if (stat(json_file_path.c_str(), &st) != 0) {
        rt_kprintf("stat json file failed: %s\n", json_file_path.c_str());
        return configs;
    }

    if (st.st_size <= 0) {
        rt_kprintf("json file is empty: %s\n", json_file_path.c_str());
        return configs;
    }

    char* json_buf = static_cast<char*>(rt_malloc((rt_size_t)st.st_size + 1));
    if (json_buf == RT_NULL) {
        rt_kprintf("malloc json buffer failed, size=%d\n", (int)st.st_size);
        return configs;
    }

    int fd = open(json_file_path.c_str(), O_RDONLY);
    if (fd < 0) {
        rt_kprintf("open json file failed: %s\n", json_file_path.c_str());
        rt_free(json_buf);
        return configs;
    }

    int len = read(fd, json_buf, (size_t)st.st_size);
    close(fd);

    if (len <= 0) {
        rt_kprintf("read json file failed: %s\n", json_file_path.c_str());
        rt_free(json_buf);
        return configs;
    }

    json_buf[len] = '\0';

    JsonDocument doc;

    DeserializationError error = deserializeJson(doc, json_buf);
    rt_free(json_buf);

    if (error) {
        rt_kprintf("parse json failed: %s\n", error.c_str());
        return configs;
    }

    const int version = doc["version"] | 1;
    if (version != 1) {
        rt_kprintf("unsupported servo config version: %d\n", version);
        return configs;
    }

    JsonArray servos = doc["servos"].as<JsonArray>();
    if (servos.isNull()) {
        rt_kprintf("json field 'servos' missing or invalid\n");
        return configs;
    }

    const int servo_count = doc["servo_count"] | -1;
    if (servo_count >= 0 && servo_count != (int)servos.size()) {
        rt_kprintf("servo_count mismatch, declared=%d actual=%d\n",
                   servo_count,
                   (int)servos.size());
        return configs;
    }

    configs.reserve(servos.size());

    for (JsonObject servo : servos) {
        const char* servo_id = servo["id"] | "";
        JsonObject driver = servo["driver"].as<JsonObject>();
        JsonObject control = servo["control"].as<JsonObject>();

        if (servo_id[0] == '\0' || driver.isNull()) {
            rt_kprintf("invalid servo config: id empty or driver missing\n");
            continue;
        }

        ServoConfig cfg;
        cfg.id = servo_id;

        cfg.driver.pwm_dev_name = driver["pwm_dev_name"] | "";
        cfg.driver.pwm_channel = driver["pwm_channel"] | 0;

        cfg.driver.period_ns = driver["period_ns"] | 20000000;
        cfg.driver.min_pulse_ns = driver["min_pulse_ns"] | 500000;
        cfg.driver.max_pulse_ns = driver["max_pulse_ns"] | 2500000;

        cfg.driver.min_angle_rad = driver["min_angle_rad"] | -1.5707963f;
        cfg.driver.max_angle_rad = driver["max_angle_rad"] | 1.5707963f;

        cfg.limit_min_angle_rad = servo["limit_min_angle_rad"] | cfg.driver.min_angle_rad;
        cfg.limit_max_angle_rad = servo["limit_max_angle_rad"] | cfg.driver.max_angle_rad;
        cfg.home_angle_rad = servo["home_angle_rad"] | 0.0f;

        cfg.control.max_velocity_rad = control["max_velocity_rad"] | 6.28;
        cfg.control.max_acceleration_rad = control["max_acceleration_rad"] | 100.0;
        cfg.control.max_jerk_rad = control["max_jerk_rad"] | 500.0;
        cfg.control.position_gain = control["position_gain"] | 8.0;

        if (cfg.driver.pwm_dev_name.empty() ||
            cfg.driver.pwm_channel <= 0 ||
            cfg.driver.period_ns == 0 ||
            cfg.driver.min_pulse_ns >= cfg.driver.max_pulse_ns ||
            cfg.driver.min_angle_rad >= cfg.driver.max_angle_rad ||
            cfg.limit_min_angle_rad < cfg.driver.min_angle_rad ||
            cfg.limit_max_angle_rad > cfg.driver.max_angle_rad ||
            cfg.limit_min_angle_rad >= cfg.limit_max_angle_rad ||
            cfg.home_angle_rad < cfg.limit_min_angle_rad ||
            cfg.home_angle_rad > cfg.limit_max_angle_rad ||
            cfg.control.max_velocity_rad <= 0.0 ||
            cfg.control.max_acceleration_rad <= 0.0 ||
            cfg.control.max_jerk_rad <= 0.0 ||
            cfg.control.position_gain < 0.0) {
            rt_kprintf("invalid servo config values, id=%s\n", cfg.id.c_str());
            continue;
        }

        bool duplicated = false;
        for (const auto& existing : configs) {
            if (existing.id == cfg.id) {
                duplicated = true;
                break;
            }
        }
        if (duplicated) {
            rt_kprintf("duplicate servo id in config: %s\n", cfg.id.c_str());
            continue;
        }

        configs.push_back(cfg);
    }

    rt_kprintf("load servo config from %s, count=%d\n",
               json_file_path.c_str(),
               (int)configs.size());

    return configs;
}
