//includes
  #include <LiquidCrystal.h>
  #include <LiquidMenu.h>
  #include <AbleButtons.h> 
  #include <RotaryEncoder.h>
//defines
  #define LCD_RS_Pin 12
  #define LCD_E_Pin 11
  #define LCD_D4_Pin 4
  #define LCD_D5_Pin 5
  #define LCD_D6_Pin 6
  #define LCD_D7_Pin 7

  #define EncoderL_CLK_Pin 22
  #define EncoderL_DT_Pin 2
  #define EncoderL_SW_Pin 23
  #define EncoderR_CLK_Pin 24
  #define EncoderR_DT_Pin 3
  #define EncoderR_SW_Pin 25

  #define SlidePotA_Pin A13
  #define SlidePotB_Pin A11
  #define RotaryPotA_Pin A12

  #define MotDrv_Enable_Pin 8
  #define MotDrv_Step_Pin 9
  #define MotDrv_Direction_Pin 10

  #define AnalogHoriz_Pin A14
  #define AnalogVert_Pin A15
  #define AnalogSW_Pin 26
  #define Nunchuck_Btn1_Pin 29
  #define Nunchuck_Btn2_Pin 27

  #define DIP1_Pin 46
  #define DIP2_Pin 47
  #define DIP3_Pin 48
  #define DIP4_Pin 49
  #define DIP5_Pin 50
  #define DIP6_Pin 51
  #define DIP7_Pin 52
  #define DIP8_Pin 53

  #define RunBtn_Pin 28
  #define RunBtnLED_Pin 31

  #define MotorEnableLED_Pin 30
  #define MotorDirectionLED_Pin 33
  #define RunningLED_Pin 32
  #define FaultLED_Pin 35

  #define Limit1_Pin 18
  #define Limit2_Pin 19
  #define EmergStop_Pin 20

//init
  LiquidCrystal lcd(LCD_RS_Pin, LCD_E_Pin, LCD_D4_Pin, LCD_D5_Pin, LCD_D6_Pin, LCD_D7_Pin);

//Global Vars
  int EncoderL_CLK_Val = 0;
  int EncoderL_DT_Val = 0;
  bool EncoderL_CW = false;
  bool EncoderL_CCW = false;
  bool LeftEncoderBtn_State = false;
  int EncoderR_CLK_Val = 0;
  int EncoderR_DT_Val = 0;
  bool EncoderR_CW = false;
  bool EncoderR_CCW = false;
  bool RightEncoderBtn_State = false;

  int SlidePot_Val = 0;
  int SlidePotB_Val = 0;
  int RotaryPotA_Val = 0;

  bool MotDrvEnable_State = false;
  bool MotDrvStep_State = false;
  bool MotDrvDirection_State = false;

  int AnalogHoriz_Val = 0;
  int AnalogVert_Val = 0;
  bool AnalogSW_State = false;
  bool NunchuckBtn1_State = false;
  bool NunchuckBtn2_State = false;

  bool DIP1_State = false;
  bool DIP2_State = false;
  bool DIP3_State = false;
  bool DIP4_State = false;
  bool DIP5_State = false;
  bool DIP6_State = false;
  bool DIP7_State = false;
  bool DIP8_State = false;

  bool RunBtn_State = false;
  bool RunBtnLED_State = false;

  bool MotorEnableLED_State = false;
  bool MotorDirectionLED_State = false;
  bool RunningLED_State = false;
  bool FaultLED_State = false;

  bool Limit1_State = false;
  bool Limit2_State = false;
  bool EmergStop_State = false;

  int MenuLevel = 0;
  bool UseAutoOff = false;
  int AutoOffTarget = 000;
  bool UsePresets = false;
  int SelectedPreset = 0;
  bool AutoIncPresets = false;
  int AutoIncTarget = 000;
  bool FullManual = true;
  int RunSpeed = 0;
  int FwdAccel = 0;
  int FwdDecel = 0;
  int RevAccel = 0;
  int RevDecel = 0;
  int FwdDwell = 0;
  int RevDwell = 0;

