#include <stdio.h>
#include "display_lcd.h"
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/adc.h"

#define OUT 1
#define IN 0
#define OUT_H 1
#define OUT_L 0

#define LIGA_MOTOR 7
#define DIR_MOTOR 8
#define GPIO_POT_PEDAL 27
#define ADC_PEDAL_CHANNEL 1
#define GPIO_POT_TPS 28
#define ADC_TPS_CHANNEL 2
/*
uint16_t valor_pedal=0;
uint16_t pos_pedal=0;
uint16_t valor_tps=0;
uint16_t pos_tps=0;
*/

void system_init(void);

void main(void){
  stdio_init_all();
  unsigned char posicao_tps_frase[17]="Angulo TPS:";
  unsigned char posicao_pedal_frase[17]="Angulo Pedal:";
  unsigned char inicial_frase[17]="Iniciando...";

  uint16_t valor_pedal=0;
  uint16_t pos_pedal=0;
  uint16_t valor_tps=0;
  uint16_t pos_tps=0;
  uint16_t angulo_tps_at=0;
  uint16_t angulo_tps_ant=100;
  uint16_t angulo_pedal_at=0;
  uint16_t angulo_pedal_ant=100;

  system_init();
  
  posicao_cursor_lcd(1,0);
  escreve_frase_ram_lcd(inicial_frase);
  DesligaCursor();
  
  sleep_ms(3000);

  LIMPA_DISPLAY();
  printf("\n Sistema Inicializado");

  while(true)
  {
  adc_select_input(ADC_PEDAL_CHANNEL);
  adc_read();
  valor_pedal=adc_read();

  if(valor_pedal>3724)
  {
    valor_pedal=3724;
  }
  else if(valor_pedal<372)
  {
    valor_pedal=372;
  }
  
  pos_pedal= (((100*valor_pedal)-37200)/3352);
  angulo_pedal_at = ((90*pos_pedal)/100);

  adc_select_input(ADC_TPS_CHANNEL);
  adc_read();
  valor_tps=adc_read();

  if(valor_tps>3724)
  {
    valor_tps=3724;
  }
  else if(valor_tps<372)
  {
    valor_tps=372;
  }

  pos_tps=(((100*valor_tps)-37200)/3352);
  angulo_tps_at = ((90*pos_tps)/100);

  if((pos_tps)!=(pos_pedal))
  {
      if((pos_tps)>(pos_pedal))
      {
        gpio_put(DIR_MOTOR,OUT_H);
      }
      else
      {
        gpio_put(DIR_MOTOR,OUT_L);
      }
      gpio_put(LIGA_MOTOR,OUT_H);
      sleep_ms(10);
      gpio_put(LIGA_MOTOR,OUT_L);
  }
  else
  {
  gpio_put(LIGA_MOTOR,OUT_L);
  }
  
  if(((angulo_pedal_at)!=(angulo_pedal_ant))||((angulo_tps_at)!=(angulo_tps_ant)))
  {
  LIMPA_DISPLAY();
  posicao_cursor_lcd(1,0);
  escreve_frase_ram_lcd(posicao_pedal_frase);
  posicao_cursor_lcd(1,14);
  escreve_inteiro_lcd(angulo_pedal_at);
  posicao_cursor_lcd(2,0);
  escreve_frase_ram_lcd(posicao_tps_frase);
  posicao_cursor_lcd(2,14);
  escreve_inteiro_lcd(angulo_tps_at);
  }
  angulo_pedal_ant=angulo_pedal_at;
  angulo_tps_ant=angulo_tps_at;
  sleep_ms(10);

  //DesligaCursor();
  }//while
}


void system_init(void)
{
  printf("\n Sistema Inicializado");
  gpio_init(LIGA_MOTOR);
  gpio_set_dir(LIGA_MOTOR,OUT);
  gpio_put(LIGA_MOTOR,OUT_L);

  gpio_init(DIR_MOTOR);
  gpio_set_dir(DIR_MOTOR,OUT);
  gpio_put(DIR_MOTOR,OUT_L);

  adc_init();
  adc_gpio_init(GPIO_POT_PEDAL);
  adc_init();
  adc_gpio_init(GPIO_POT_TPS);
  
  init_lcd();
  sleep_ms(100);
}