/*//57.2957795*/
#include <EEPROM.h>
#include <LiquidCrystal.h>
#include <SPI.h>
#include <Wire.h>

#define btnCAMBER 8
#define btnSALVAR 9
#define btnCASTER 10
#define btnZERO 11
#define btnLASER 12
#define laser 13

const int ENDERECO_SENSOR=0x68;

float girX,girY,girZ,acelX,acelY,acelZ,temperatura;
float atu,atu2,nivel,camber,caster,kpi,camberI,camberF,casterI,casterF,kpiI,kpiF,zero,angVal,calibraX,calibraY;

/*float RateRoll,RatePitch,RateYaw,AccX,AccY,AccZ,AngleRoll,AnglePitch,LoopTimer;
float KalmanAngleRoll=0,KalmanUncertaintyAngleRoll=2*2;
float KalmanAnglePitch=0,KalmanUncertaintyAnglePitch=2*2;
float Kalman1DOutput[]={0,0};*/

int volt = 0;
int voltA = 0;
int perc = 2;
int limpa = 0;
int volta = 0;
int valorbtnCAMBER = 0;
int valorbtnSALVAR = 0;
int valorbtnCASTER = 0;
int valorbtnZERO = 0;
int valorbtnLASER = 0;
int valorlaser = 0;
int lado = 0;

unsigned long tBolha=0;
unsigned long tNivel=0; 
unsigned long timer=0;

bool cBolha = false;
bool cNivel = false;
bool inicio = true;
bool show = true;
bool nrep = true;
bool vrep = true;
bool menuCALIBRAR = false;
bool menuCAMBER = false;
bool menuCASTER = false;

byte sbolha[8]{B11111,B11111,B10001,B00000,B00000,B10001,B11111,B11111,};
byte bateria100[8]{B01110,B11111,B11111,B11111,B11111,B11111,B11111,B00000,};
byte bateria80[8]{B01110,B10001,B10001,B11111,B11111,B11111,B11111,B00000,};
byte bateria40[8]{B01110,B10001,B10001,B10001,B10001,B11111,B11111,B00000,};
byte bateria0[8]{B01110,B10001,B10001,B10001,B10001,B10001,B11111,B00000,};

/*const int MPU_addr=0x68;
int16_t axis_X,axis_Y,axis_Z;
int minVal=265;
int maxVal=402;
double x;
double y;
double z;*/

LiquidCrystal lcd(2,3,4,5,6,7);

void setup() {
  // put your setup code here, to run once:
  lcd.createChar(0,sbolha);
  lcd.createChar(1,bateria100);
  lcd.createChar(2,bateria80);
  lcd.createChar(3,bateria40);
  lcd.createChar(4,bateria0);

  pinMode(btnCAMBER, INPUT);
  pinMode(btnSALVAR, INPUT);
  pinMode(btnCASTER, INPUT);
  pinMode(btnZERO, INPUT);
  pinMode(btnLASER, INPUT);
  pinMode(laser, OUTPUT);

  girX=girY=girZ=acelX=acelY=acelZ=temperatura=0;
  atu=atu2=nivel=camber=caster=kpi=camberI=camberF=casterI=casterF=kpiI=kpiF=zero=angVal=0.00;

  if (EEPROM[0] == 9){
    EEPROM.get(0, calibraX);
    EEPROM.get(4, calibraY);
  } else {
    calibraX=calibraY=-0.05;
  }
  
  Serial.begin(9600);

  //Inicia biblioteca
  Wire.begin();
  Wire.beginTransmission(ENDERECO_SENSOR);
  Wire.write(0x6B);

  //Inicia Sensor
  Wire.write(0);
  Wire.endTransmission(true);

  lcd.begin(16,2);
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("   ALINHADOR");
  lcd.setCursor(0,1);
  lcd.print("    DIGITAL");
  delay(2000);
  lcd.clear();

  /*Serial.begin(57600);
  Wire.setClock(400000);
  Wire.begin();
  delay(250);  
  
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();*/

  //Serial.println(calibraX);

}

