/* GPIO Example

   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "motors.h"
#include "encoders.h"
#include "hwTimer.h"

#define OMNI_CTRL_TIMER 0.05 // s
#define MAX_OUTPUT 100.0
#define NMOTORS 3
#define OMNI_NRO_TIMER 0

static void timer_func() {
    printf("ECHO TIMER \n");
}

void app_main(void)
{

       //1. mcpwm gpio initialization
    motor_h_t *mot;
    motor_install(27,26, MCPWM_UNIT_0, MCPWM0A, MCPWM0B, MCPWM_TIMER_0, &mot);

    //2. initial mcpwm configuration
    printf("Configuring Initial Parameters of mcpwm...\n");
    motor_start(mot);    //Configure PWM0A & PWM0B with above settings


    int inc = 25;
    while (1) {
        if (inc == 20){
            motor_start(mot);
        }
        printf("Vel %i\n", inc);
        motor_set_speed(mot, inc);
        vTaskDelay(2000 / portTICK_RATE_MS);
        motor_set_speed(mot, -inc);
        vTaskDelay(2000 / portTICK_RATE_MS);


        inc++;
        if (inc == 30){
            motor_stop(mot);
            inc = 20;
            printf("Reseting vel ...\n");
        }
    }
}



/*
static void callback_enc_func(int i_callback, int8_t dir, uint32_t counter) {
    printf("motor %i, dir %i, counter %i\n", i_callback, dir, counter);    
}

void app_main(void)
{
    encoder_h_t *encoder;
    encoder_setup(37,39,&encoder);
    encoder_register_callback(encoder, callback_enc_func, 0, 1);

    int cnt = 0;
    while(1) {
        printf("cnt: %d\n", cnt++);
        vTaskDelay(10000 / portTICK_RATE_MS);
        if (!(cnt % 10)){
            printf("Minimum free heap size: %d bytes\n", esp_get_minimum_free_heap_size());
        }
    }
}

static void mcpwm_example_brushed_motor_control(void *arg)
{
    //1. mcpwm gpio initialization
    motor_h_t *mot;
    motor_install(27,26, MCPWM_UNIT_0, MCPWM0A, MCPWM0B, MCPWM_TIMER_0, &mot);

    //2. initial mcpwm configuration
    printf("Configuring Initial Parameters of mcpwm...\n");
    motor_start(mot);    //Configure PWM0A & PWM0B with above settings


    int inc = 25;
    while (1) {
        printf("Vel %i\n", inc);
        motor_set_speed(mot, inc);
        vTaskDelay(2000 / portTICK_RATE_MS);
        motor_set_speed(mot, -inc);
        vTaskDelay(2000 / portTICK_RATE_MS);


        inc++;
        if (inc == 10){
            inc = 0;
            printf("Reseting vel ...\n");

        }
    }
}
*/