#include <LiquidCrystal.h>
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

int latchPin = 5;
int clockPin = 6;
int dataPin = 4;

boolean dbg = false ; // activate debug logs to serial

/*
TO DO

complete 
  read_values_from_Serial () ;
  send_values_to_serial ();

  implement V/S varition input (to be evaluated if use buttons on rotary enc) 

*/


// ########## AUTOPILOT BUTTONS ##########

#define BTN_AP_MASTER 14
#define BTN_AP_HDG 15
#define BTN_AP_CRS 16
#define BTN_AP_ALT 17
#define BTN_AP_SPD 18
#define BTN_AP_VSP 19
#define BTN_AP_TOGGLE_SPD 20

#define BTN_VSP_UP 21
#define BTN_VSP_DOWN 22


// ########## AUTOPILOT LEDS ##########

#define LED_AP_MASTER 26
#define LED_AP_HDG 27
#define LED_AP_CRS 28
#define LED_AP_ALT 29
#define LED_AP_SPD 30
#define LED_AP_VSP 31

// ########## ROTARY ENCODERS + SWITCH ##########

#define ENCODER_HDG_CLK 38
#define ENCODER_HDG_DT 39
#define ENCODER_HDG_SW 50

#define ENCODER_CRS_CLK 40
#define ENCODER_CRS_DT 41
#define ENCODER_CRS_SW 51

#define ENCODER_ALT_CLK 42
#define ENCODER_ALT_DT 43
#define ENCODER_ALT_SW 52

#define ENCODER_SPD_CLK 44
#define ENCODER_SPD_DT 45
#define ENCODER_SPD_SW 53

// ############### 74HC595




// --------------------------------------

// buttons array
const int8_t buttonPins[] = { BTN_AP_MASTER, BTN_AP_HDG, BTN_AP_CRS, BTN_AP_ALT, BTN_AP_SPD, BTN_AP_VSP, BTN_AP_TOGGLE_SPD,
                            ENCODER_HDG_SW, ENCODER_CRS_SW, ENCODER_ALT_SW,ENCODER_SPD_SW,
                            BTN_VSP_UP,BTN_VSP_DOWN };

// leds array                            
const int8_t ledPins[] = { LED_AP_MASTER,LED_AP_HDG,LED_AP_CRS,LED_AP_ALT,LED_AP_SPD,LED_AP_VSP }; // check mapping between PINS & VARS in doLEDS()

// encoders arrays
const int8_t encoders_clk [] = {ENCODER_HDG_CLK ,ENCODER_CRS_CLK, ENCODER_ALT_CLK, ENCODER_SPD_CLK};
const int8_t encoders_dt  [] = {ENCODER_HDG_DT, ENCODER_CRS_DT, ENCODER_ALT_DT, ENCODER_SPD_DT};
      int8_t encoders_last[] = {HIGH,HIGH,HIGH,HIGH}; // last encoder clock value

// --------------------------------------



boolean showSPD = false ;
boolean displayChanged = true ;

// current AP settings (Random values just for testing porpuse - real values will be read from serial/usb)
int HDG = 355 ;
int CRS = 350 ;
int ALT = 210 ;
int SPD = 185 ;
int VSP = -1800 ;

// current plane values (Random values just for testing porpuse - real values will be read from serial/usb)
int current_plane_HDG = 274 ;
int current_plane_ALT = 105 ;
int current_plane_SPD = 145 ;
int current_plane_VSP = 1500 ;

// current AP leds values
boolean isAP  = false ;
boolean isCRS = false ;
boolean isHDG = false ;
boolean isALT = false ;
boolean isVSP = false ;
boolean isSPD = false ;

// request change AP switch
boolean change_switch_AP  = false ;
boolean change_switch_CRS = false ;
boolean change_switch_HDG = false ;
boolean change_switch_ALT = false ;
boolean change_switch_VSP = false ;
boolean change_switch_SPD = false ;

// request change AP values
boolean change_value_CRS = false ;
boolean change_value_HDG = false ;
boolean change_value_ALT = false ;
boolean change_value_VSP = false ;
boolean change_value_SPD = false ;

// new AP values to set
int new_value_CRS = 0 ;
int new_value_HDG = 0 ;
int new_value_ALT = 0 ;
int new_value_VSP = 0 ;
int new_value_SPD = 0 ;






unsigned long timer_read_values = millis();