void loop() {
  // put your main code here, to run repeatedly:
  valorbtnCAMBER = digitalRead(btnCAMBER);
  valorbtnSALVAR = digitalRead(btnSALVAR);
  valorbtnCASTER = digitalRead(btnCASTER);
  valorbtnZERO = digitalRead(btnZERO);
  valorbtnLASER = digitalRead(btnLASER);

  if ((valorbtnZERO == 1 && valorbtnSALVAR == 1 && inicio == true) || (menuCALIBRAR == true)){
    show = true;
    telaCALIBRAR();
  }

  inicio=false;

  sensorNivel();

  volt = map(analogRead(A0),0,1023,0,100);

  espera();

  if (valorbtnSALVAR == 1 && menuCALIBRAR == false){
    show = true;
    telaSALVAR();
  } else if (valorbtnZERO == 1){
    show = true;
    menuCALIBRAR = false;
    telaZERO();
  } else if (valorbtnLASER == 1 && menuCALIBRAR == false){
    show = true;
    nrep = true;
    vrep = true;
    telaLASER();
  } else if ((valorbtnCAMBER == 1 || (menuCAMBER == true && valorbtnCASTER != 1)) && menuCALIBRAR == false){
    if (valorbtnCAMBER == 1){
      show = true;
      nrep = true;
      vrep = true;
    }
    telaCAMBER();
  } else if ((valorbtnCASTER == 1 || menuCASTER == true) && menuCALIBRAR == false){
    show = true;
    telaCASTER();
  } else {
    if (menuCALIBRAR == true){
      //menuCALIBRAR = true;
    } else {
      //show = true;
      menuCAMBER = true;
    }
  }

  delay(100);

  /*gyro_signals();
  Serial.print("Roll rate [°/s]= ");
  Serial.print(RateRoll);
  Serial.print(" Pitch rate [°/s]= ");
  Serial.print(RatePitch);
  Serial.print(" Yaw rate [°/s]= ");
  Serial.println(RateYaw);

  Serial.print("Acc X [g]= ");
  Serial.print(AccX);
  Serial.print(" Acc Y [g]= ");
  Serial.print(AccY);
  Serial.print(" Acc Z [g]= ");
  Serial.println(AccZ);

  kalman_1d(KalmanAngleRoll,KalmanUncertaintyAngleRoll,RateRoll,AngleRoll);
  KalmanAngleRoll=Kalman1DOutput[0];
  KalmanUncertaintyAngleRoll=Kalman1DOutput[1];
  kalman_1d(KalmanAnglePitch,KalmanUncertaintyAnglePitch,RatePitch,AnglePitch);
  KalmanAnglePitch=Kalman1DOutput[0];
  KalmanUncertaintyAnglePitch=Kalman1DOutput[1];

  Serial.print("Roll angle [°]= ");
  Serial.print(KalmanAngleRoll);
  Serial.print(" Pitch angle [°]]= ");
  Serial.println(KalmanAnglePitch);
  while(micros()-LoopTimer < 4000);
  LoopTimer = micros();

  delay(50);*/
}

void sensorNivel(){
  Wire.beginTransmission(ENDERECO_SENSOR);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(ENDERECO_SENSOR, 14, true);

  acelX = Wire.read()<<8|Wire.read();
  acelY = Wire.read()<<8|Wire.read();
  acelZ = Wire.read()<<8|Wire.read();
  
  temperatura = Wire.read()<<8|Wire.read();

  girX = Wire.read()<<8|Wire.read();
  girY = Wire.read()<<8|Wire.read();
  girZ = Wire.read()<<8|Wire.read();

  /*float angle = acelY;
  angle = angle/57.29577951;
  acelX=(64+(sin(angle)*25));
  acelY=(32-(cos(angle)*25));

  Serial.println(acelX);*/
  /*int xAng = map(acelX,minVal,maxVal,-90,90);
  int yAng = map(acelY,minVal,maxVal,-90,90);
  int zAng = map(acelZ,minVal,maxVal,-90,90);
  x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
  y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
  z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
  Serial.print("Angle of inclination in X axis = ");
  Serial.print(x);
  Serial.println((char)176);
  Serial.print("Angle of inclination in Y axis= ");
  Serial.print(y);
  Serial.println((char)176);
  Serial.print("Angle of inclination in Z axis= ");
  Serial.print(z);
  Serial.println((char)176);
  Serial.println("-------------------------------------------");
  delay(1000);*/


  return girX,girY,girZ,acelX,acelY,acelZ,temperatura; 
  /*
  Acelerometro
  +/-2g=16384 (-90° = -16384 / 90° = 16384) acelY/182.04 ---- 364.08
  +/-4g=8192
  +/-8g=4096
  +/-16g=2048

  Giroscopio
  +/-250'/s = 131
  +/-500'/s = 65.6
  +/-1000'/s = 32.8
  +/-2000'/s = 16.4
  */
}

