#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/pwm.h>
#include <zephyr/drivers/gpio.h>

#include <zephyr/logging/log.h>

#ifdef CONFIG_RX_SERVO
#include "rx_servo.h"
#endif

LOG_MODULE_REGISTER(RX_CTRL_FW, LOG_LEVEL_INF);
/*10 ms sleep time*/

#define MOTOR_0_DIRECTION_NODE DT_ALIAS(m0d)
static const struct gpio_dt_spec dose_motor_dir = GPIO_DT_SPEC_GET(MOTOR_0_DIRECTION_NODE, gpios);

#define MOTOR_0_SPEED DT_ALIAS(m0s)
static const struct pwm_dt_spec dose_motor_spd = PWM_DT_SPEC_GET(MOTOR_0_SPEED);

#define MOTOR_1_DIRECTION_NODE DT_ALIAS(m1d)
static const struct gpio_dt_spec valve_motor_dir = GPIO_DT_SPEC_GET(MOTOR_1_DIRECTION_NODE, gpios);

#define MOTOR_1_SPEED DT_ALIAS(m1s)
static const struct pwm_dt_spec valve_motor_spd = PWM_DT_SPEC_GET(MOTOR_1_SPEED);

/* SW0_NODE is the devicetree node identifier for the "sw0" alias */
#define SW0_NODE DT_ALIAS(sw0)
static const struct gpio_dt_spec button = GPIO_DT_SPEC_GET(SW0_NODE, gpios);

/* SW0_NODE is the devicetree node identifier for the "sw1" alias */
#define SW1_NODE DT_ALIAS(sw1)
static const struct gpio_dt_spec clr_button = GPIO_DT_SPEC_GET(SW1_NODE, gpios);

static const struct gpio_dt_spec dose_enc_a =
    GPIO_DT_SPEC_GET(DT_ALIAS(m0halla), gpios);
static const struct gpio_dt_spec dose_enc_b =
    GPIO_DT_SPEC_GET(DT_ALIAS(m0hallb), gpios);

static const struct gpio_dt_spec valve_enc_a =
    GPIO_DT_SPEC_GET(DT_ALIAS(m1halla), gpios);
static const struct gpio_dt_spec valve_enc_b =
    GPIO_DT_SPEC_GET(DT_ALIAS(m1hallb), gpios);

rx_servo_t dose_servo = {
        .name = "Dose Servo",
        .motor.name = "Dose Motor",
        .encoder.name = "Dose Encoder",
};
rx_servo_t valve_servo = {
        .name = "Valve Servo",
        .motor.name = "Valve Motor",
        .encoder.name = "Valve Encoder",
};

void welcome_message(void)
{
        LOG_INF("-----------Welcome to the RX Control Firmware-------------");
        LOG_INF("Starting RX_Control_SW application...");
        LOG_DBG("A log message in debug level");
        LOG_WRN("A log message in warning level!");
        LOG_ERR("A log message in Error level!");
}

void button_pressed(const struct device *dev, struct gpio_callback *cb, uint32_t pins)
{
        dose_servo.move(100);
        //        valve_servo.move(100);
}

void clr_button_pressed(const struct device *dev, struct gpio_callback *cb, uint32_t pins)
{

        dose_servo.home();
        // valve_servo.home();
}

static struct gpio_callback button_cb_data;
static struct gpio_callback clr_button_cb_data;

int main(void)
{
        welcome_message();
        int ret;
        ret = gpio_pin_configure_dt(&button, GPIO_INPUT);
        if (ret < 0)
        {
                return -1;
        }
        ret = gpio_pin_configure_dt(&clr_button, GPIO_INPUT);
        if (ret < 0)
        {
                return -1;
        }

        if (!device_is_ready(button.port))
        {
                return -1;
        }

        rx_servo_init(&dose_servo, &dose_enc_a, &dose_enc_b, &dose_motor_dir, &dose_motor_spd);
        k_sleep(K_MSEC(5000));
        rx_servo_init(&valve_servo, &valve_enc_a, &valve_enc_b, &valve_motor_dir, &valve_motor_spd);


        ret = gpio_pin_interrupt_configure_dt(&button, GPIO_INT_EDGE_TO_ACTIVE);

        gpio_init_callback(&button_cb_data, button_pressed, BIT(button.pin));

        gpio_add_callback(button.port, &button_cb_data);

        ret = gpio_pin_interrupt_configure_dt(&clr_button, GPIO_INT_EDGE_TO_ACTIVE);

        gpio_init_callback(&clr_button_cb_data, clr_button_pressed, BIT(clr_button.pin));

        gpio_add_callback(clr_button.port, &clr_button_cb_data);

        while (1)
        {
                // LOG_INF("RX Control Firmware running...");
                // LOG_INF("%s servo count: %d", dose_servo.name, dose_servo.encoder.count);
                // LOG_INF("%s servo count: %d", valve_servo.name, valve_servo.encoder.count);         
                k_sleep(K_MSEC(1000));
        }

        return 0;
}
