#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include <AccelStepper.h>
#include <EEPROM.h>


// Definição dos pinos para controle do motor de passo
#define stepPin 5 //Passo eixo X
#define dirPin 6 // Direção eixo X

//Inicialização do driver do motor de passo;
AccelStepper stepper1(AccelStepper::DRIVER, stepPin, dirPin);

//Definições para tela LCD i2c
#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

//Definição para os pinos do encoder rotativo
#define ENCODER_CLK 3
#define ENCODER_DT  4
#define ENCODER_SW  2

//Váriáveis para controle do encoder rotativo
int NewCLK,LastCLK;

// I2C address of the MPU-6050 - será substituido pelo encoder absoluto
const int mpuAddress = 0x69;          

//Inicia o LCD
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

//Variáveis para Leitura do MPU-6050
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
float xByGyro;
float x;

//Variáveis para controle de menu 
String menu1          [3] = { "Operation","Home", "Settings" };
String menu2          [6] = {"Step: ", "Stoptime: ", "Speed: ", "Accel: ", "Save", "Return" };
String menu2_step     [4] = {"+- 0.1 step","+- 1   step","+- 10  step","Return"};
String menu2_Stoptime [4] = {"+- 0.1 s","+- 1   s","+- 10  s","Return"};
String menu2_Speed    [4] = {"+- 10 steps/s","+- 100 steps /s","+- 1000 steps/s","Return"};
String menu2_Accel    [4] = {"+- 1 steps/s","+- 10 steps /s","+- 100 steps/s","Return"};
String menu3          [4] = {"Position:","Start","Cancel", "Return" };
String menu4          [3] = { "Timed","Manual", "Return" };
String menu5          [4] = { "Position:","Go","Back", "Return" };

bool menu_1_was            = true;
bool menu_3_was            = true;
bool menu_4_was            = true;
bool menu_5_was            = true;

bool  menu_1               = true;
bool  menu_2               = false;
bool  menu_3               = false;
bool  menu_4               = false;
bool  menu_5               = false;

bool  menu2_Step           = false;
bool  menu2_Step_was       = true;
bool  menu2_Step01         = false;
bool  menu2_Step01_was     = false;
bool  menu2_Step1          = false;
bool  menu2_Step10         = false;

bool  menu2_stop           = false;
bool  menu2_Stoptime_was   = true;
bool  menu2_Stoptime01     = false;
bool  menu2_Stoptime01_was = false;
bool  menu2_Stoptime1      = false;
bool  menu2_Stoptime10     = false;

bool  menu2_Sped           = false;
bool  menu2_Sped_was       = true;
bool  menu2_Sped10         = false;
bool  menu2_Sped10_was     = false;
bool  menu2_Sped100        = false;
bool  menu2_Sped1000       = false;

bool  menu2_Acel           = false;
bool  menu2_Acel_was       = true;
bool  menu2_Acel1          = false;
bool  menu2_Acel1_was      = false;
bool  menu2_Acel10         = false;
bool  menu2_Acel100        = false;



int opcoes_menu1           = 0;
int opcoes_menu2           = 0;
int opcoes_menu2_Step      = 0;
int opcoes_menu2_Stoptime  = 0;
int opcoes_menu2_Speed     = 0;
int opcoes_menu2_Accel     = 0;
int opcoes_menu2_Datetime  = 0;
int opcoes_menu3           = 0;
int opcoes_menu4           = 0;
int opcoes_menu5           = 0;

int seta_menu1             = 0;
int seta_menu2             = 0;
int seta_menu3             = 1;
int seta_menu4             = 0;
int seta_menu5             = 1;

int seta_menu2_step        = 0;
int seta_menu2_stoptime    = 0;
int seta_menu2_speed       = 0;
int seta_menu2_accel       = 0;
int seta_menu2_Datetime    = 0;

//Váriaveis para iniciar do motor de passo pela primeira vez
bool anda_motor            = false;
float speed                = 10000;
float accel                = 500;
float step                 = 0.5;
float timestop             = 3;
float posicao_desejada     = 0.0;

float sel_step;
float sel_time;
int   sel_speed;
int   sel_accel;

//Váriavel para criação de caracter "ao quadrado" (²)
byte newCharSquare[8] = {
  B01100,
  B10010,
  B00100,
  B01000,
  B11110,
  B00000,
  B00000,
  B00000
};

//Variável para controle de memória
int address_step            = 0;
int address_timestop        = 1;
int address_velo            = 2;
int address_acel            = 3;
int address_flag            = 255;
int address                 = 0;

int cont_opcoes;
int inicio, fim, meio;
int ano;

float data [4] = {0.5 , 3 , 10000, 500};
//data[0]: step
//data[1]: timestop
//data[2]: speed
//data[3]: accel

