//YWROBOT
//Compatible with the Arduino IDE 1.0
//Library version:1.1
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27,20,4);  // set the LCD address to 0x27 for a 16 chars and 2 line display


byte lock_char[] = {
  0x1F,
  0x11,
  0x11,
  0x1F,
  0x1B,
  0x1B,
  0x1F,
  0x00
};

byte phi[] = {
  0x00,
  0x12,
  0x15,
  0x15,
  0x0E,
  0x04,
  0x04,
  0x00
};

byte Receiver_char[] = {
  0x02,
  0x02,
  0x02,
  0x1F,
  0x13,
  0x13,
  0x1F,
  0x00
};

byte Transmitter_char[] = {
  0x00,
  0x04,
  0x0A,
  0x0E,
  0x0E,
  0x04,
  0x0A,
  0x11
};

byte batteryAlert[] = {
  0x03,
  0x03,
  0x03,
  0x03,
  0x03,
  0x00,
  0x03,
  0x03
};

byte battery100[] = {
  0x0E,
  0x1F,
  0x1F,
  0x1F,
  0x1F,
  0x1F,
  0x1F,
  0x1F
};

byte battery80[] = {
  0x0E,
  0x1F,
  0x11,
  0x1F,
  0x1F,
  0x1F,
  0x1F,
  0x1F
};

byte battery60[] = {
  0x0E,
  0x1F,
  0x11,
  0x11,
  0x1F,
  0x1F,
  0x1F,
  0x1F
};

byte battery40[] = {
  0x0E,
  0x1F,
  0x11,
  0x11,
  0x11,
  0x1F,
  0x1F,
  0x1F
};

byte battery20[] = {
  0x0E,
  0x1F,
  0x11,
  0x11,
  0x11,
  0x11,
  0x1F,
  0x1F
};

int Battery[] = {battery100,battery80,battery60,battery40,battery20};
int BatteryLevelR;
int BatteryLevelT;

String s[] = {""," ","  ","   ","    ","     ","      "};

int thetaMultis[] = {1,2,5,10,20,50};
int thetaMulti = 0;
bool thetaLock = false;

int phiMultis[] = {1,2,5,10,20,50};
int phiMulti = 0;
bool phiLock = false;




#define ENCODER_CLK1 18
#define ENCODER_DT1 30
#define ENCODER_SW1 31

int previousDT1;
int previousCLK1;
int previousSW1;
int ENCODER_POS1 = 0;
int ENCODER_PRE1;

#define ENCODER_CLK2 19
#define ENCODER_DT2 32
#define ENCODER_SW2 33

int previousDT2;
int previousCLK2;
int previousSW2;  
int ENCODER_POS2 = 0;
int ENCODER_PRE2;

float M1[4];
float M2[4];
float M3[4];

int idcnt = 0;

float actualTheta = 0;
float actualPhi = 0;


void setup()
{
  lcd.init();
  lcd.backlight();

  pinMode(A0, INPUT);
  pinMode(A1, INPUT);

  pinMode(34, INPUT_PULLUP);
  pinMode(35, INPUT_PULLUP);

  pinMode(26, INPUT_PULLUP);
  pinMode(53, INPUT_PULLUP);

  pinMode(ENCODER_CLK1, INPUT);
  pinMode(ENCODER_DT1, INPUT);
  pinMode(ENCODER_SW1, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK1), readEncoder1, FALLING);
  previousDT1 = digitalRead(ENCODER_DT1);
  previousCLK1 = digitalRead(ENCODER_CLK1);
  previousSW1 = digitalRead(ENCODER_SW1);  
  

  pinMode(ENCODER_CLK2, INPUT);
  pinMode(ENCODER_DT2, INPUT);
  pinMode(ENCODER_SW2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK2), readEncoder2, FALLING);
  previousDT2 = digitalRead(ENCODER_DT2);
  previousCLK2 = digitalRead(ENCODER_CLK2);
  previousSW2 = digitalRead(ENCODER_SW2);
}

void readEncoder1() {
  int dtValue1 = digitalRead(ENCODER_DT1);
  if (dtValue1 == HIGH) {
    
    ENCODER_POS1++;

  }
  if (dtValue1 == LOW) {

    ENCODER_POS1--;

  }
}

void readEncoder2() {
  int dtValue2 = digitalRead(ENCODER_DT2);
  ENCODER_PRE2 = ENCODER_POS2;
  if (dtValue2 == HIGH) {
    
    ENCODER_POS2++;

  }
  if (dtValue2 == LOW) {

    ENCODER_POS2--;

  }
}


