#include "car0107_idle.h"
#include "car0107_control.h"
#include "devcontrol_common.h"
#include "modules_common.h"
#include "gpio_common.h"

#define CAR0107_IDLE_CHECK_MS       10000
#define CAR0107_IDLE_TIMEOUT_MS     (5 * 60 * 1000)
#define CAR0107_IDLE_STEER_HOLD_MS  450
#define CAR0107_IDLE_STEER_LEFT     115
#define CAR0107_IDLE_STEER_RIGHT    65

typedef struct {
    pthread_mutex_t mutex;
    ThreadPool_t *pool;
    bool shutdown;
    bool pool_ready;
    int next_left;
} car0107_idle_ctx_t;

static car0107_idle_ctx_t s_idle_ctx = {
    .mutex = PTHREAD_MUTEX_INITIALIZER,
    .pool = NULL,
    .shutdown = false,
    .pool_ready = false,
    .next_left = 1,
};

static void car0107_idle_lock(void)
{
    pthread_mutex_lock(&s_idle_ctx.mutex);
}

static void car0107_idle_unlock(void)
{
    pthread_mutex_unlock(&s_idle_ctx.mutex);
}

static bool car0107_idle_shutdown_get(void)
{
    bool shutdown;

    car0107_idle_lock();
    shutdown = s_idle_ctx.shutdown;
    car0107_idle_unlock();
    return shutdown;
}

static int car0107_idle_pool_init(void)
{
    car0107_idle_lock();
    if (s_idle_ctx.pool_ready) {
        car0107_idle_unlock();
        return 0;
    }

    s_idle_ctx.pool = thread_pool_init(1, 1);
    if (!s_idle_ctx.pool) {
        car0107_idle_unlock();
        my_zlog_error("car0107 idle thread pool init failed");
        return -1;
    }

    s_idle_ctx.pool_ready = true;
    car0107_idle_unlock();
    my_zlog_info("car0107 idle monitor thread pool opened");
    return 0;
}

static void car0107_idle_nudge_steering(int turn_left)
{
    int angle = turn_left ? CAR0107_IDLE_STEER_LEFT : CAR0107_IDLE_STEER_RIGHT;

    /* 速度 PWM 只由持有0107硬件锁的控制路径写，idle线程不直接碰速度。 */
    car0107_steering_set_angle_sync(angle);
    delay_ms(CAR0107_IDLE_STEER_HOLD_MS);
    car0107_steering_set_angle_sync(90);
}

static void car0107_idle_monitor_task(void *arg)
{
    (void)arg;

    my_zlog_info("car0107 idle monitor task start");
    while (!car0107_idle_shutdown_get()) {
        long long now;
        long long last;
        int turn_left;

        delay_ms(CAR0107_IDLE_CHECK_MS);
        if (car0107_idle_shutdown_get() || g_device_type != DEVICE_CAR0107) {
            break;
        }

        now = get_current_time_millis();
        last = car0107_last_control_ms();
        if (last <= 0 || (now - last) < CAR0107_IDLE_TIMEOUT_MS) {
            continue;
        }
        if (car0107_esc_is_active()) {
            continue;
        }

        car0107_idle_lock();
        turn_left = s_idle_ctx.next_left;
        s_idle_ctx.next_left = !s_idle_ctx.next_left;
        car0107_idle_unlock();

        my_zlog_info("car0107 idle %dmin: nudge steer %s",
                     CAR0107_IDLE_TIMEOUT_MS / 60000,
                     turn_left ? "left" : "right");
        car0107_idle_nudge_steering(turn_left);
        car0107_notify_control_activity();
    }

    my_zlog_info("car0107 idle monitor task exit");
}

void car0107_idle_startup(int device_id)
{
    if (device_id != DEVICE_CAR0107) {
        return;
    }
    if (car0107_idle_pool_init() != 0) {
        return;
    }

    car0107_idle_lock();
    s_idle_ctx.shutdown = false;
    if (thread_pool_add_task(s_idle_ctx.pool, car0107_idle_monitor_task, NULL) != 0) {
        my_zlog_error("car0107 idle monitor task submit failed");
    }
    car0107_idle_unlock();

    car0107_notify_control_activity();
    my_zlog_info("car0107 idle monitor enabled: steer nudge every %dmin when idle",
                 CAR0107_IDLE_TIMEOUT_MS / 60000);
}

void car0107_idle_thread_close(void)
{
    car0107_idle_lock();
    s_idle_ctx.shutdown = true;
    car0107_idle_unlock();

    if (s_idle_ctx.pool) {
        thread_pool_destroy(s_idle_ctx.pool);
        s_idle_ctx.pool = NULL;
        s_idle_ctx.pool_ready = false;
    }

    car0107_esc_thread_close();

    my_zlog_info("car0107 idle monitor and ESC threads closed");
}