//menu stuff

  //start menu-welcome screen
    LiquidLine welcome_line1(5, 0, "WELCOME TO");
    LiquidLine welcome_line2(8,1, "THE");
    LiquidLine welcome_line3(5, 2, "F-BOMBER!!");
    LiquidScreen welcome_screen(welcome_line1, welcome_line2, welcome_line3);
  //use a-off?
    LiquidLine AutoOff_Line1(3,0, "Use Automatic");
    LiquidLine AutoOff_Line2(2,1, "Off Cycle Count?");
    LiquidLine AutoOff_Line3(8,2,"Yes");
    LiquidLine AutoOff_Line4(8,3,"No");
    LiquidScreen UseAOff_Screen(AutoOff_Line1, AutoOff_Line2, AutoOff_Line3, AutoOff_Line4);
    //UseAutoOff;
    //yes
      //set a off counter
        LiquidLine AutoCount_Line1(3,0,"Set Automatic");
        LiquidLine AutoCount_Line2(2,1,"Off Cycle Count");
        LiquidLine AutoCount_Line3(0,2,"");
        LiquidLine AutoCount_Line4(9,3,AutoOffTarget);
        LiquidScreen AutoCount_Screen(AutoCount_Line1, AutoCount_Line2, AutoCount_Line3, AutoCount_Line4);
        //AutoOffTarget
    //no
      //manual shutoff
  //use pre?
    LiquidLine UsePre_Line1(4,0,"Use Presets?");
    LiquidLine UsePre_Line2(0,1,"");
    LiquidLine UsePre_Line3(0,2,"");
    LiquidLine UsePre_Line4(0,3,"");
    LiquidScreen UsePre_Screen(UsePre_Line1, UsePre_Line2, UsePre_Line3, UsePre_Line4);
    //UsePreset
    //yes
      //inc preset?
        LiquidLine IncPre_Line1(3,0,"Automatically");
        LiquidLine IncPre_Line2(5,1,"Incriment");
        LiquidLine IncPre_Line3(6,2,"Presets?");
        LiquidLine IncPre_Line4(6,3,"");
        LiquidScreen IncPre_Screen(IncPre_Line1, IncPre_Line2, IncPre_Line3, IncPre_Line4);
        //AutoIncPresets
        //yes
          //set incriment count
            LiquidLine IncCount_Line1(3,0,"Set Incriment");
            LiquidLine IncCount_Line2(4,1,"Preset Count");
            LiquidLine IncCount_Line3(0,2,"");
            LiquidLine IncCount_Line4(0,3,"");
            LiquidScreen IncCount_Screen(IncCount_Line1, IncCount_Line2, IncCount_Line3, IncCount_Line4);
            //AutoIncTarget
              //set start preset
              LiquidLine StartPre_Line1(0,0,"Select Start Preset");
              LiquidLine StartPre_Line2(2,1,"");
              LiquidLine StartPre_Line3(0,2,"");
              LiquidLine StartPre_Line4(0,3,"");
              LiquidScreen StartPre_Screen(StartPre_Line1,StartPre_Line2, StartPre_Line4); //add scrolling
              //SelectedPreset
        //no
          //select run preset
          LiquidLine RunPre_Line1(1,0,"Select Run Preset");
          LiquidLine RunPre_Line2(0,1,"");
          LiquidLine RunPre_Line3(0,2,"");
          LiquidLine RunPre_Line4(0,3,"");
          LiquidScreen RunPre_Screen(RunPre_Line1, RunPre_Line2, RunPre_Line3, RunPre_Line4); //add scrolling
          //SelectedPreset
    //no
      //Manual setup
        LiquidLine NoPre_Line1(4,0,"Set Runtime");
        LiquidLine NoPre_Line2(4,1,"Parameters?");
        LiquidLine NoPre_Line3(0,2,"");
        LiquidLine NoPre_Line4(0,3,"");
        LiquidScreen NoPre_Screen(NoPre_Line1, NoPre_Line2, NoPre_Line3, NoPre_Line4);
        //FullManual
    //Setup Run Params?
      //yes
        //3A-Set Run Speed
        LiquidLine RunSpd_Line1(4,0,"Set Overall");
        LiquidLine RunSpd_Line2(3,1,"Running Speed");
        LiquidLine RunSpd_Line3(0,2,"");
        LiquidLine RunSpd_Line4(0,3,"");
        LiquidScreen RunSpd_Screen(RunSpd_Line1, RunSpd_Line2, RunSpd_Line3, RunSpd_Line4);
        //RunSpeed
        //3B-Set Fwd Accel
        //3C-Set Fwd Decel
        LiquidLine FwdAcc_Line1(3,0,"Set Fwd Accel");
        LiquidLine FwdAcc_Line2(0,1,"");
        LiquidLine FwdDec_Line3(3,2,"Set Fwd Decel");
        LiquidLine FwdDec_Line4(0,3,"");
        LiquidScreen FwdDelta_Screen(FwdAcc_Line1, FwdAcc_Line2, FwdDec_Line3, FwdDec_Line4);
        //FwdAccel
        //FwdDecel
        //3E-Set Back Accel
        //3F-Set Bac Decel
        LiquidLine RevAcc_Line1(3,0,"Set Rev Accel");
        LiquidLine RevAcc_Line2(0,1,"");
        LiquidLine RevDec_Line3(3,2,"Set Rev Decel");
        LiquidLine RevDec_Line4(0,3,"");
        LiquidScreen RevDelta_Screen(RevAcc_Line1, RevAcc_Line2, RevDec_Line3, RevDec_Line4);
        //RevAccel;
        //RevDecel;
        //3D-Set Forward Dwell
        //3G-Set Backward Dwell
        LiquidLine FwdDwell_Line1(3,0,"Set Fwd Dwell");
        LiquidLine FwdDwell_Line2(0,1,"");
        LiquidLine RevDwell_Line3(3,2,"Set Rev Dwell");
        LiquidLine RevDwell_Line4(0,3,"");
        LiquidScreen Dwell_Screen(FwdDwell_Line1, FwdDwell_Line2, RevDwell_Line3, RevDwell_Line4);
        //FwdDwell;
        //RevDwell;
      //NO
        //full manual
        LiquidLine Manual_Line1(0,0,"explain full");
        LiquidLine Manual_Line2(0,1,"manual mode");
        LiquidLine Manual_Line3(0,2,"here");
        LiquidLine Manual_Line4(0,3,"");
        LiquidScreen Manual_Screen(Manual_Line1, Manual_Line2, Manual_Line3, Manual_Line4);

  // Run Settings Summarry
    LiquidLine Summary_Line1(0,0,"put runtime");
    LiquidLine Summary_Line2(0,1,"summary");
    LiquidLine Summary_Line3(0,2,"here");
    LiquidLine Summary_Line4(0,3,"");
    LiquidScreen Summary_Screen(Summary_Line1, Summary_Line2, Summary_Line3, Summary_Line4);

  LiquidMenu menu(lcd);

  //menu.next_screen();
	//menu.previous_screen();
	//menu.call_function(1);
	//menu.call_function(2);
	//menu.switch_focus();