void loop()
{
  BatteryLevelR = map(analogRead(A0),0,1023,0,100);
  BatteryLevelT = map(analogRead(A1),0,1023,0,100);




  if (digitalRead(ENCODER_SW1) == LOW) {
    thetaLock = !thetaLock;
  } 
  if (digitalRead(ENCODER_SW2) == LOW) {
    phiLock = !phiLock;
  } 


  if (digitalRead(34) == LOW) {
    if (ENCODER_POS1 > ENCODER_PRE1){
      thetaMulti++;
    }
    if (ENCODER_POS1 < ENCODER_PRE1){
      thetaMulti--;
    }
    ENCODER_POS1=ENCODER_PRE1;
    if (thetaMulti<0) {thetaMulti=0;}
    if (thetaMulti>5) {thetaMulti=5;}
  } 
  else{
    if (thetaLock == false) {
      if (ENCODER_POS1 != ENCODER_PRE1){
        if (ENCODER_POS1 > ENCODER_PRE1){
          actualTheta++;
        }
        if (ENCODER_POS1 < ENCODER_PRE1){
          actualTheta--;
        }
      }
    }
    ENCODER_PRE1 = ENCODER_POS1;
  }
  
  M1[2]=actualTheta*thetaMultis[thetaMulti]*0.1;

  if (digitalRead(35) == LOW) {
    if (ENCODER_POS2 > ENCODER_PRE2){
      phiMulti++;
    }
    if (ENCODER_POS2 < ENCODER_PRE2){
      phiMulti--;
    }
    ENCODER_POS2=ENCODER_PRE2;
    if (phiMulti<0) {phiMulti=0;}
    if (phiMulti>5) {phiMulti=5;}
  } 
  else{
    if (phiLock == false && (digitalRead(53)!=LOW)) {

      if (ENCODER_POS2 != ENCODER_PRE2){
        if (ENCODER_POS2 > ENCODER_PRE2){actualPhi++;}
        if (ENCODER_POS2 < ENCODER_PRE2){actualPhi--;}
      }

      M1[3]=actualPhi*phiMultis[phiMulti]*0.1;

      if (M1[3] < -90.0) {M1[3]=-90.0;}

      if (M1[3] > 90.0) {
        M1[2] += 180.0;
        M1[3] -= 2*(M1[3]-90);
      }
    }
    ENCODER_PRE2 = ENCODER_POS2;
  }
  
  

  
  if (digitalRead(26) == LOW) {
    measure();
    for (int i=0; i<4; i++){
      M3[i]=M2[i];
      M2[i]=M1[i];
      M1[i]=0;
    }
  }







  principale();
}



void principale()
{
  lcd.setCursor(0,0);
  lcd.print(s[6-String(M3[1],3).length()]+String(M3[1],3)+s[7-String(M3[2],1).length()]+String(M3[2],1)+s[7-String(M3[3],1).length()]+String(M3[3],1)); 

  lcd.setCursor(0,1);
  lcd.print(s[6-String(M2[1],3).length()]+String(M2[1],3)+s[7-String(M2[2],1).length()]+String(M2[2],1)+s[7-String(M2[3],1).length()]+String(M2[3],1)); 

  lcd.setCursor(0,2);
  lcd.print(s[6-String(M1[1],3).length()]+String(M1[1],3)+s[7-String(M1[2],1).length()]+String(M1[2],1)+s[7-String(M1[3],1).length()]+String(M1[3],1)); 


/*
0 - batteryAlert
1 - batteryR
2 - Receiver_char
3 - batteryT
4 - Transmitter_char
5 - lock_char
6 - phi
7-
*/


  if (BatteryLevelR<10) {
    lcd.createChar(0,batteryAlert);
    lcd.setCursor(0,3);
    lcd.write(byte(0));
  }
  else {
    lcd.setCursor(0,3);
    lcd.print(" ");
  }

  lcd.createChar(1,Battery[map(BatteryLevelR,0,100,4,0)]);
  lcd.setCursor(1,3);
  lcd.write(byte(1));

  lcd.createChar(2,Receiver_char);
  lcd.setCursor(2,3);
  lcd.write(byte(2));

  if (BatteryLevelT<10) {
    lcd.createChar(0,batteryAlert);
    lcd.setCursor(3,3);
    lcd.write(byte(0));
  }
  else {
    lcd.setCursor(3,3);
    lcd.print(" ");
  }

  lcd.createChar(3,Battery[map(BatteryLevelT,0,100,4,0)]);
  lcd.setCursor(4,3);
  lcd.write(byte(3));

  lcd.createChar(4,Transmitter_char);
  lcd.setCursor(5,3);
  lcd.write(byte(4));
  
  lcd.print(s[1]);
  lcd.print(s[3-String(thetaMultis[thetaMulti]).length()]+String(thetaMultis[thetaMulti])+"x");
  lcd.write((char)0b11110010);

  
  if (thetaLock) {
    lcd.createChar(5,lock_char);
    lcd.setCursor(12,3);
    lcd.write(byte(5));
  }
  else {
    lcd.setCursor(12,3);
    lcd.print(s[1]);
  }


  lcd.print(s[4-String(phiMultis[phiMulti]).length()]+String(phiMultis[phiMulti])+"x");
  lcd.createChar(6,phi);
  lcd.setCursor(18,3);
  lcd.write(byte(6));

  if (phiLock) {
    lcd.createChar(5,lock_char);
    lcd.setCursor(19,3);
    lcd.write(byte(5));
  }
  else {
    lcd.setCursor(19,3);
    lcd.print(s[1]);
  }

  delay(10);
}


void measure(){

  M1[0]=idcnt;
  idcnt++;

  M1[1]=12.345;


}