#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <math.h>

// ============================================================
// ROWINGCOACH V9.0
// ============================================================
// 1 clique = 1 remada simulada
//
// BOTÃO:
// GPIO 4 -> botão -> GND
//
// A simulação gera uma curva de movimento equivalente
// a uma remada para testar a lógica do sistema.
//
// Depois podemos substituir a simulação pelo MPU6050 real.
// ============================================================


// ============================================================
// OLED
// ============================================================

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

Adafruit_SSD1306 display(
  SCREEN_WIDTH,
  SCREEN_HEIGHT,
  &Wire,
  OLED_RESET
);


// ============================================================
// MPU6050
// ============================================================

Adafruit_MPU6050 mpu;


// ============================================================
// BOTÃO DE TESTE
// ============================================================

#define BOTAO_TESTE 4


// ============================================================
// CONFIGURAÇÕES DE MOVIMENTO
// ============================================================

const float LIMIAR_MOVIMENTO = 1.20;

const float LIMIAR_FORTE = 2.50;

const float LIMIAR_REPOUSO = 3.00;


// ============================================================
// CONFIGURAÇÕES DE LEITURA
// ============================================================

const unsigned long INTERVALO_LEITURA = 50;


// ============================================================
// CONFIGURAÇÕES DO BOTÃO
// ============================================================

const unsigned long DEBOUNCE_MS = 80;


// ============================================================
// REFERÊNCIA DO MPU6050
// ============================================================

float refX = 0;
float refY = 0;
float refZ = 0;


// ============================================================
// MOVIMENTO
// ============================================================

float movX = 0;
float movY = 0;
float movZ = 0;

float movimento = 0;

float pico = 0;


// ============================================================
// REMADAS
// ============================================================

int remadas = 0;

float voga = 0;


// ============================================================
// TEMPOS
// ============================================================

unsigned long ultimaLeitura = 0;

unsigned long ultimaRemada = 0;


// ============================================================
// BOTÃO
// ============================================================

bool ultimoEstadoBotao = HIGH;

unsigned long ultimoDebounce = 0;


// ============================================================
// SIMULAÇÃO
// ============================================================

bool simulando = false;


// ============================================================
// ESTADOS DA REMADA
// ============================================================

enum Estado {

  ESPERANDO,

  REMADA

};

Estado estado = ESPERANDO;


// ============================================================
// NOME DO ESTADO
// ============================================================

const char* nomeEstado() {

  switch (estado) {

    case ESPERANDO:
      return "ESPERANDO";

    case REMADA:
      return "REMADA";
  }

  return "?";
}


// ============================================================
// CALIBRAÇÃO
// ============================================================

void calibrarReferencia() {

  float somaX = 0;
  float somaY = 0;
  float somaZ = 0;

  const int amostras = 30;


  Serial.println(
    "Calibrando referencia de repouso..."
  );


  for (int i = 0; i < amostras; i++) {

    sensors_event_t a;
    sensors_event_t g;
    sensors_event_t temp;

    mpu.getEvent(
      &a,
      &g,
      &temp
    );


    somaX += a.acceleration.x;

    somaY += a.acceleration.y;

    somaZ += a.acceleration.z;


    delay(20);
  }


  refX = somaX / amostras;

  refY = somaY / amostras;

  refZ = somaZ / amostras;


  Serial.println(
    "Referencia estabelecida."
  );


  Serial.print("REF X: ");
  Serial.println(refX, 2);

  Serial.print("REF Y: ");
  Serial.println(refY, 2);

  Serial.print("REF Z: ");
  Serial.println(refZ, 2);
}


// ============================================================
// LEITURA DO MPU6050
// ============================================================

void lerSensor() {

  sensors_event_t a;
  sensors_event_t g;
  sensors_event_t temp;


  mpu.getEvent(
    &a,
    &g,
    &temp
  );


  // ----------------------------------------------------------
  // DADOS RAW
  // ----------------------------------------------------------

  Serial.print("RAW X: ");
  Serial.print(a.acceleration.x, 2);

  Serial.print(" | RAW Y: ");
  Serial.print(a.acceleration.y, 2);

  Serial.print(" | RAW Z: ");
  Serial.println(a.acceleration.z, 2);


  // ----------------------------------------------------------
  // DIFERENÇA DA REFERÊNCIA
  // ----------------------------------------------------------

  movX = fabs(
    a.acceleration.x - refX
  );

  movY = fabs(
    a.acceleration.y - refY
  );

  movZ = fabs(
    a.acceleration.z - refZ
  );


  // ----------------------------------------------------------
  // MOVIMENTO COMBINADO
  // ----------------------------------------------------------

  movimento = sqrt(

    movX * movX +

    movY * movY +

    movZ * movZ
  );


  // ----------------------------------------------------------
  // PICO
  // ----------------------------------------------------------

  if (
    estado == REMADA &&
    movimento > pico
  ) {

    pico = movimento;
  }
}