//Button Setup
  using Button = AblePullupClickerButton; ///< Using clicker pull-up button.
  using ButtonList = AblePullupClickerButtonList; ///< Using clicker pull-up button list.
  Button Next(EncoderR_SW_Pin);
  Button Back(EncoderL_SW_Pin);
  Button Run(RunBtn_Pin);
  Button Stop(EmergStop_Pin);
  Button NunchuckBtn1(Nunchuck_Btn1_Pin);
  Button NunchuckBtn2(Nunchuck_Btn2_Pin);
  Button AnalogStickBtn(AnalogSW_Pin);




//encoder stuff
  RotaryEncoder EncoderLeft(EncoderL_CLK_Pin, EncoderL_DT_Pin, RotaryEncoder::LatchMode::TWO03);
  RotaryEncoder EncoderRight(EncoderR_CLK_Pin, EncoderR_DT_Pin, RotaryEncoder::LatchMode::TWO03);





void setup() {
  // Initialize
    Serial.begin(9600);
    lcd.begin(20,4);
    //ReadDipSw();

  //PinModes
    pinMode(DIP1_Pin, INPUT_PULLUP);
    pinMode(DIP2_Pin, INPUT_PULLUP);
    pinMode(DIP3_Pin, INPUT_PULLUP);
    pinMode(DIP4_Pin, INPUT_PULLUP);
    pinMode(DIP5_Pin, INPUT_PULLUP);
    pinMode(DIP6_Pin, INPUT_PULLUP);
    pinMode(DIP7_Pin, INPUT_PULLUP);
    pinMode(DIP8_Pin, INPUT_PULLUP);
    pinMode(RunBtnLED_Pin, OUTPUT);
    pinMode(MotorEnableLED_Pin, OUTPUT);
    pinMode(MotorDirectionLED_Pin, OUTPUT);
    pinMode(RunningLED_Pin, OUTPUT);
    pinMode(FaultLED_Pin, OUTPUT);

  //Menu Init
    AutoOff_Line3.set_focusPosition(Position::LEFT);  //yes
    AutoOff_Line3.attach_function(1, Blank);          //yes
    AutoOff_Line4.set_focusPosition(Position::LEFT);  //no
    AutoOff_Line4.attach_function(1, Blank);          //no

    //AutoCount_Line3

    UsePre_Line2.set_focusPosition(Position::LEFT); //yes
    UsePre_Line2.attach_function(1, Blank);         //yes
    UsePre_Line3.set_focusPosition(Position::LEFT); //no
    UsePre_Line3.attach_function(1, Blank);         //no

    IncPre_Line2.set_focusPosition(Position::LEFT);  //yes
    IncPre_Line2.attach_function(1, Blank);          //yes
    IncPre_Line3.set_focusPosition(Position::LEFT);  //no
    IncPre_Line3.attach_function(1, Blank);          //no

    //IncCount_Line3

    NoPre_Line2.set_focusPosition(Position::LEFT);  //yes
    NoPre_Line2.attach_function(1, Blank);          //yes
    NoPre_Line3.set_focusPosition(Position::LEFT);  //no
    NoPre_Line3.attach_function(1, Blank);          //no
    
    //menu.add_screen(welcome_screen);  // This is the method used to add a screen object to the menu.
    menu.add_screen(UseAOff_Screen);
    menu.add_screen(AutoCount_Screen);
    menu.add_screen(UsePre_Screen);
    menu.add_screen(IncPre_Screen);
    menu.add_screen(IncCount_Screen);
    menu.add_screen(StartPre_Screen);
    menu.add_screen(RunPre_Screen);
    menu.add_screen(NoPre_Screen);
    menu.add_screen(RunSpd_Screen);
    menu.add_screen(FwdDelta_Screen);
    menu.add_screen(RevDelta_Screen);
    menu.add_screen(Dwell_Screen);
    menu.add_screen(Manual_Screen);
    menu.add_screen(Summary_Screen);
    menu.update();

  //Debug
    ReadDipSw();

  //button Setup
    Next.begin();
    Back.begin();
    Run.begin();
    Stop.begin();
    NunchuckBtn1.begin();
    NunchuckBtn2.begin();
    AnalogStickBtn.begin();

  //LED Test
    if(DIP8_State == true){
      digitalWrite(RunBtnLED_Pin, HIGH);
      digitalWrite(MotorEnableLED_Pin, HIGH);
      digitalWrite(MotorDirectionLED_Pin, HIGH);
      digitalWrite(RunningLED_Pin, HIGH);
      digitalWrite(FaultLED_Pin, HIGH);
      delay(1000);
      digitalWrite(RunBtnLED_Pin, LOW);
      digitalWrite(MotorEnableLED_Pin, LOW);
      digitalWrite(MotorDirectionLED_Pin, LOW);
      digitalWrite(RunningLED_Pin, LOW);
      digitalWrite(FaultLED_Pin, LOW);
    }

  
  //menu.switch_focus();
  //menu.update();
}  //Close Setup 


