#include "car0102_pin27_common.h"
#include "devcontrol_common.h"
#include "modules_common.h"
#include "gpio_common.h"

#define CAR0102_PIN27_PWM_DUTY        80
#define CAR0102_PIN27_STEP_MS         500
#define CAR0102_PIN27_BOOTSTRAP_MS    1000
#define CAR0102_PIN27_DEBOUNCE_MS     1000
#define CAR0102_PIN27_PIN_FWD         5
#define CAR0102_PIN27_PIN_REV         7
#define CAR0102_PIN27_IDLE_POLL_MS    10
#define CAR0102_PIN27_WAIT_SLICE_MS   10

typedef struct {
	pthread_mutex_t mutex;
	ThreadPool_t *pool;
	car0102_pin27_runtime_t runtime;
} car0102_pin27_ctx_t;

static car0102_pin27_ctx_t s_pin27_ctx = {
	.mutex = PTHREAD_MUTEX_INITIALIZER,
	.pool = NULL,
	.runtime.v = {
		.fsm = CAR0102_PIN27_FSM_NONE,
		.bootstrap_done = false,
		.shutdown = false,
		.last_trigger_ms = 0,
	},
};

static void car0102_pin27_lock(void)
{
	pthread_mutex_lock(&s_pin27_ctx.mutex);
}

static void car0102_pin27_unlock(void)
{
	pthread_mutex_unlock(&s_pin27_ctx.mutex);
}

static bool car0102_pin27_shutdown_get(void)
{
	bool shutdown;

	car0102_pin27_lock();
	shutdown = s_pin27_ctx.runtime.v.shutdown;
	car0102_pin27_unlock();
	return shutdown;
}

static void car0102_pin27_pwm_stop_locked(void)
{
	softPwmWrite(CAR0102_PIN27_PIN_FWD, 0);
	softPwmWrite(CAR0102_PIN27_PIN_REV, 0);
}

static void car0102_pin27_pwm_stop(void)
{
	car0102_pin27_lock();
	car0102_pin27_pwm_stop_locked();
	car0102_pin27_unlock();
}

static void car0102_pin27_pwm_apply(int fwd_duty, int rev_duty)
{
	softPwmWrite(CAR0102_PIN27_PIN_FWD, fwd_duty);
	softPwmWrite(CAR0102_PIN27_PIN_REV, rev_duty);
}

static void car0102_pin27_delay_ms_interruptible(int ms)
{
	int elapsed = 0;

	while (elapsed < ms) {
		if (car0102_pin27_shutdown_get()) {
			return;
		}
		delay_ms(CAR0102_PIN27_WAIT_SLICE_MS);
		elapsed += CAR0102_PIN27_WAIT_SLICE_MS;
	}
}

static void car0102_pin27_bootstrap_run(void)
{
	my_zlog_debug("car0102 pin27 bootstrap: pin%d %d%% %dms",
	              CAR0102_PIN27_PIN_FWD, CAR0102_PIN27_PWM_DUTY, CAR0102_PIN27_BOOTSTRAP_MS);

	car0102_pin27_pwm_apply(CAR0102_PIN27_PWM_DUTY, 0);
	car0102_pin27_delay_ms_interruptible(CAR0102_PIN27_BOOTSTRAP_MS);
	car0102_pin27_pwm_stop();
}

static void car0102_pin27_sequence_run(void)
{
	my_zlog_debug("car0102 pin27 cycle: pin%d->pin%d->brake %d%% %dms",
	              CAR0102_PIN27_PIN_REV, CAR0102_PIN27_PIN_FWD,
	              CAR0102_PIN27_PWM_DUTY, CAR0102_PIN27_STEP_MS);

	car0102_pin27_pwm_apply(0, CAR0102_PIN27_PWM_DUTY);
	car0102_pin27_delay_ms_interruptible(CAR0102_PIN27_STEP_MS);
	if (car0102_pin27_shutdown_get()) {
		car0102_pin27_pwm_stop();
		return;
	}

	car0102_pin27_pwm_apply(CAR0102_PIN27_PWM_DUTY, 0);
	car0102_pin27_delay_ms_interruptible(CAR0102_PIN27_STEP_MS);
	if (car0102_pin27_shutdown_get()) {
		car0102_pin27_pwm_stop();
		return;
	}

	car0102_pin27_pwm_apply(CAR0102_PIN27_PWM_DUTY, CAR0102_PIN27_PWM_DUTY);
	car0102_pin27_delay_ms_interruptible(CAR0102_PIN27_STEP_MS);

	car0102_pin27_pwm_stop();
}

