#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "esp_system.h"
#include "esp_rom_sys.h" // For esp_rom_delay_us

// Define pin connections
#define STEP_PIN GPIO_NUM_9
#define DIR_PIN GPIO_NUM_10
#define ENABLE_PIN GPIO_NUM_17
#define HOMING_SWITCH_PIN GPIO_NUM_8
#define END_STOP_PIN GPIO_NUM_7

void setup();
void loop();
void stepMotor(int steps, bool direction);
void homeMotor();

void app_main() {
    setup();
    homeMotor(); // Homing the motor
    while (1) {
        loop();
        vTaskDelay(10 / portTICK_PERIOD_MS); // Small delay to avoid watchdog timer reset
    }
}

void setup() {
    // Initialize pins
    gpio_reset_pin(STEP_PIN);
    gpio_set_direction(STEP_PIN, GPIO_MODE_OUTPUT);

    gpio_reset_pin(DIR_PIN);
    gpio_set_direction(DIR_PIN, GPIO_MODE_OUTPUT);

    gpio_reset_pin(ENABLE_PIN);
    gpio_set_direction(ENABLE_PIN, GPIO_MODE_OUTPUT);

    gpio_reset_pin(HOMING_SWITCH_PIN);
    gpio_set_direction(HOMING_SWITCH_PIN, GPIO_MODE_INPUT);
    gpio_set_pull_mode(HOMING_SWITCH_PIN, GPIO_PULLUP_ONLY);

    gpio_reset_pin(END_STOP_PIN);
    gpio_set_direction(END_STOP_PIN, GPIO_MODE_INPUT);
    gpio_set_pull_mode(END_STOP_PIN, GPIO_PULLUP_ONLY);

    // Enable motor driver
    gpio_set_level(ENABLE_PIN, 0); // LOW to enable
    printf("Motor driver enabled\n");

    // Homing routine
    homeMotor();
}

void loop() {
    // Normal operation
    if (gpio_get_level(END_STOP_PIN) == 0) {
        // End-stop triggered, stop motor
        gpio_set_level(ENABLE_PIN, 1); // HIGH to disable
        printf("End-stop triggered, motor stopped\n");
    } else {
        // Move motor
        printf("Moving motor forward\n");
        stepMotor(200, true); // Example: 200 steps forward
    }
}

void stepMotor(int steps, bool direction) {
    gpio_set_level(DIR_PIN, direction);
    for (int i = 0; i < steps; i++) {
        gpio_set_level(STEP_PIN, 1);
        esp_rom_delay_us(1000); // Replace ets_delay_us with esp_rom_delay_us
        gpio_set_level(STEP_PIN, 0);
        esp_rom_delay_us(1000); // Replace ets_delay_us with esp_rom_delay_us
    }
    printf("Stepped motor %d steps %s\n", steps, direction ? "forward" : "backward");
}

void homeMotor() {
    gpio_set_level(DIR_PIN, 0); // LOW for homing direction
    printf("Starting homing routine\n");
    while (gpio_get_level(HOMING_SWITCH_PIN) == 1) {
        gpio_set_level(STEP_PIN, 1);
        esp_rom_delay_us(1000); // Replace ets_delay_us with esp_rom_delay_us
        gpio_set_level(STEP_PIN, 0);
        esp_rom_delay_us(1000); // Replace ets_delay_us with esp_rom_delay_us
    }
    printf("Homing complete\n");
    // Reset position counter (assume zero position)
}