void loop() {
  SetupMenu();
  //ReadAllBtns();
 //ReadRightEncoder();
}//close loop


void SetupMenu(){
  //menu.switch_focus();
  //MenuLevel
  //int EncVal = 0;
  //int PrevEncVal = 0;
  switch(MenuLevel){
    case 0:
      Serial.println("MenuLevel 0 Run");
      menu.switch_focus();
      menu.update();
      
      MenuLevel = 1;
      
      break;
    
    case 1:
      //Serial.println("MenuLevel 1 Run");
        ReadAllBtns();
        ReadRightEncoder();
        if(EncoderR_CW == true || EncoderR_CCW == true){
          menu.switch_focus();
          menu.softUpdate();
        }
        if(RightEncoderBtn_State == true){
          MenuLevel = 2;
          menu.next_screen();
        }
      //MenuLevel = 2;
      break;

    case 2:
      Serial.println("MenuLevel 2 Run");
      // //ReadRightEncoder();
      // int OldRtEncVal = ReadRightEncoder();
      // Serial.println(OldRtEncVal);
      // break;
  }





} //Close SetupMenu

void ReadAllBtns() {
  //button maint stuff
    Next.handle();
    Back.handle();
    Run.handle();
    Stop.handle();
    NunchuckBtn1.handle();
    NunchuckBtn2.handle();
    AnalogStickBtn.handle();

  //default all btn vars
    RightEncoderBtn_State = false;
    LeftEncoderBtn_State = false;
    RunBtn_State = false;
    EmergStop_State = false;
    NunchuckBtn1_State = false;
    NunchuckBtn2_State = false;
    AnalogSW_State = false;

  if(Next.resetClicked()){
    Serial.println("Next Button Pressed");
    RightEncoderBtn_State = true;
    //menu.next_screen();
  }
  if(Back.resetClicked()){
    Serial.println("Back Button Pressed");
    LeftEncoderBtn_State = true;
    //menu.previous_screen();
  }
  if(Run.resetClicked()){
    Serial.println("Run Button Pressed");
    //goto run screen in the menu on actual run
    RunBtn_State = true;
  }
  if(Stop.resetClicked()){
    EmergStop_State = true;
    Serial.println("Stop Button Pressed");
    //goto run screen in the menu on actual run
    //RunBtn_State = true;
  }
  if(NunchuckBtn1.resetClicked()){
    Serial.println("Nunchuck Button 1 Pressed");
    NunchuckBtn1_State = true;
    //menu.previous_screen();
  }
  if(NunchuckBtn2.resetClicked()){
    Serial.println("Nunchuck Button 2 Pressed");
    //menu.previous_screen();
    NunchuckBtn2_State = true;
  }
  if(AnalogStickBtn.resetClicked()){
    Serial.println("Analog Stick Button Button Pressed");
    //goto run screen in the menu on actual run
    AnalogSW_State = true;
  }

} //Close ReadAllBtns

