#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "sdkconfig.h"

#include "../include/stepper.h"

// FOR L928 DRIVER
// const uint8_t half_steps_phase[8] = {0x09, 0x01, 0x03, 0x02, 0x06, 0x04, 0x0c, 0x08};

// void stepper_init(struct stepper_pins_t *stepper_ptr) {
//     gpio_set_direction(stepper_ptr->pin1, GPIO_MODE_OUTPUT);
//     gpio_set_direction(stepper_ptr->pin2, GPIO_MODE_OUTPUT);
//     gpio_set_direction(stepper_ptr->pin3, GPIO_MODE_OUTPUT);
//     gpio_set_direction(stepper_ptr->pin4, GPIO_MODE_OUTPUT);

//     printf("stepper init success\n");
// }

// void stepper_uninit(struct stepper_pins_t * stepper_ptr) {
//     gpio_set_direction(stepper_ptr->pin1, GPIO_MODE_INPUT);
//     gpio_set_direction(stepper_ptr->pin2, GPIO_MODE_INPUT);
//     gpio_set_direction(stepper_ptr->pin3, GPIO_MODE_INPUT);
//     gpio_set_direction(stepper_ptr->pin4, GPIO_MODE_INPUT);
// }

// void step(struct stepper_pins_t *stepper_ptr, int step) {
//     gpio_set_level(stepper_ptr->pin1, step & 1);
//     gpio_set_level(stepper_ptr->pin3, (step & 0b10) >> 1);
//     gpio_set_level(stepper_ptr->pin2, (step & 0b100) >> 2);
//     gpio_set_level(stepper_ptr->pin4, (step & 0b1000) >> 3);
// }

// void half_steps(struct stepper_pins_t *stepper_ptr, int steps, bool dir) {
//     int step_count;
//     int phase_index;

//     if(dir) {
//         phase_index = 0;
//         for(step_count = 0; step_count < steps; step_count++) {
//             if(phase_index > 7)
//                 phase_index = 0;
//             step(stepper_ptr, half_steps_phase[phase_index]);
//             vTaskDelay(10 / portTICK_PERIOD_MS);
//             phase_index++;
//         }
//     } else {
//         phase_index = 7;
//         for(step_count = 0; step_count < steps; step_count++) {
//             if (phase_index < 0)
//                 phase_index = 7;
//             step(stepper_ptr, half_steps_phase[phase_index]);
//             vTaskDelay(10 / portTICK_PERIOD_MS);
//             phase_index--;
//         }
//     }
// }

// FOR A4988 DRIVER
const uint8_t step_pin = 19;
const uint8_t dir_pin = 18;
const uint8_t en_pin = 5;

void stepper_init(void) {
    gpio_set_direction(en_pin, GPIO_MODE_OUTPUT);
    gpio_set_level(en_pin, HIGH);
    gpio_set_direction(dir_pin, GPIO_MODE_OUTPUT);
    gpio_set_direction(step_pin, GPIO_MODE_OUTPUT);
}

void stepper_uninit(void) {
    gpio_set_level(en_pin, HIGH);
    gpio_set_direction(en_pin, GPIO_MODE_DISABLE);
    gpio_set_direction(dir_pin, GPIO_MODE_DISABLE);
    gpio_set_direction(step_pin, GPIO_MODE_DISABLE);
}

void stepper_open(void) {
    gpio_set_level(en_pin, LOW);
    gpio_set_level(dir_pin, HIGH);
    for(uint16_t i = 0; i < 1600; i++) {
        gpio_set_level(step_pin, HIGH);
        vTaskDelay(10 / portTICK_PERIOD_MS);
        gpio_set_level(step_pin, LOW);
        vTaskDelay(10 / portTICK_PERIOD_MS);
    }
}

void stepper_close(void) {
    gpio_set_level(en_pin, LOW);
    gpio_set_level(dir_pin, LOW);
    for(uint16_t i = 0; i < 1600; i++) {
        gpio_set_level(step_pin, HIGH);
        vTaskDelay(10 / portTICK_PERIOD_MS);
        gpio_set_level(step_pin, LOW);
        vTaskDelay(10 / portTICK_PERIOD_MS);
    }
}