// ============================================================
// INICIAR REMADA
// ============================================================

void iniciarRemada() {

  estado = REMADA;

  pico = movimento;


  Serial.println();

  Serial.println(
    "================================"
  );

  Serial.println(
    ">>> INICIO DA REMADA"
  );

  Serial.println(
    "================================"
  );
}


// ============================================================
// FINALIZAR REMADA
// ============================================================

void finalizarRemada() {

  unsigned long agora = millis();


  // ----------------------------------------------------------
  // CALCULA VOGA
  // ----------------------------------------------------------

  if (ultimaRemada > 0) {

    unsigned long intervalo =
      agora - ultimaRemada;


    if (intervalo > 0) {

      float novaVoga =
        60000.0 / intervalo;


      if (
        novaVoga >= 5.0 &&
        novaVoga <= 60.0
      ) {

        voga = novaVoga;
      }
    }
  }


  // ----------------------------------------------------------
  // INCREMENTA REMADAS
  // ----------------------------------------------------------

  remadas++;


  ultimaRemada = agora;


  // ----------------------------------------------------------
  // RESULTADO
  // ----------------------------------------------------------

  Serial.println();

  Serial.println(
    "================================"
  );

  Serial.println(
    ">>> REMADA FINALIZADA"
  );

  Serial.println(
    "================================"
  );


  Serial.print(
    "REMADA: "
  );

  Serial.println(remadas);


  Serial.print(
    "PICO: "
  );

  Serial.println(
    pico,
    2
  );


  Serial.print(
    "VOGA: "
  );

  Serial.println(
    voga,
    1
  );


  Serial.println();


  // ----------------------------------------------------------
  // VOLTA PARA ESPERA
  // ----------------------------------------------------------

  estado = ESPERANDO;

  pico = 0;
}


// ============================================================
// MOSTRAR SERIAL
// ============================================================

void mostrarSerial() {

  Serial.print("MOV: ");

  Serial.print(
    movimento,
    2
  );


  Serial.print(" | X: ");

  Serial.print(
    movX,
    2
  );


  Serial.print(" | Y: ");

  Serial.print(
    movY,
    2
  );


  Serial.print(" | Z: ");

  Serial.print(
    movZ,
    2
  );


  Serial.print(" | REMADAS: ");

  Serial.print(
    remadas
  );


  Serial.print(" | VOGA: ");

  Serial.print(
    voga,
    1
  );


  Serial.print(" | PICO: ");

  Serial.print(
    pico,
    2
  );


  Serial.print(" | STATUS: ");

  Serial.println(
    nomeEstado()
  );
}


// ============================================================
// OLED
// ============================================================

void mostrarOLED() {

  display.clearDisplay();


  display.setTextSize(1);

  display.setTextColor(
    SSD1306_WHITE
  );


  // ----------------------------------------------------------
  // TÍTULO
  // ----------------------------------------------------------

  display.setCursor(
    0,
    0
  );

  display.print(
    "ROWINGCOACH V9.0"
  );


  // ----------------------------------------------------------
  // MOVIMENTO
  // ----------------------------------------------------------

  display.setCursor(
    0,
    14
  );

  display.print(
    "MOV: "
  );

  display.print(
    movimento,
    1
  );


  // ----------------------------------------------------------
  // REMADAS
  // ----------------------------------------------------------

  display.setCursor(
    0,
    26
  );

  display.print(
    "REMADAS: "
  );

  display.print(
    remadas
  );


  // ----------------------------------------------------------
  // VOGA
  // ----------------------------------------------------------

  display.setCursor(
    0,
    38
  );

  display.print(
    "VOGA: "
  );

  display.print(
    voga,
    1
  );


  // ----------------------------------------------------------
  // STATUS
  // ----------------------------------------------------------

  display.setCursor(
    0,
    50
  );


  if (simulando) {

    display.print(
      "SIMULANDO..."
    );

  }
  else {

    display.print(
      nomeEstado()
    );
  }


  display.display();
}