void setup() {
  lcd.begin(16, 2);
  lcd.setCursor(0,0);
  lcd.print("Luc & Friends");
  lcd.setCursor(0,1);
  lcd.print("Autopilot panel");
  

  pinMode(latchPin, OUTPUT);
  pinMode(dataPin, OUTPUT);  
  pinMode(clockPin, OUTPUT);

  
  if(dbg)Serial.begin(115200);

  for (uint8_t i = 0; i < sizeof(buttonPins); i++) {
    pinMode(buttonPins[i], INPUT_PULLUP);
    if(dbg)Serial.println("Initialising PULLUP button #"+String(i)+" on pin #"+String(buttonPins[i]));
  }

  for (uint8_t i = 0; i < sizeof(ledPins); i++) {
    pinMode(ledPins[i], OUTPUT);
    if(dbg)Serial.println("Initialising OUTPUT led #"+String(i)+" on pin #"+String(ledPins[i]));
  }

  for (uint8_t i = 0; i < sizeof(encoders_clk); i++) {
    pinMode(encoders_clk[i], INPUT);
    pinMode(encoders_dt [i], INPUT);
    if(dbg)Serial.println("Initialising ENCODERS #"+String(i)+" CLK on pin #"+String(encoders_clk[i]) +" and DT on pin #"+String(encoders_dt[i]));
  }

  delay(500);
  
}

///////////////////////////////////////////
void loop() {

  //do_random_values();
 
  read_values_from_serial () ;

  doDisplay();
  doLEDS();
  doLEDS_ShiftRegister();

  read_buttons();
  read_encoders();

  send_values_to_serial ();

  

}



////////////////////////////////////////////
// functions declaration

String pad(int str, int n, char padChar = ' ');
String pad(String str, int n, char padChar = ' ');


////////////////////////////////////////////
// functions implementation




void read_encoders(){
  for (uint8_t i = 0; i < sizeof(encoders_clk); i++) {
    int value = 0 ;
    int newClk = digitalRead(encoders_clk[i]);  
    
    if (newClk != encoders_last[i] ) {
      // There was a change on the CLK pin

      boolean fast_rotate = (  millis() - timer_read_values ) < 100 ;
      timer_read_values = millis();

      encoders_last[i] = newClk;

      int dtValue = digitalRead(encoders_dt[i]);

      if (newClk == LOW && dtValue == HIGH) {
        if(dbg)Serial.println("Encoder #"+ String(i)+" -> Rotated clockwise ⏩");
        value = +1 ;
      }
      else
      if (newClk == LOW && dtValue == LOW) {
        if(dbg)Serial.println("Encoder #"+ String(i)+" -> Rotated counterclockwise ⏪");
        value = -1 ;
      }

      if(value!=0)
      switch(i){
          case 0: increaseHDG(value, fast_rotate); break;
          case 1: increaseCRS(value, fast_rotate); break;
          case 2: increaseALT(value, fast_rotate); break;
          case 3: increaseSPD(value, fast_rotate); break;
        }


    }
  }

}

int roundBy (int startVal, int roundVal, boolean up){

  if (startVal % roundVal !=0){
    if(dbg)Serial.println( "*** ROUNDING ***");
    if(dbg)Serial.println("  UP: "+String(up));
    if(dbg)Serial.println("  START VAL -> "+ String(startVal) ) ;

    
    int myDiv = startVal / roundVal ;
    startVal = roundVal * myDiv;

    if (up) 
      startVal += roundVal ;
    else
      startVal -= roundVal ;
    if(dbg)Serial.println("  NEW VAL --> " + String(startVal));
  }

  return startVal ;
}

void increaseHDG(int val, boolean fast_rotate){
  if(dbg)Serial.print("Increasing HDG "+String(HDG)+" by "+String(val));
  change_value_HDG = true ;
  new_value_HDG = HDG + val ;
  if (fast_rotate) new_value_HDG = roundBy(new_value_HDG, 10, val>0 );
  if(new_value_HDG>360) new_value_HDG = 1;
  if(new_value_HDG<1) new_value_HDG = 360;
  if(dbg)Serial.print(" -> new val: "+String(new_value_HDG));
}

void increaseCRS(int val, boolean fast_rotate){
  change_value_CRS = true ;
  new_value_CRS = CRS + val ;
  if (fast_rotate) new_value_CRS = roundBy(new_value_CRS, 10, val>0 );
  if(new_value_CRS>360) new_value_CRS = 1;
  if(new_value_CRS<1) new_value_CRS = 360;
}

void increaseALT(int val, boolean fast_rotate){
  change_value_ALT = true ;
  new_value_ALT = ALT + val ;
  if (fast_rotate) new_value_ALT = roundBy(new_value_ALT, 10, val>0 );
  if(new_value_ALT>400) new_value_ALT = 400;
  if(new_value_ALT<0) new_value_ALT = 0;
}

void increaseSPD(int val, boolean fast_rotate){
  change_value_SPD = true ;
  new_value_SPD = SPD + val ;
  if (fast_rotate) new_value_SPD = roundBy(new_value_SPD, 10, val>0 );
  if(new_value_SPD>500) new_value_SPD = 500;
  if(new_value_SPD<0) new_value_SPD = 0;  
  showSPD = true ;
}


