#include "actuator.h"
#include <zephyr/kernel.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/drivers/counter.h>
#include <zephyr/logging/log.h>
#include <zephyr/device.h>
#include <zephyr/drivers/counter.h>

LOG_MODULE_REGISTER(actuator, LOG_LEVEL_INF);

/* === GPIOs from devicetree === */
#define USER_NODE DT_PATH(zephyr_user)
static const struct gpio_dt_spec do1   = GPIO_DT_SPEC_GET(USER_NODE, do1_gpios);
static const struct gpio_dt_spec do2   = GPIO_DT_SPEC_GET(USER_NODE, do2_gpios);
static const struct gpio_dt_spec do_en = GPIO_DT_SPEC_GET(USER_NODE, do_en_gpios);

/* === Counter device (Timer3) === */
#define COUNTER_NODE DT_NODELABEL(timer3)
static const struct device *counter_dev = DEVICE_DT_GET(COUNTER_NODE);

/* === Constants === */
#define MOTOR_FREQ_HZ   200
#define MOTOR_PERIOD_US (USEC_PER_SEC / MOTOR_FREQ_HZ)

#define SERVO_FREQ_HZ   50
#define SERVO_PERIOD_US (USEC_PER_SEC / SERVO_FREQ_HZ)

/* Servo duty cycle limits */
#define SERVO_MIN_PULSE_US (SERVO_PERIOD_US * 5 / 100)   /* 5% → 1 ms */
#define SERVO_MAX_PULSE_US (SERVO_PERIOD_US * 10 / 100)  /* 10% → 2 ms */

/* === State === */
static enum actuator_mode current_mode;
static bool initialized = false;
static int motor_speed = 0;   /* -100..100 */
static int servo_angle = 0;   /* -90..90 */

static struct counter_alarm_cfg alarm_cfg;
static bool active_phase = false;

/* === Forward declarations === */
static void pwm_isr(const struct device *dev,
                    uint8_t chan_id,
                    uint32_t ticks,
                    void *user_data);

/* === Helpers === */
static int configure_gpio(void)
{
    if (!device_is_ready(do1.port) || !device_is_ready(do2.port) || !device_is_ready(do_en.port)) {
        LOG_ERR("GPIO device not ready");
        return -ENODEV;
    }

    gpio_pin_configure_dt(&do1, GPIO_OUTPUT_INACTIVE);
    gpio_pin_configure_dt(&do2, GPIO_OUTPUT_INACTIVE);
    gpio_pin_configure_dt(&do_en, GPIO_OUTPUT_INACTIVE);
    return 0;
}

static void start_counter(uint32_t us)
{
    uint32_t now_ticks;
    int err = counter_get_value(counter_dev, &now_ticks);
    if (err)
    {
        LOG_ERR("Failed to get counter value (%d)", err);
        return;
    }

    uint32_t delay_ticks = counter_us_to_ticks(counter_dev, us);
    uint32_t next_ticks = now_ticks + delay_ticks;

    alarm_cfg.flags = COUNTER_ALARM_CFG_ABSOLUTE;
    alarm_cfg.ticks = next_ticks;
    alarm_cfg.callback = pwm_isr;
    alarm_cfg.user_data = NULL;

    counter_set_channel_alarm(counter_dev, 0, &alarm_cfg);
}

/* === ISR callback === */
static void pwm_isr(const struct device *dev,
                    uint8_t chan_id,
                    uint32_t ticks,
                    void *user_data)
{
    ARG_UNUSED(dev);
    ARG_UNUSED(chan_id);
    ARG_UNUSED(ticks);
    ARG_UNUSED(user_data);

    if (current_mode == ACTUATOR_MODE_MOTOR) {
        if (motor_speed == 0) {
            /* Brake mode */
            gpio_pin_set_dt(&do1, 1);
            gpio_pin_set_dt(&do2, 1);
            return;
        }

        if (!active_phase) {
            /* Start ON phase */
            if (motor_speed > 0) {
                gpio_pin_set_dt(&do1, 1);
                gpio_pin_set_dt(&do2, 0);
            } else {
                gpio_pin_set_dt(&do1, 0);
                gpio_pin_set_dt(&do2, 1);
            }
            uint32_t duty_us = (MOTOR_PERIOD_US * abs(motor_speed)) / 100;
            start_counter(duty_us);
            active_phase = true;
        } else {
            /* Start OFF (coast) phase */
            gpio_pin_set_dt(&do1, 0);
            gpio_pin_set_dt(&do2, 0);
            uint32_t off_time = MOTOR_PERIOD_US - (MOTOR_PERIOD_US * abs(motor_speed)) / 100;
            start_counter(off_time);
            active_phase = false;
        }
    }
    else if (current_mode == ACTUATOR_MODE_SERVO) {
        if (!active_phase) {
            /* Start ON phase */
            gpio_pin_set_dt(&do1, 1);
            gpio_pin_set_dt(&do2, 1);
            uint32_t duty_us = SERVO_MIN_PULSE_US +
                (servo_angle + 90) * (SERVO_MAX_PULSE_US - SERVO_MIN_PULSE_US) / 180;
            start_counter(duty_us);
            active_phase = true;
        } else {
            /* Start OFF phase */
            gpio_pin_set_dt(&do2, 0);
            uint32_t off_time = SERVO_PERIOD_US -
                (SERVO_MIN_PULSE_US + (servo_angle + 90) * (SERVO_MAX_PULSE_US - SERVO_MIN_PULSE_US) / 180);
            start_counter(off_time);
            active_phase = false;
        }
    }
}

/* === API implementation === */
int actuator_init(enum actuator_mode mode)
{
    int ret = configure_gpio();
    if (ret) return ret;

    if (!device_is_ready(counter_dev)) {
        LOG_ERR("Counter device not ready");
        return -ENODEV;
    }

    counter_start(counter_dev);

    current_mode = mode;
    initialized = true;
    active_phase = false;
    LOG_INF("Actuator initialized in mode %d", mode);

    return 0;
}

int actuator_enable(bool enable)
{
    if (!initialized) return -EACCES;
    return gpio_pin_set_dt(&do_en, enable ? 1 : 0);
}

int actuator_motor_set_speed(int speed_percent)
{
    if (!initialized || current_mode != ACTUATOR_MODE_MOTOR) return -EACCES;
    if (speed_percent < -100) speed_percent = -100;
    if (speed_percent > 100)  speed_percent = 100;

    motor_speed = speed_percent;
    active_phase = false;
    start_counter(1); /* trigger ISR soon */
    return 0;
}

int actuator_servo_set_angle(int angle_deg)
{
    if (!initialized || current_mode != ACTUATOR_MODE_SERVO) return -EACCES;
    if (angle_deg < -90) angle_deg = -90;
    if (angle_deg > 90)  angle_deg = 90;

    servo_angle = angle_deg;
    active_phase = false;
    start_counter(1); /* trigger ISR soon */
    return 0;
}