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

#include "rx_servo.h"
#include <stdlib.h>

LOG_MODULE_REGISTER(RX_SERVO, LOG_LEVEL_INF);

#define RX_SERVO_PERIOD 1000

#define RX_SERVO_STALL_THRESHOLD 10
#define RX_SERVO_MOTOR_PERIOD_USEC 500
#define RX_SERVO_MOTOR_MIN_DUTYCYCLE RX_SERVO_MOTOR_PERIOD_USEC / 4
#define RX_SERVO_MOTOR_MAX_DUTYCYCLE RX_SERVO_MOTOR_PERIOD_USEC * 3 / 5

#include <stdio.h>

// Function declarations
// TODO: Add your function declarations here
rx_servo_t *servo;
// Function definitions
// TODO: Add your function definitions here

/**
 * @brief Homes the RX servo.
 *
 * This function is responsible for homing the RX servo.
 * Homing refers to the process of moving the servo to a known reference position.
 *
 * @note This function does not return any value.
 */
void rx_servo_home();

/**
 * Moves the RX servo to the specified position.
 *
 * @param position The desired position of the RX servo.
 */
void rx_servo_move(int32_t position);

/**
 * @brief Monitors the stall condition of the RX servo.
 *
 * This function is responsible for monitoring the stall condition of the RX servo.
 * It checks if the servo is in a stall condition and takes appropriate action.
 *
 * @note This function does not return any value.
 */
void rx_servo_stall_monitor();

/**
 * @brief Updates the parameters of the RX servo.
 *
 * This function is responsible for updating the parameters of the RX servo.
 * It performs the necessary calculations and adjustments to ensure the servo operates correctly.
 *
 * @note This function does not return a value.
 */
void rx_servo_update_params();

void rx_servo_work_handler(struct k_work *work)
{
    rx_servo_t *svo = CONTAINER_OF(work, rx_servo_t, work);
    LOG_INF("%s Servo work handler running", servo->name);
    rx_servo_stall_monitor();
    int state = svo->state;
    rx_servo_update_params();
    switch (state)
    {
    case SERVO_LOST:

        break;
    case SERVO_IDLE:

        break;
    case SERVO_STOP:
        LOG_INF("%s is stopped", svo->name);
        break;
    case SERVO_JOG:
        LOG_INF("%s is jogging", svo->name);
        break;
    case SERVO_MOVE:
        if (svo->motor_state == SERVO_MOTOR_CW)
        {
            if (svo->position >= svo->target_position)
            {
                LOG_INF("%s has reached the target position %d", svo->name, svo->position);
                svo->motor.stop();
                svo->state = SERVO_IDLE;
            }
        }
        else if (svo->motor_state == SERVO_MOTOR_CCW)
        {
            if (svo->position <= svo->target_position)
            {
                LOG_INF("%s has reached the target position %d", svo->name, svo->position);
                svo->motor.stop();
                svo->state = SERVO_IDLE;
            }
        }

        break;
    case SERVO_HOME:

        break;
    default:
        LOG_INF("Servo is in an unknown state");
        break;
    }
}

// static K_WORK_DEFINE(rx_servo_work, rx_servo_work_handler);

static void rx_servo_timer_handler(struct k_timer *dummy)
{
    k_work_submit(&servo->work);
}

// static K_TIMER_DEFINE(rx_servo_timer, rx_servo_timer_handler, NULL);

void rx_servo_init(rx_servo_t *svo, const struct gpio_dt_spec *enc_a, const struct gpio_dt_spec *enc_b,
                   const struct gpio_dt_spec *mtr_ph, const struct pwm_dt_spec *mtr_en)
{
    LOG_INF("Initializing %s servo", svo->name);
    servo = svo;
    // strncpy(servo->name, "RX Servo", 32);
    rx_encoder_init(&servo->encoder, enc_a, enc_b);
    rx_motor_init(&servo->motor, mtr_ph, mtr_en);

    servo->home = rx_servo_home;                              // Assign the home function to the servo object
    servo->move = rx_servo_move;                              // Assign the move function to the servo object
    servo->state = SERVO_LOST;                                // Set the initial state of the servo
    servo->motor.stop();                                      // Stop the motor
    servo->motor.set_period(RX_SERVO_MOTOR_PERIOD_USEC);      // Set the period of the motor
    servo->motor.set_dutycycle(RX_SERVO_MOTOR_MIN_DUTYCYCLE); // Set the initial duty cycle
    k_work_init(&svo->work, rx_servo_work_handler);           // Initialize the work item
    k_timer_init(&svo->timer, rx_servo_timer_handler, NULL);  // Initialize the timer
    k_timer_start(&svo->timer, K_MSEC(RX_SERVO_PERIOD),
                  K_MSEC(RX_SERVO_PERIOD));
    if(servo->initialized == 1)
    {
        LOG_ERR("Servo %s already initialized", svo->name);
        return;
    }
    servo->initialized = 1; // Set the initialized flag
    LOG_INF("Initializing servo %s complete", svo->name);
}

void rx_servo_home()
{
    LOG_INF("Homing %s", servo->name);
    servo->state = SERVO_HOME;
    servo->motor.stop();
    servo->encoder.count = 0;
    servo->motor.set_dutycycle(RX_SERVO_MOTOR_MIN_DUTYCYCLE);
    servo->motor.rotate_ccw();
}

void rx_servo_stall_monitor()
{
    static int zero_speed_count = 0; // Counter for consecutive zero speed calls

    if (servo->state == SERVO_HOME)
    {
        if (0 >= abs(servo->speed_raw))
        {
            zero_speed_count++; // Increment the zero speed counter

            if (zero_speed_count >= RX_SERVO_STALL_THRESHOLD) // Check if the counter reaches 4
            {
                LOG_INF("%s Homing complete", servo->name);
                servo->motor.stop();
                servo->encoder.count = 0;
                zero_speed_count = 0;
                servo->state = SERVO_IDLE;
            }
        }
        else
        {
            zero_speed_count = 0; // Reset the zero speed counter
        }
    }
}

void rx_servo_update_params()
{
    /* update speed parameter*/
    static int count = 0;
    servo->speed_raw = servo->encoder.count - count;
    count = servo->encoder.count;
    /* update position parameter*/
    servo->position = servo->encoder.count;
}

void rx_servo_move(int32_t position)
{
    LOG_INF("Moving %s to position %d", servo->name, position);
    servo->state = SERVO_MOVE;
    servo->target_position = position;
    servo->motor.stop();
    servo->motor.set_dutycycle(RX_SERVO_MOTOR_MAX_DUTYCYCLE);
    if (servo->position < position)
    {
        servo->motor.rotate_cw();
        servo->motor_state = SERVO_MOTOR_CW;
    }
    else
    {
        servo->motor.rotate_ccw();
        servo->motor_state = SERVO_MOTOR_CCW;
    }
}
