#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "driver/gpio.h"
#include "driver/ledc.h"

#define DELAY_US 1000
#define STEP_COUNT 200
#define STEPPER_A_PIN 19
#define STEPPER_B_PIN 18
#define STEPPER_C_PIN 5
#define STEPPER_D_PIN 17

static const char *TAG = "main";

void rotate_stepper(int pin1, int pin2, int pin3, int pin4, int steps) {
    int i;
    for (i = 0; i < steps; i++) {
        gpio_set_level(pin1, 1);
        gpio_set_level(pin2, 0);
        gpio_set_level(pin3, 1);
        gpio_set_level(pin4, 0);
        esp_rom_delay_us(DELAY_US);

        gpio_set_level(pin1, 0);
        gpio_set_level(pin2, 1);
        gpio_set_level(pin3, 0);
        gpio_set_level(pin4, 1);
        esp_rom_delay_us(DELAY_US);

        gpio_set_level(pin1, 1);
        gpio_set_level(pin2, 1);
        gpio_set_level(pin3, 0);
        gpio_set_level(pin4, 0);
        esp_rom_delay_us(DELAY_US);

        gpio_set_level(pin1, 0);
        gpio_set_level(pin2, 0);
        gpio_set_level(pin3, 1);
        gpio_set_level(pin4, 1);
        esp_rom_delay_us(DELAY_US);
    }
}

void app_main(void) {
    gpio_config_t io_conf;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = (1ULL << STEPPER_A_PIN) | (1ULL << STEPPER_B_PIN) | (1ULL << STEPPER_C_PIN) | (1ULL << STEPPER_D_PIN);
    gpio_config(&io_conf);

    int i;
    for (i = 0; i < 10; i++) {
        ESP_LOGI(TAG, "Rotating stepper A clockwise");
        rotate_stepper(STEPPER_A_PIN, STEPPER_B_PIN, STEPPER_C_PIN, STEPPER_D_PIN, STEP_COUNT);
        vTaskDelay(1000 / portTICK_PERIOD_MS);

        ESP_LOGI(TAG, "Rotating stepper B counterclockwise");
        rotate_stepper(STEPPER_A_PIN, STEPPER_B_PIN, STEPPER_C_PIN, STEPPER_D_PIN, -STEP_COUNT);
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
}