void nBolha(float nivel){
  if(cBolha){
    if (lado == 0){
      nivel = nivel*(-1);
    }
    //Bolha Nivel
    if (nivel == 0.00){
      lcd.setCursor(11,0);
      lcd.write(byte(255));
      lcd.setCursor(12,0);
      lcd.write(byte(255));
      lcd.setCursor(13,0);
      lcd.write(byte(0));
      lcd.setCursor(14,0);
      lcd.write(byte(255));
      lcd.setCursor(15,0);
      lcd.write(byte(255));
    } else if (nivel > 0.00 && nivel <= 1.00){
      //nivelar();
      lcd.setCursor(11,0);
      lcd.write(byte(255));
      lcd.setCursor(12,0);
      lcd.write(byte(0));
      lcd.setCursor(13,0);
      lcd.write(byte(255));
      lcd.setCursor(14,0);
      lcd.write(byte(255));
      lcd.setCursor(15,0);
      lcd.write(byte(255));
    } else if (nivel > 1.00 && nivel < 2.00){
      //nivelar();
      lcd.setCursor(11,0);
      lcd.write(byte(0));
      lcd.setCursor(12,0);
      lcd.write(byte(255));
      lcd.setCursor(13,0);
      lcd.write(byte(255));
      lcd.setCursor(14,0);
      lcd.write(byte(255));
      lcd.setCursor(15,0);
      lcd.write(byte(255));
    } else if (nivel < 0.00 && nivel >= -1.00){
      //nivelar();
      lcd.setCursor(11,0);
      lcd.write(byte(255));
      lcd.setCursor(12,0);
      lcd.write(byte(255));
      lcd.setCursor(13,0);
      lcd.write(byte(255));
      lcd.setCursor(14,0);
      lcd.write(byte(0));
      lcd.setCursor(15,0);
      lcd.write(byte(255));
    } else if (nivel < -1.00 && nivel > -2.00){
      //nivelar();
      lcd.setCursor(11,0);
      lcd.write(byte(255));
      lcd.setCursor(12,0);
      lcd.write(byte(255));
      lcd.setCursor(13,0);
      lcd.write(byte(255));
      lcd.setCursor(14,0);
      lcd.write(byte(255));
      lcd.setCursor(15,0);
      lcd.write(byte(0));
    }   
    tBolha=millis();
    cBolha=false; 
  }
}

void nivelBateria(int carga){
  lcd.setCursor(3,0);
  lcd.write("        ");
  if (carga == 100){
      lcd.setCursor(0,0);
      lcd.write(byte(1));
      lcd.print(carga);
      lcd.write('%');
      perc = 2;
      
    } else {
      if (perc == 2){
        lcd.setCursor(1,0);
        lcd.print("    ");
      }
      perc = 1;
      if (carga >= 80){
        lcd.setCursor(0,0);
        lcd.write(byte(1));
        lcd.print(carga);
        lcd.write('%');
      } else if (carga >= 40){
        lcd.setCursor(0,0);
        lcd.write(byte(2));
        lcd.print(carga);
        lcd.write('%');
      } else if (carga >= 9){
        lcd.setCursor(0,0);
        lcd.write(byte(3));
        lcd.print(carga);
        lcd.write('%');
      } else {
        //Piscar Bateria
        if (perc == 1){
          lcd.setCursor(1,0);
          lcd.print("    ");
        }
        perc = 1;
        lcd.setCursor(0,0);
        lcd.write(byte(4));
        lcd.print(carga);
        lcd.write('%');
        delay(1000);
        lcd.setCursor(0,0);
        lcd.write(byte(32));
        lcd.print(carga);
        lcd.write('%');
        delay(300);
      }
    }
}

void nivelar(){
  if(cNivel){
    lcd.setCursor(0,0);
    lcd.print(" NIVELAR   ");
    delay(100);
    //lcd.clear();
    lcd.setCursor(0,0);
    lcd.print(" NIVELAR>  ");
    delay(100);
    //lcd.clear();
    lcd.setCursor(0,0);
    lcd.print(" NIVELAR>> ");
    delay(100);
    //lcd.clear();
    lcd.setCursor(0,0);
    lcd.print(" NIVELAR>>>");
    delay(100);
    //lcd.clear();
    lcd.setCursor(0,0);
    lcd.print(" NIVELAR >>");
    delay(100);
    //lcd.clear();
    lcd.setCursor(0,0);
    lcd.print(" NIVELAR  >");
    delay(100);
    //lcd.clear();
    tNivel=millis();
    cNivel=false;
  }
}