// ============================================================
// SIMULA UMA ETAPA
// ============================================================

void simularEtapa(
  const char* nome,
  float x,
  float y,
  float z,
  int duracao
) {


  Serial.print(
    "Etapa: "
  );

  Serial.println(
    nome
  );


  movX = x;

  movY = y;

  movZ = z;


  movimento = sqrt(

    movX * movX +

    movY * movY +

    movZ * movZ
  );


  if (
    movimento > pico
  ) {

    pico = movimento;
  }


  unsigned long inicio =
    millis();


  while (
    millis() - inicio < duracao
  ) {

    mostrarSerial();

    mostrarOLED();

    delay(INTERVALO_LEITURA);
  }
}


// ============================================================
// SIMULA UMA REMADA COMPLETA
// ============================================================

void simularRemada() {


  // ----------------------------------------------------------
  // BLOQUEIA NOVOS CLIQUES
  // ----------------------------------------------------------

  if (simulando) {

    return;
  }


  simulando = true;


  Serial.println();

  Serial.println(
    "================================"
  );

  Serial.println(
    ">>> SIMULACAO DE REMADA"
  );

  Serial.println(
    "================================"
  );


  // ----------------------------------------------------------
  // PREPARAÇÃO
  // ----------------------------------------------------------

  estado = ESPERANDO;

  pico = 0;


  simularEtapa(

    "PREPARACAO",

    0.0,
    0.0,
    0.0,

    300
  );


  // ----------------------------------------------------------
  // INÍCIO DA PUXADA
  // ----------------------------------------------------------

  simularEtapa(

    "INICIO DA PUXADA",

    0.50,
    0.30,
    0.50,

    250
  );


  // ----------------------------------------------------------
  // PUXADA FORTE
  // ----------------------------------------------------------

  movX = 2.0;

  movY = 1.5;

  movZ = 2.0;


  movimento = sqrt(

    movX * movX +

    movY * movY +

    movZ * movZ
  );


  iniciarRemada();


  simularEtapa(

    "PUXADA FORTE",

    2.0,
    1.5,
    2.0,

    200
  );


  // ----------------------------------------------------------
  // PICO
  // ----------------------------------------------------------

  simularEtapa(

    "PICO DA REMADA",

    4.5,
    3.0,
    4.0,

    200
  );


  // ----------------------------------------------------------
  // FINAL DA PUXADA
  // ----------------------------------------------------------

  simularEtapa(

    "FINAL DA PUXADA",

    5.5,
    4.0,
    5.0,

    200
  );


  // ----------------------------------------------------------
  // RETORNO
  // ----------------------------------------------------------

  simularEtapa(

    "RETORNO",

    2.0,
    1.0,
    1.5,

    350
  );


  // ----------------------------------------------------------
  // REPOUSO
  // ----------------------------------------------------------

  simularEtapa(

    "REPOUSO",

    0.5,
    0.4,
    0.5,

    300
  );


  // ----------------------------------------------------------
  // FINALIZA
  // ----------------------------------------------------------

  Serial.println();

  Serial.println(
    "================================"
  );

  Serial.println(
    ">>> SIMULACAO TERMINADA"
  );

  Serial.println(
    "================================"
  );


  finalizarRemada();


  // ----------------------------------------------------------
  // ZERA MOVIMENTO
  // ----------------------------------------------------------

  movX = 0;

  movY = 0;

  movZ = 0;

  movimento = 0;


  simulando = false;


  mostrarOLED();
}


// ============================================================
// LEITURA DO BOTÃO
// ============================================================

void verificarBotao() {

  bool leitura =
    digitalRead(
      BOTAO_TESTE
    );


  // ----------------------------------------------------------
  // DETECTOU MUDANÇA
  // ----------------------------------------------------------

  if (
    leitura != ultimoEstadoBotao
  ) {

    ultimoDebounce =
      millis();
  }


  // ----------------------------------------------------------
  // DEBOUNCE
  // ----------------------------------------------------------

  if (
    millis() - ultimoDebounce >
    DEBOUNCE_MS
  ) {


    // --------------------------------------------------------
    // BOTÃO PRESSIONADO
    // --------------------------------------------------------

    if (
      leitura == LOW &&
      ultimoEstadoBotao == HIGH
    ) {


      Serial.println();

      Serial.println(
        ">>> BOTAO DE TESTE PRESSIONADO"
      );


      // ------------------------------------------------------
      // SOMENTE ACEITA SE NÃO ESTIVER SIMULANDO
      // ------------------------------------------------------

      if (!simulando) {

        simularRemada();

      }
      else {

        Serial.println(
          ">>> IGNORADO: simulacao em andamento"
        );
      }
    }
  }


  ultimoEstadoBotao =
    leitura;
}