void doLEDS(){
  for (uint8_t i = 0; i < sizeof(ledPins); i++) {
    boolean value = LOW ;
    switch( ledPins[i] ){
      case LED_AP_MASTER: if(isAP) value=HIGH; break;
      case LED_AP_HDG: if(isHDG) value=HIGH; break;
      case LED_AP_CRS: if(isCRS) value=HIGH; break;
      case LED_AP_ALT: if(isALT) value=HIGH; break;
      case LED_AP_SPD: if(isSPD) value=HIGH; break;
      case LED_AP_VSP: if(isVSP) value=HIGH; break;
    }
    digitalWrite(ledPins[i],  value);
    //if(dbg)Serial.print(" led #" + String(ledPins[i])+ " value" + value);
  }

}

void doLEDS_ShiftRegister()
{
  byte leds1 = 0;
  byte leds2 = 0;

  if(isAP)   bitSet(leds1, 0);
  if(isAP)   bitSet(leds1, 6);
  if(isAP)   bitSet(leds1, 7);

  if(isHDG)  bitSet(leds1, 1); 
  if(isCRS)  bitSet(leds1, 2);
  if(isALT)  bitSet(leds1, 3);
  if(isSPD)  bitSet(leds1, 4);
  if(isVSP)  bitSet(leds1, 5);


  if(isAP)   bitSet(leds2, 7);

  digitalWrite(latchPin, LOW);
  shiftOut(dataPin, clockPin, LSBFIRST, leds2 );
  shiftOut(dataPin, clockPin, LSBFIRST, leds1 );
  digitalWrite(latchPin, HIGH);
}


void send_values_to_serial (){

  boolean sendSomething = (
      change_switch_AP  
    || change_switch_CRS 
    || change_switch_HDG 
    || change_switch_ALT 
    || change_switch_VSP 
    || change_switch_SPD
    
    || change_value_CRS
    || change_value_HDG
    || change_value_ALT  
    || change_value_VSP 
    || change_value_SPD  
	) ;

  if (!sendSomething)return;

  if(dbg)Serial.println("Sending values to USB ...");

  // To be implemented


  // testing
  {
	  displayChanged = true ;
    if(change_switch_AP  ) { isAP  = !isAP ; change_switch_AP  = false ;}
    if(change_switch_CRS ) { isCRS = !isCRS; change_switch_CRS = false ;}
    if(change_switch_HDG ) { isHDG = !isHDG; change_switch_HDG = false ;}
    if(change_switch_ALT ) { isALT = !isALT; change_switch_ALT = false ;}
    if(change_switch_VSP ) { isVSP = !isVSP; change_switch_VSP = false ;}
    if(change_switch_SPD ) { isSPD = !isSPD; change_switch_SPD = false ;}
	
	  if(change_value_CRS ) { CRS = new_value_CRS; change_value_CRS = false ; }
	  if(change_value_HDG ) { HDG = new_value_HDG; change_value_HDG = false ; }
	  if(change_value_ALT ) { ALT = new_value_ALT; change_value_ALT = false ; }
	  if(change_value_VSP ) { VSP = new_value_VSP; change_value_VSP = false ; }
	  if(change_value_SPD ) { SPD = new_value_SPD; change_value_SPD = false ; }
  }


 

}

void read_values_from_serial () {
  // To be implemented
  return;

  isAP  = false ;
  isCRS = false ;
  isHDG = false ;
  isALT = false ;
  isVSP = false ;
  isSPD = false ;

  current_plane_HDG = 274 ;
  current_plane_ALT = 105 ;
  current_plane_SPD = 145 ;
  current_plane_VSP = 1500 ;

  if(dbg)Serial.println("Reading values from USB ...");
}


void do_random_values (){

    if ( millis() - timer_read_values > 3000 ){

      timer_read_values = millis();

      //if(dbg)Serial.println("Randomizing for test ...");

      int rndVal = random(-5,+5);

      switch ( random(7)) {
        case 0 : 
          HDG += rndVal; 
          if (HDG>360) HDG=5;
          if (HDG<=0) HDG=360;
          displayChanged = true ;
          break;

        case 1 : 
          CRS += rndVal; 
          if (CRS>360) CRS=5;
          if (CRS<=0) CRS=360;
          displayChanged = true ;
          break;  
          
        case 2 : 
          ALT += rndVal; 
          if (ALT>360) ALT=360;
          if (ALT<=0) ALT=0;
          displayChanged = true ;
          break; 
          
        case 3 : 
          VSP += rndVal*100; 
          if (VSP>4000) VSP=4000;
          if (VSP<-3000) VSP=-3000;
          displayChanged = true ;
          showSPD = false ;
          break; 
          
        case 4 : 
          SPD += rndVal; 
          if (SPD>250) SPD=250;
          if (SPD<=0) SPD=0;
          showSPD = true ;
          displayChanged = true ;
          break;           

        default:
          break;
      }

    }

}

