#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/adc.h"
#include "driver/ledc.h"
#include "esp_err.h"

// Define ADC channels for joystick
#define JOYSTICK_X ADC1_CHANNEL_6 // GPIO34
//#define JOYSTICK_Y ADC1_CHANNEL_7 // GPIO35
#define NUM_SAMPLES 100

// Define PWM channel for servo
#define SERVO_PWM_GPIO GPIO_NUM_17
#define SERVO_MIN_PULSEWIDTH_US 500    // Minimum pulse width in microseconds
#define SERVO_MAX_PULSEWIDTH_US 2500  // Maximum pulse width in microseconds
#define SERVO_MAX_DEGREE 360          // Maximum angle in degrees

// Function to map joystick value to servo angle
float map_value(float input, float input_min, float input_max, float output_min, float output_max) {
    return output_min + ((output_max - output_min) / (input_max - input_min)) * (input - input_min);
}

// Function to set servo angle
void set_servo_angle(uint32_t angle) {
    uint32_t duty = SERVO_MIN_PULSEWIDTH_US +
                    ((SERVO_MAX_PULSEWIDTH_US - SERVO_MIN_PULSEWIDTH_US) * angle / SERVO_MAX_DEGREE);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, duty);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
}

int32_t get_stable_adc_reading() {
    int32_t sum = 0;
    for (int i = 0; i < NUM_SAMPLES; i++) {
        sum += adc1_get_raw(JOYSTICK_X);
    }
    return sum / NUM_SAMPLES;
}

void app_main(void) {
    // Initialize ADC for joystick
    adc1_config_width(ADC_WIDTH_BIT_12);
    adc1_config_channel_atten(JOYSTICK_X, ADC_ATTEN_DB_11);
    // adc1_config_channel_atten(JOYSTICK_Y, ADC_ATTEN_DB_11);

    // Initialize LEDC for servo control
    ledc_timer_config_t ledc_timer = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .timer_num = LEDC_TIMER_0,
        .duty_resolution = LEDC_TIMER_16_BIT,
        .freq_hz = 50, // Servo frequency: 50Hz
        .clk_cfg = LEDC_AUTO_CLK,
    };
    ledc_timer_config(&ledc_timer);

    ledc_channel_config_t ledc_channel = {
        .gpio_num = SERVO_PWM_GPIO,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = LEDC_CHANNEL_0,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,
        .hpoint = 0,
    };
    ledc_channel_config(&ledc_channel);

    while (1) {
        // Read joystick X-axis value
        int x_value = get_stable_adc_reading();

        // Map joystick value to servo angle (0–180 degrees)
        uint32_t angle = map_value(x_value, 0, 3200, 0, SERVO_MAX_DEGREE);

        // Set servo angle based on joystick position
        set_servo_angle(angle);

        vTaskDelay(pdMS_TO_TICKS(50)); // Delay for smooth movement
    }
}