#include "servo.h"

#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/zbus/zbus.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/pwm.h>
#include <errno.h>
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include <string.h>

LOG_MODULE_REGISTER(servo);

static const struct pwm_dt_spec servo_pwm_specs[NUM_SERVO_CHANNELS] = {
	PWM_DT_SPEC_GET(DT_ALIAS(servo1)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo2)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo3)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo4)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo5)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo6)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo7)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo8)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo9)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo10)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo11)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo12)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo13)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo14)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo15)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo16)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo17)),
	PWM_DT_SPEC_GET(DT_ALIAS(servo18)),
};

int servo_init(void) {
	int ret;

	for (uint8_t ch = 0; ch < NUM_SERVO_CHANNELS; ch++) {
		if (!pwm_is_ready_dt(&servo_pwm_specs[ch])) {
			LOG_ERR("PWM device for servo %d is not ready", ch + 1);
			return -ENODEV;
		}
	}

	for (uint8_t ch = 0; ch < NUM_SERVO_CHANNELS; ch++) {
		ret = pwm_set_pulse_dt(&servo_pwm_specs[ch], PWM_USEC(0));
		if (ret != 0) {
			LOG_ERR("Failed to initialize PWM for servo %d: %d", ch + 1, ret);
			return ret;
		}
	}

	return 0;
}

uint32_t servo_angle_to_pulse_us(int angle_deg) {
	if (angle_deg > SERVO_MAX_ANGLE) {
		angle_deg = SERVO_MAX_ANGLE;
	} else if (angle_deg < SERVO_MIN_ANGLE) {
		angle_deg = SERVO_MIN_ANGLE;
	}

	/* Convert: 0 deg = center, ±max_angle = ±half_pulse_range from center */
	int16_t half_pulse_range = ((int16_t)(SERVO_MAX_PULSE - SERVO_MIN_PULSE)) / 2;
	int16_t pulse_offset_us = (angle_deg * half_pulse_range) / SERVO_MAX_ANGLE;
	uint32_t pulse_us = (uint32_t)(SERVO_CENTER_PULSE + pulse_offset_us);

	if (pulse_us < SERVO_MIN_PULSE) {
		pulse_us = SERVO_MIN_PULSE;
	} else if (pulse_us > SERVO_MAX_PULSE) {
		pulse_us = SERVO_MAX_PULSE;
	}

	return pulse_us;
}

int servo_set_angle(uint8_t channel, int angle_deg) {
	if (channel >= NUM_SERVO_CHANNELS) {
		return -EINVAL;
	}

	int offset_deg = 0;
	int effective_angle_deg = angle_deg + offset_deg;

	uint32_t pulse_us = servo_angle_to_pulse_us(effective_angle_deg);
	int ret = pwm_set_pulse_dt(&servo_pwm_specs[channel], PWM_USEC(pulse_us));
	if (ret != 0) {
		LOG_ERR("Failed to set PWM pulse for servo %d: %d", channel + 1, ret);
		return ret;
	}

	return 0;
}