static void car0102_pin27_task_function(void *arg)
{
	if (arg != NULL) {
		free(arg);
	}

	my_zlog_debug("car0102 pin27 thread task start");
	car0102_pin27_bootstrap_run();

	car0102_pin27_lock();
	s_pin27_ctx.runtime.v.bootstrap_done = true;
	if (s_pin27_ctx.runtime.v.fsm == CAR0102_PIN27_FSM_NONE) {
		s_pin27_ctx.runtime.v.fsm = CAR0102_PIN27_FSM_IDLE;
	}
	car0102_pin27_unlock();

	while (!car0102_pin27_shutdown_get()) {
		bool run_sequence = false;

		car0102_pin27_lock();
		if (s_pin27_ctx.runtime.v.fsm == CAR0102_PIN27_FSM_PENDING) {
			s_pin27_ctx.runtime.v.fsm = CAR0102_PIN27_FSM_RUNNING;
			run_sequence = true;
		}
		car0102_pin27_unlock();

		if (run_sequence) {
			car0102_pin27_sequence_run();

			car0102_pin27_lock();
			if (!s_pin27_ctx.runtime.v.shutdown &&
			    s_pin27_ctx.runtime.v.fsm == CAR0102_PIN27_FSM_RUNNING) {
				s_pin27_ctx.runtime.v.fsm = CAR0102_PIN27_FSM_IDLE;
			}
			car0102_pin27_unlock();
		}

		delay_ms(CAR0102_PIN27_IDLE_POLL_MS);
	}

	my_zlog_debug("car0102 pin27 thread task exit");
}

static int car0102_pin27_pthrpoll_task_init(void)
{
	int *arg;
	ThreadPool_t *pool;

	car0102_pin27_lock();
	if (s_pin27_ctx.pool != NULL) {
		car0102_pin27_unlock();
		return 0;
	}
	car0102_pin27_unlock();

	arg = malloc(sizeof(int));
	if (arg == NULL) {
		my_zlog_error("car0102 pin27 thread arg alloc failed");
		return -1;
	}
	*arg = 1;

	pool = thread_pool_init(1, 1);
	if (pool == NULL) {
		my_zlog_error("car0102 pin27 thread_pool_init failed");
		free(arg);
		return -1;
	}

	if (thread_pool_add_task(pool, car0102_pin27_task_function, arg) != 0) {
		my_zlog_error("car0102 pin27 thread_pool_add_task failed");
		thread_pool_destroy(pool);
		free(arg);
		return -1;
	}

	car0102_pin27_lock();
	s_pin27_ctx.pool = pool;
	car0102_pin27_unlock();

	my_zlog_info("car0102 pin27 thread pool opened");
	return 0;
}

void car0102_pin27_startup(int device_id)
{
	if (device_id != DEVICE_CAR0102) {
		return;
	}

	if (car0102_pin27_pthrpoll_task_init() != 0) {
		my_zlog_error("car0102 pin27 startup failed");
	}
}

static int car0102_pin27_handle(int pin, int val)
{
	long long now;
	int rc = 0;

	if (pin != 27) {
		return 1;
	}

	car0102_pin27_lock();

	if (s_pin27_ctx.runtime.v.shutdown) {
		car0102_pin27_unlock();
		return 0;
	}

	/* 松手 val=0 不参与电机驱动控制，长按/点按均只由 val=1 触发 */
	if (val == 0) {
		car0102_pin27_unlock();
		return 0;
	}

	if (!s_pin27_ctx.runtime.v.bootstrap_done) {
		car0102_pin27_unlock();
		return 0;
	}

	if (s_pin27_ctx.runtime.v.fsm != CAR0102_PIN27_FSM_IDLE) {
		car0102_pin27_unlock();
		return 0;
	}

	now = get_current_time_millis();
	if (s_pin27_ctx.runtime.v.last_trigger_ms > 0 &&
	    (now - s_pin27_ctx.runtime.v.last_trigger_ms) < CAR0102_PIN27_DEBOUNCE_MS) {
		car0102_pin27_unlock();
		return 0;
	}

	s_pin27_ctx.runtime.v.last_trigger_ms = now;
	s_pin27_ctx.runtime.v.fsm = CAR0102_PIN27_FSM_PENDING;
	car0102_pin27_unlock();

	if (s_pin27_ctx.pool == NULL) {
		rc = car0102_pin27_pthrpoll_task_init();
		if (rc != 0) {
			car0102_pin27_lock();
			s_pin27_ctx.runtime.v.fsm = CAR0102_PIN27_FSM_NONE;
			s_pin27_ctx.runtime.v.bootstrap_done = false;
			s_pin27_ctx.runtime.v.last_trigger_ms = 0;
			car0102_pin27_unlock();
		}
	}

	return rc;
}

void car0102_pin27_stop_control(int device_id, int pin, int val)
{
	if (device_id != DEVICE_CAR0102) {
		return;
	}
	car0102_pin27_handle(pin, val);
}

void car0102_pin27_thread_close(void)
{
	ThreadPool_t *pool = NULL;

	car0102_pin27_lock();
	s_pin27_ctx.runtime.v.shutdown = true;
	car0102_pin27_pwm_stop_locked();
	pool = s_pin27_ctx.pool;
	s_pin27_ctx.pool = NULL;
	s_pin27_ctx.runtime.v.fsm = CAR0102_PIN27_FSM_NONE;
	s_pin27_ctx.runtime.v.bootstrap_done = false;
	s_pin27_ctx.runtime.v.last_trigger_ms = 0;
	car0102_pin27_unlock();

	if (pool != NULL) {
		thread_pool_destroy(pool);
	}

	my_zlog_info("car0102 pin27 thread pool closed");
}