void espera(){
  if(!cBolha && millis()-tBolha>=50){
    cBolha = true;
  }
  if(!cNivel && millis()-tNivel>=300){
    cNivel = true;
  }
}

void telaCAMBER(){
  //lcd.clear();
  nivel = (acelY/16384);//-(calibraY);
  atu2 = acelX/182.04;
  //Serial.println(nivel);
  if (nivel != 0.00){
    nBolha(nivel);
    nivelar();
    nrep = true;
  } else {
    if (nrep == true){
      nBolha(nivel);
      nivelBateria(volt);
      nrep = false;
    }
  }
  if (volt != voltA){
    nivelBateria(volt);
    voltA = volt;
    vrep = true;
  } else {
    if (vrep == true){
      nBolha(nivel);
      nivelBateria(volt);
      vrep = false;
    } else if (volt < 9){
      nivelBateria(volt);
      vrep = false;
    }
  }
  if (show == true || atu != atu2){
    lcd.setCursor(0,1);
    lcd.print("CAMBER: ");
    lcd.setCursor(8,1);
    lcd.print("        ");
    lcd.setCursor(8,1);
    lcd.print(acelX/182.04);//-(calibraX));//16384);
    lcd.write(byte(223));
    //Serial.print(acelX/364.08);
    atu = atu2;
    show = false;
  }
  menuCAMBER = true;
  menuCASTER = false;
  limpa = 1;
  //Serial.println(vrep);
}

void telaSALVAR(){
  if (menuCASTER == true){
    if (casterF == 0){
      casterF = (acelX/182.04)-casterI;
      kpiF = (acelY/182.04)-kpiI;
      //casterI = acelX/16384;
      //kpiI = acelY/16384;
      caster = 0.00;
      kpi = 0.00;
      zero = 2;
      lcd.clear();
      lcd.print("  DADOS SALVOS");
      delay(1000);
      lcd.clear();
    } else {
      lcd.clear();
      lcd.print("   CASTER/KPI");
      lcd.setCursor(0,1);
      lcd.print("   CALCULADO!");
      delay(1000);
      lcd.clear();
    }
  }
  menuCAMBER = false;
}

void telaCASTER(){
  if (valorbtnCASTER == 1){
    lcd.clear();
  }
  if (zero == 0){
    if (acelY != kpiI){      
      telaSUJA();
      if (volta == 0){
        lcd.print(" PRESSIONE ZERO");
        volta = 1;
     }
    } else {
      lcd.setCursor(0,0);
      lcd.print("GIRE O PNEU 10");
      lcd.write(byte(223));
      lcd.setCursor(0,1);
      lcd.print("  PARA DENTRO");
      limpa = 1;
      volta = 0;
    }  
  } else if (zero == 1){
    if (acelY/182.04 != kpiI){
      telaSUJA();
      if (volta == 0){
        lcd.print("PRESSIONE SALVAR");
        volta = 1;
      }
    } else {
      lcd.setCursor(0,0);
      lcd.print("GIRE O PNEU 10");
      lcd.write(byte(223));
      lcd.setCursor(0,1);
      lcd.print("  PARA FORA");
      limpa = 1;
      volta = 0;
    }
  } else if (zero == 2){
    lcd.setCursor(0,0);
    lcd.print("CASTER: ");
    lcd.print(casterF);
    lcd.setCursor(0,1);
    lcd.print("KPI: ");
    lcd.print(kpiF);
  } else {
    casterF = (acelX/182.04)-casterI;
    kpiF = (acelY/182.04)-kpiI;
    if (caster != casterF){
      caster = casterF;
    }
    if (kpi != kpiF){
      kpi = kpiF;
    }
    //caster = caster-casterI;
    lcd.setCursor(0,0);
    lcd.print("CASTER: ");
    lcd.print(caster);
    lcd.setCursor(0,1);
    lcd.print("KPI: ");
    lcd.print(kpi);
  }
  menuCAMBER = false;
  menuCASTER = true;
}