// ============================================================
// SETUP
// ============================================================

void setup() {

  Serial.begin(
    115200
  );


  delay(500);


  Serial.println();

  Serial.println(
    "=============================="
  );

  Serial.println(
    "       ROWINGCOACH V9.0"
  );

  Serial.println(
    "=============================="
  );


  Serial.println(
    "Botao de teste: GPIO 4"
  );

  Serial.println(
    "Botao ligado entre GPIO 4 e GND"
  );


  // ==========================================================
  // BOTÃO
  // ==========================================================

  pinMode(
    BOTAO_TESTE,
    INPUT_PULLUP
  );


  // ==========================================================
  // OLED
  // ==========================================================

  if (
    !display.begin(
      SSD1306_SWITCHCAPVCC,
      0x3C
    )
  ) {

    Serial.println(
      "ERRO OLED!"
    );


    while (true) {

      delay(1000);
    }
  }


  Serial.println(
    "OLED OK!"
  );


  // ==========================================================
  // MPU6050
  // ==========================================================

  if (
    !mpu.begin()
  ) {

    Serial.println(
      "ERRO MPU6050!"
    );


    while (true) {

      delay(1000);
    }
  }


  Serial.println(
    "MPU6050 OK!"
  );


  // ==========================================================
  // CONFIGURAÇÃO MPU6050
  // ==========================================================

  mpu.setAccelerometerRange(
    MPU6050_RANGE_4_G
  );


  mpu.setGyroRange(
    MPU6050_RANGE_500_DEG
  );


  mpu.setFilterBandwidth(
    MPU6050_BAND_21_HZ
  );


  Serial.println(
    "Sensor configurado!"
  );


  // ==========================================================
  // TELA DE CALIBRAÇÃO
  // ==========================================================

  display.clearDisplay();


  display.setTextSize(1);

  display.setTextColor(
    SSD1306_WHITE
  );


  display.setCursor(
    0,
    0
  );

  display.println(
    "ROWINGCOACH V9.0"
  );


  display.setCursor(
    0,
    18
  );

  display.println(
    "Calibrando..."
  );


  display.display();


  delay(500);


  // ==========================================================
  // CALIBRA
  // ==========================================================

  calibrarReferencia();


  // ==========================================================
  // SISTEMA PRONTO
  // ==========================================================

  display.clearDisplay();


  display.setCursor(
    0,
    0
  );

  display.println(
    "ROWINGCOACH V9.0"
  );


  display.setCursor(
    0,
    16
  );

  display.println(
    "SISTEMA PRONTO"
  );


  display.setCursor(
    0,
    30
  );

  display.println(
    "1 clique ="
  );


  display.setCursor(
    0,
    42
  );

  display.println(
    "1 remada"
  );


  display.display();


  Serial.println();

  Serial.println(
    "================================"
  );

  Serial.println(
    "SISTEMA PRONTO"
  );

  Serial.println(
    "================================"
  );

  Serial.println(
    "BOTAO DE TESTE: GPIO 4"
  );

  Serial.println(
    "1 clique = 1 remada simulada"
  );

  Serial.println();
}


// ============================================================
// LOOP
// ============================================================

void loop() {


  // ==========================================================
  // BOTÃO
  // ==========================================================

  verificarBotao();


  // ==========================================================
  // LEITURA NORMAL DO MPU6050
  // ==========================================================

  unsigned long agora =
    millis();


  if (
    agora - ultimaLeitura >=
    INTERVALO_LEITURA
  ) {

    ultimaLeitura =
      agora;


    // --------------------------------------------------------
    // NÃO ALTERA OS VALORES DURANTE A SIMULAÇÃO
    // --------------------------------------------------------

    if (!simulando) {

      lerSensor();

      mostrarSerial();

      mostrarOLED();
    }
  }
}