void ReadRightEncoder() {
    EncoderR_CW = false;
    EncoderR_CCW = false;
    static int EncoderRightPos = 0;
    EncoderRight.tick();
    int EncoderRightNewPos = EncoderRight.getPosition();
    if (EncoderRightPos > EncoderRightNewPos) {
      EncoderR_CCW = true;
      Serial.print("Right Encoder CCW");
      Serial.print(EncoderRightNewPos);
      Serial.println("  ");
      EncoderRightPos = EncoderRightNewPos;
    } // if
    if (EncoderRightPos < EncoderRightNewPos) {
      EncoderR_CW = true;
      Serial.print("Right Encoder CW ");
      Serial.print(EncoderRightNewPos);
      Serial.println("  ");
      EncoderRightPos = EncoderRightNewPos;
    } // if
    Serial.println(EncoderR_CCW);
} // Close ReadRightEncoder

int ReadLeftEncoder(){
   static int EncoderLeftPos = 0;
    EncoderLeft.tick();
    int EncoderLeftNewPos = EncoderLeft.getPosition();
    if (EncoderLeftPos != EncoderLeftNewPos) {
      Serial.print("Left Encoder = ");
      Serial.print(EncoderLeftNewPos);
      Serial.println("  ");
      EncoderLeftPos = EncoderLeftNewPos;
    } // if
    return EncoderLeftPos;
} // Close ReadLeftEncoder

void ReadDipSw() {  //Read Dip Switch States for Debugging/Setup
  DIP1_State = digitalRead(DIP1_Pin);
  if(DIP1_State == LOW){
    Serial.println("Dip Sw 1 On!");
    DIP1_State = true;
  }
  else{DIP1_State = false;}

  DIP2_State = digitalRead(DIP2_Pin);
  if(DIP2_State == LOW){
    Serial.println("Dip Sw 2 On!");
    DIP2_State = true;
  }
  else{DIP2_State = false;}

  DIP3_State = digitalRead(DIP3_Pin);
  if(DIP3_State == LOW){
    Serial.println("Dip Sw 3 On!");
    DIP3_State = true;
  }
  else{DIP3_State = false;}

  DIP4_State = digitalRead(DIP4_Pin);
  if(DIP4_State == LOW){
    Serial.println("Dip Sw 4 On!");
    DIP4_State = true;
  }
  else{DIP4_State = false;}

  DIP5_State = digitalRead(DIP5_Pin);
  if(DIP5_State == LOW){
    Serial.println("Dip Sw 5 On!");
    DIP5_State = true;
  }
  else{DIP5_State = false;}

  DIP6_State = digitalRead(DIP6_Pin);
  if(DIP6_State == LOW){
    Serial.println("Dip Sw 6 On!");
    DIP6_State = true;
  }
  else{DIP6_State = false;}

  DIP7_State = digitalRead(DIP7_Pin);
  if(DIP7_State == LOW){
    Serial.println("Dip Sw 7 On!");
    DIP7_State = true;
  }
  else{DIP7_State = false;}

  DIP8_State = digitalRead(DIP8_Pin);
  if(DIP8_State == LOW){
    Serial.println("Dip Sw 8 On!");
    DIP8_State = true;
  }
  else{DIP8_State = false;}

} // Close ReadDipSw

void Blank() {
	return;
}