void telaZERO(){
  if (menuCASTER == true){
    if (casterF == 0){
      casterI = acelX/182.04;
      kpiI = acelY/182.04;
      caster = 0.00;
      kpi = 0.00;
      zero = 1;
      lcd.clear();
      lcd.print("ZERO CASTER/KPI");
      delay(1000);
      lcd.clear();
      volta = 0;
    } else {
      lcd.clear();
      lcd.print("   CASTER/KPI");
      lcd.setCursor(0,1);
      lcd.print("   CALCULADO!");
      delay(1000);
      lcd.clear();
    }
  } else if (menuCAMBER == true){
    casterI = 0;
    kpiI = 0;
    casterF = 0;
    kpiF = 0;
    zero = 0;
    limpa = 0;
    show = true;
    nrep = true;
    vrep = true;
    lcd.clear();
    lcd.print("  ZERO INICIAL");
    delay(1000);
    lcd.clear();
  }
}

void telaLASER(){
  lcd.setCursor(15,1);
  //lcd.print("L");
  if (valorlaser == 0){
    digitalWrite(laser, HIGH);
    valorlaser = 1;
    lcd.clear();
    lcd.write("   LASER ON");
    lcd.setCursor(0,1);
    lcd.write("   O-------|");
    delay(1000);
  } else {
    digitalWrite(laser, LOW);
    valorlaser = 0;
    lcd.clear();
    lcd.write("   LASER OFF");
    lcd.setCursor(0,1);
    lcd.write("   0---*   |");
    delay(1000);
  }
  menuCAMBER = true;
}

void telaCALIBRAR(){
  lcd.clear();
  lcd.print("  CONFIGURAR");
  lcd.setCursor(0,1);
  lcd.write("    SISTEMA");
  delay(1000);
  lcd.clear();
  while(true){
    lcd.setCursor(0,0);
    lcd.print("X=CMB: ");
    lcd.setCursor(7,0);
    lcd.print(acelX/182.04);
    lcd.setCursor(0,1);
    lcd.write("Y=KPI: ");
    lcd.setCursor(7,1);
    lcd.print(acelY/182.04);

    calibraX=acelX/182.04;
    calibraY=acelY/182.04;

    EEPROM.put(0, calibraX);
    EEPROM.put(4, calibraY);
    //delay(5000);
    if (digitalRead(btnCAMBER) == HIGH){
      menuCAMBER = true;
      menuCALIBRAR = false;
      break;
    }
    if (digitalRead(btnZERO) == HIGH){
      menuCALIBRAR = false;
      break;
    }
  }
  menuCALIBRAR = true;
}

void telaSUJA(){
  if (limpa == 1){
    lcd.clear();
    limpa = 0;
  }
}

/*void gyro_signals(void){
  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();

  Wire.requestFrom(0x68,6);

  int16_t AccXLBS=Wire.read()<<8|Wire.read();
  int16_t AccYLBS=Wire.read()<<8|Wire.read();
  int16_t AccZLBS=Wire.read()<<8|Wire.read();

  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x8);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission();

  Wire.requestFrom(0x68,6);
  
  int16_t GyroX=Wire.read()<<8|Wire.read();
  int16_t GyroY=Wire.read()<<8|Wire.read();
  int16_t GyroZ=Wire.read()<<8|Wire.read();

  RateRoll=(float)GyroX/65.5;
  RatePitch=(float)GyroY/65.5;
  RateYaw=(float)GyroZ/65.5;

  AccX=(float)AccXLBS/4096;
  AccY=(float)AccYLBS/4096;
  AccZ=(float)AccZLBS/4096;

  AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*AccZ))*1/(3.142/180);
  AnglePitch=atan(AccX/sqrt(AccY*AccY+AccZ*AccZ))*1/(3.142/180);




}

void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement){
  KalmanState = KalmanState+0.004*KalmanInput;
  KalmanUncertainty = KalmanUncertainty+0.004*0.004*4*4;

  float KalmanGain = KalmanUncertainty*1/(1*KalmanUncertainty+3*3);
  KalmanState = KalmanState+KalmanGain*(KalmanMeasurement-KalmanState);
  KalmanUncertainty = (1-KalmanGain)*KalmanUncertainty;

  Kalman1DOutput[0] = KalmanState;
  Kalman1DOutput[1] = KalmanUncertainty;
}*/