volatile unsigned long previousMillis = 0;  // will store last time LED was updated
volatile unsigned long currentMillis = 0;
bool anda_motor_giroTodo = false;
bool start_count = false;


void setup() {
  // put your setup code here, to run once:
  pinMode(LED_BUILTIN, OUTPUT);

  //Pinos do encoder rotativo como entradas
  pinMode(ENCODER_CLK, INPUT_PULLUP);
  pinMode(ENCODER_DT, INPUT_PULLUP);
  pinMode(ENCODER_SW, INPUT_PULLUP);

  //Pinos para controle do motor de passo como saídas
  pinMode(stepPin,OUTPUT); 
  pinMode(dirPin,OUTPUT);

  //Interrupt para clique do botão do encoder rotativo e giro do encoder rotativo
  attachInterrupt(digitalPinToInterrupt(ENCODER_SW), Seleciona_acao, FALLING);
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), Muda_acao, CHANGE);

  //Inicio da comunicação serial - APENAS PARA TESTE
  Serial.begin( 115200);

  //Inicio da comunica I2C
  Wire.begin();
  
  // Initialize the MPU-6050 and test if it is connected.
  Wire.beginTransmission( mpuAddress);
  Wire.write( 0x6B);                           // PWR_MGMT_1 register
  Wire.write( 0);                              // set to zero (wakes up the MPU-6050)
  auto error = Wire.endTransmission();
  if( error != 0)
  {
    Serial.println(F( "Error, MPU-6050 not found"));
    for(;;);                                   // halt the sketch if error encountered
  }

  // Inicia o LCD e escreve mensagem inicial
  lcd.init();
  lcd.setBacklight(HIGH);
  lcd.clear();
  //Posiciona o cursor na coluna 3, linha 0;
  lcd.setCursor(3, 0);
  //Envia o texto entre aspas para o LCD
  lcd.print("Integral IMI");
  delay(500);
  lcd.clear();
  //cria caracter "ao quadrado" (²)
  lcd.createChar(0, newCharSquare);

  //Le memória para usar valores
  if(EEPROM.read(address_flag) == 1){
    EEPROM.get(address_step, data[0]);
    address += sizeof(float);
    EEPROM.get(address, data[1]);
    address += sizeof(float);
    EEPROM.get(address, data[2]);
    address += sizeof(float);
    EEPROM.get(address, data[3]);
    }
  else{//Escreve na memório valores padrões caso seja a primeira vez ligando
    EEPROM.put(address, step);
    address += sizeof(float);
    EEPROM.put(address, timestop);
    address += sizeof(float);
    EEPROM.put(address, speed);
    address += sizeof(float);
    EEPROM.put(address, accel);
    address += sizeof(float);
    EEPROM.put(address_flag, 1);

  }  
  // Seta velocidade e aceleração do motor de passo.
  stepper1.setMaxSpeed(2000000);
  stepper1.setSpeed(data[2]);
  stepper1.setAcceleration(data[3]);
}

void loop() {
  // put your main code here, to run repeatedly:
  //Atualiza o Display
  updateLcd();

  //Faz a leitura do MPU-6050
  Wire.beginTransmission( mpuAddress);
  Wire.write( 0x3B);                   // Starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission( false);        // No stop condition for a repeated start

  Wire.requestFrom( mpuAddress, 14);   // request a total of 14 bytes
  AcX = Wire.read()<<8 | Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY = Wire.read()<<8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read()<<8 | Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read()<<8 | Wire.read();  // 0x41 (TEMP_OUT_H)   & 0x42 (TEMP_OUT_L)
  GyX = Wire.read()<<8 | Wire.read();  // 0x43 (GYRO_XOUT_H)  & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read()<<8 | Wire.read();  // 0x45 (GYRO_YOUT_H)  & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read()<<8 | Wire.read();  // 0x47 (GYRO_ZOUT_H)  & 0x48 (GYRO_ZOUT_L)
  
  float xByAccel = (float) AcX * 0.0001;      // static angle by accelerometer
  xByGyro += (float) GyX * 0.00001;
  x = xByAccel + xByGyro;
  
  //movimenta ou para o motor de passo
  if(anda_motor){
    stepper1.run();
    if (abs(posicao_desejada - x) <= 0.04){
      stepper1.stop();
      anda_motor = false;
    }
  }
  else if(anda_motor_giroTodo){
    currentMillis = millis();
    if(start_count){
      previousMillis = millis();
      start_count = false;
    }
    if (currentMillis - previousMillis >= data[1]*1000) {
     
      stepper1.run();
      if (abs(posicao_desejada - x) <= 0.04){
        stepper1.stop();
        //anda_motor = false;
        start_count = true;
        posicao_desejada = posicao_desejada + data[0];
      }
    }  
  }    
}