void doDisplay(){

  if (!displayChanged)return;

  if(dbg)Serial.println("Updating display ...");

  lcd.setCursor(0,0);
  lcd.print("HDG CRS ALT ");

  if(showSPD)
    lcd.print("SPD ");
  else
    lcd.print("V/S ");


  lcd.setCursor(0,1);

  lcd.print( pad( HDG ,-3,' ') +" ");
  lcd.print( pad( CRS ,-3,' ') +" ");
  lcd.print( pad( ALT ,-3,' ') );

  if (showSPD)
    lcd.print( " "+pad( SPD ,-3,' ')+" ");
  else {
   
    lcd.print( pad( VSP ,-5,' ')+" ");
  }

  lcd.print( " ");

  displayChanged = false ;
}

void read_buttons() {

 for (uint8_t i = 0; i < sizeof(buttonPins); i++) {
    if (digitalRead(buttonPins[i]) == LOW) {

      if(dbg)Serial.println("INPUT button " + String(buttonPins[i])+ " pressed" );

      switch (buttonPins[i]){

        case BTN_AP_MASTER:
          change_switch_AP = !change_switch_AP;
          break;

        case BTN_AP_HDG:
          change_switch_HDG = !change_switch_HDG;
          if(change_switch_HDG & !isHDG & isCRS) change_switch_CRS=true;
          break;       
        case BTN_AP_CRS:
          change_switch_CRS = !change_switch_CRS;
          if(change_switch_CRS & !isCRS & isHDG) change_switch_HDG=true;
          break;    
        case BTN_AP_ALT:
          change_switch_ALT = !change_switch_ALT;
          if(change_switch_ALT) change_switch_VSP=false;
          break;     
        case BTN_AP_SPD:
          change_switch_SPD = !change_switch_SPD;
          if(change_switch_SPD) {
            change_switch_VSP = false ;
            showSPD = true;
            displayChanged = true ;
          }
          break;                        
        case BTN_AP_VSP:
          change_switch_VSP = !change_switch_VSP;
          if(change_switch_VSP) {
            change_switch_ALT = false ;
            showSPD = false;
            displayChanged = true ;
          }
          break; 

        case BTN_AP_TOGGLE_SPD:  
          showSPD = !showSPD;
          displayChanged = true ;
          break;

        case ENCODER_HDG_SW:  // push HDG rotary - set to current plane HDG
          change_value_HDG = true ;
          new_value_HDG = current_plane_HDG ;
          break;

        case ENCODER_CRS_SW:  // push CRS rotary - set to current plane HDG
          change_value_CRS = true ;
          new_value_CRS = current_plane_HDG ;
          break;

        case ENCODER_ALT_SW:  // push ALT rotary - set to current plane ALT
          change_value_ALT = true ;
          new_value_ALT = current_plane_ALT ;
          break;

        case ENCODER_SPD_SW:  // push SPD rotary - set to current plane SPD
          change_value_SPD = true ;
          new_value_SPD = current_plane_SPD ;
          showSPD = true;
          displayChanged = true ;
          break;

        case BTN_VSP_UP: // V/S UP ***** TEMP ****
          change_value_VSP = true ;
          new_value_VSP = VSP + 100;
          if (new_value_VSP>4000) new_value_VSP= 4000;
          showSPD = false;
          displayChanged = true ;
          break;

        case BTN_VSP_DOWN: // V/S DOWN ***** TEMP *****
          change_value_VSP = true ;
          new_value_VSP = VSP - 100;
          if (new_value_VSP<-4000) new_value_VSP= -4000;
          showSPD = false;
          displayChanged = true ;
          break;

        default:
        // nothing
        break;
      }

      //if(dbg)Serial.println(" change_switch_HDG " + String(change_switch_HDG)+ " - change_switch_CRS " + String(change_switch_CRS)+ " " );
      //if(dbg)Serial.println(" change_switch_ALT " + String(change_switch_ALT)+ " - change_switch_AP  " + String(change_switch_AP )+ " " );
      //if(dbg)Serial.println(" change_switch_SPD " + String(change_switch_SPD)+ " - change_switch_VSP " + String(change_switch_VSP)+ " " );

      delay(50);
      while (digitalRead(buttonPins[i]) == LOW) delay(50);

    }
  }
}

String pad(int str, int n, char padChar = ' '){
  return pad( String(str) ,n,padChar);
}

String pad(String str, int n, char padChar = ' ')
{
  
  if (n>0){
    while ( str.length() < n )
      str += padChar;
  }else{
    n = abs(n);
    while ( str.length() < n )
      str = padChar+str;

  }

  return str;
}