#include <stdint.h>
#include <math.h>
#include "pico/stdlib.h"
#include <cstdio>
#include "hardware/adc.h"
#include "ControleMotorVelocidadeFixa.h"
#include "ControleAnalogico.h"
#include "Oled.h"

typedef ControleMotorVelocidadeFixa Motor;
typedef ControleAnalogico Analogico;

#define PINO_OLED_SDA 14
#define PINO_OLED_SCL 15

#define PINO_JOYSTICK_X 27
#define CANAL_JOYSTICK_X 0 // ADC0
#define PINO_JOYSTICK_Y 26
#define CANAL_JOYSTICK_Y 1 // ADC1
#define VALOR_MIN_SAIDA -1
#define VALOR_MAX_SAIDA 1

#define PINO_STEP_MOTOR_X 2
#define PINO_DIR_MOTOR_X 3
#define VELOCIDADE_MOTOR_X 500.0f
#define POSICAO_MIN_MOTOR_X 0
#define POSICAO_ATUAL_MOTOR_X 200
#define POSICAO_MAX_MOTOR_X 400

#define PINO_STEP_MOTOR_Y 4
#define PINO_DIR_MOTOR_Y 8
#define VELOCIDADE_MOTOR_Y 500.0f
#define POSICAO_MIN_MOTOR_Y 0
#define POSICAO_ATUAL_MOTOR_Y 200
#define POSICAO_MAX_MOTOR_Y 400

#define POSICAO_MAXIMA 400
#define POSICAO_MINIMA 0


void modifica_motor(Motor *motor, float *valor, uint32_t posicao_min, uint32_t posicao_max, const char eixo = ' ') {

  printf("Valor eixo %c: %f\r\n", eixo, *valor);
  if (*valor > 0.8f) {
    motor->configurarDestino(posicao_max);
  }
  if (*valor < -0.8f) {
    motor->configurarDestino(posicao_min);
  }
  if (*valor > -0.5f && *valor < 0.5f) {
    motor->configurarDestino(motor->obterPosicaoAtual());
  }
  return;
}

int main() {
  stdio_init_all();

  Motor motorX(PINO_STEP_MOTOR_X, PINO_DIR_MOTOR_X, VELOCIDADE_MOTOR_X);
  Motor motorY(PINO_STEP_MOTOR_Y, PINO_DIR_MOTOR_Y, VELOCIDADE_MOTOR_Y);

  Analogico joystick_X(PINO_JOYSTICK_X, CANAL_JOYSTICK_X, VALOR_MIN_SAIDA, VALOR_MAX_SAIDA);
  Analogico joystick_Y(PINO_JOYSTICK_Y, CANAL_JOYSTICK_Y, VALOR_MIN_SAIDA, VALOR_MAX_SAIDA);

  motorX.setarPosicaoAtual(POSICAO_ATUAL_MOTOR_X);
  motorY.setarPosicaoAtual(POSICAO_ATUAL_MOTOR_Y);

  while (true) {


    motorX.atualizar();
    motorY.atualizar();

    if (joystick_X.houveAlteracao()) {
      float valor_X = joystick_X.lerAlteracao();
      modifica_motor(&motorX, &valor_X, POSICAO_MIN_MOTOR_X, POSICAO_MAX_MOTOR_X, 'X');
    }

    if (joystick_Y.houveAlteracao()) {
      float valor_y = joystick_Y.lerAlteracao();
      modifica_motor(&motorY, &valor_y, POSICAO_MIN_MOTOR_Y, POSICAO_MAX_MOTOR_Y, 'Y');
    }

    sleep_ms(10);
  }

}