#include <LedControl.h>
#include <Quadrature.h>
#include "math.h"



Quadrature quad1(22, 23);
Quadrature quad2(25, 26);
                               // ******   LedControl Library is required to drive the MAX7219 7 segment LED displays.  *******
                                                    // led_Display_1 is the variable name of my set of displays chained
LedControl led_Display_1 = LedControl(12,10,11,8);  // together running off of pins 12,11,10.
                                                    // Pin 12 is the DataOut
                                                    // Pin 11 is Load or CS
                                                    // Pin 10 is clock.
                                                    // The 8 is for how many displays you have to chain.
                                                    // I have 4 but put 8 incase of expansion. It cost no memory.
                                                    // You can run any pins you want in the instruction,
                                                    // just plug the module in correctly.
                                                    // LedControl(Data,Clock,Load,Displays)
                                                    // I used 12,10,11 so I could have a straight ribbon connection
                                                    // from the module without crossing leads.


int CodeIn;      // used on all serial reads from Jim's code
String Digit;    // Variable as a string to take from getChar()
int Count;       // This variable used in loops in the EQUALS() function
int Com1Active[5]; // AP_alt_set[5] is an array of the 5 digits that are the Auto Pilot AltitudeSet
int Com1Stby[5]; // AP_alt_set[5] is an array of the 5 digits that are the Auto Pilot AltitudeSet
int Nav1Active[5];
int Nav1Stby[5];
long Xold1;// the first encoder "old" reading
long Xold2;// the second encoder "old" reading
int KpinNo;
int Koutpin;

String KoldpinStateSTR, KpinStateSTR, Kstringnewstate,Kstringoldstate;


//**********************************************************
//*************( START of void setup() )********************

void setup()
{
  Kstringoldstate = "111111111111111111111111111111111111111111111111111111111111111111111";

  for (int KoutPin = 22; KoutPin <=70; KoutPin++)// Get all the pins ready for simconnect codes and "Keys"(all inputs)
  {
    pinMode(KoutPin, INPUT);
    digitalWrite(KoutPin, HIGH);

    //The MAX72XX is in power-saving mode on startup, we have to do a wakeup call
    delay (500);
    led_Display_1.shutdown(0,false);      // I have 4 displays, these start them up
    delay (500);                          // I put the delay in so they all dont start drawing
    led_Display_1.shutdown(1,false);      // current at the same time.
    delay (500);                          // I had an issue with running on only USB power and getting a display glitch.
    led_Display_1.shutdown(2,false);      // The delay seems to have fixed this issue.
   // delay (500);
   // led_Display_1.shutdown(3,false);

    // Set the brightness to a lower than medium values
    led_Display_1.setIntensity(0,8);
    led_Display_1.setIntensity(1,8);
    led_Display_1.setIntensity(2,8);
    //led_Display_1.setIntensity(3,8);

    // and clear the display
    led_Display_1.clearDisplay(0);
    led_Display_1.clearDisplay(1);
    led_Display_1.clearDisplay(2);
    //led_Display_1.clearDisplay(3);

    Serial.begin(115200);
  }
}

void loop() {

   {INPUTS();} //Check the Simconnect and "keys" section for any input pins

   long X1 =(quad1.position())/2;  // for full cycle encoders
  if (X1 != Xold1) {
    if (digitalRead(24) != 0) {
      if (X1 > Xold1) Serial.println("A02"); else Serial.println("A01");} //Nav1 MHz
    else {
     if (X1 > Xold1) Serial.println("A04"); else Serial.println("A03");}  //Nav1 KHz
     // end of "check for HOLD-DOWN"
    Xold1 = X1; // overwrites the old reading with the new one.
  }//end of quad1 read

    long X2 =(quad2.position())/2;  // for full cycle encoders
  if (X2 != Xold2) {
    if (digitalRead(27) != 0) {
      if (X2 > Xold2) Serial.println("A14"); else Serial.println("A13");}  //Comm1 MHz
    else {
     if (X2 > Xold2) Serial.println("A16"); else Serial.println("A15");}  //Comm1 Khz
     // end of "check for HOLD-DOWN"
    Xold2 = X2; // overwrites the old reading with the new one.
  }//end of quad2 read

    if (Serial.available())
    {
        CodeIn = getChar();
        if (CodeIn == '=')
        {
            EQUALS();              // The first identifier is "="
        }
  }
}
char getChar()// Get a character from the serial buffer
{
  while(Serial.available() == 0);   // wait for data
  return((char)Serial.read());      // Thanks Doug
}


void EQUALS()               // The first identifier was "="
{
    CodeIn = getChar();     // Get another character
    switch(CodeIn)          // Now lets find what to do with it
    {
        case 'A':           //  Comm1 Active
            Count = 0;               // clear the Count
            while (Count < 5)        // we will count to 5 from 0 to 4
            {
                Digit = "";          // clears the string variable Digit
                Digit += getChar();  // Makes the string Digit what ever getChar() is holding
                  if (Digit ==".")          // ******   This looks for the "." *************
            {
        Digit = "";           // If we find the "." we clear it and get the next digit
        Digit += getChar();  //  Because we know where the "." always goes.
            }
                Com1Active[Count] = Digit.toInt(); // Turns the string Digit into an Integer and
                                                   // then stores it in the Array AP_alt_set[] 0,1,2,3,4
                Count++;
            }

            led_Display_1.setDigit(0,7,Com1Active[0],false);   // First digit of Alt Set is displayed
            led_Display_1.setDigit(0,6,Com1Active[1],false);   // Second digit of Alt Set is displayed
            led_Display_1.setDigit(0,5,Com1Active[2],true);   // Third digit of Alt Set is displayed
            led_Display_1.setDigit(0,4,Com1Active[3],false);   // Fourth digit of Alt Set is displayed
            led_Display_1.setDigit(0,3,Com1Active[4],false);   // Fifth digit of Alt Set is displayed
                                                               // false denotes NO decimal point
        break;


         case 'B':           //  Comm1 Stby
            Count = 0;               // clear the Count
            while (Count < 5)        // we will count to 5 from 0 to 4
            {
                Digit = "";          // clears the string variable Digit
                Digit += getChar();  // Makes the string Digit what ever getChar() is holding
                  if (Digit ==".")          // ******   This looks for the "." *************
            {
        Digit = "";           // If we find the "." we clear it and get the next digit
        Digit += getChar();  //  Because we know where the "." always goes.
            }
                Com1Stby[Count] = Digit.toInt(); // Turns the string Digit into an Integer and
                                                   // then stores it in the Array AP_alt_set[] 0,1,2,3,4
                Count++;
            }

            led_Display_1.setDigit(0,1,Com1Stby[0],false);   // First digit of Alt Set is displayed
            led_Display_1.setDigit(0,0,Com1Stby[1],false);   // Second digit of Alt Set is displayed
            led_Display_1.setDigit(1,7,Com1Stby[2],true);   // Third digit of Alt Set is displayed
            led_Display_1.setDigit(1,6,Com1Stby[3],false);   // Fourth digit of Alt Set is displayed
            led_Display_1.setDigit(1,5,Com1Stby[4],false);   // Fifth digit of Alt Set is displayed
                                                               // false denotes NO decimal point
    break;


      case 'E':           //  Nav1Active
            Count = 0;               // clear the Count
            while (Count < 5)        // we will count to 5 from 0 to 4
            {
                Digit = "";          // clears the string variable Digit
                Digit += getChar();  // Makes the string Digit what ever getChar() is holding
                  if (Digit ==".")          // ******   This looks for the "." *************
            {
        Digit = "";           // If we find the "." we clear it and get the next digit
        Digit += getChar();  //  Because we know where the "." always goes.
            }
                Nav1Active[Count] = Digit.toInt(); // Turns the string Digit into an Integer and
                                                   // then stores it in the Array AP_alt_set[] 0,1,2,3,4
                Count++;
            }

            led_Display_1.setDigit(1,2,Nav1Active[0],false);   // First digit of Alt Set is displayed
            led_Display_1.setDigit(1,1,Nav1Active[1],false);   // Second digit of Alt Set is displayed
            led_Display_1.setDigit(1,0,Nav1Active[2],true);   // Third digit of Alt Set is displayed
            led_Display_1.setDigit(2,7,Nav1Active[3],false);   // Fourth digit of Alt Set is displayed
            led_Display_1.setDigit(2,6,Nav1Active[4],false);   // Fifth digit of Alt Set is displayed
                                                               // false denotes NO decimal point

   break;
      case 'F':           //  Nav1Stby
            Count = 0;               // clear the Count
            while (Count < 5)        // we will count to 5 from 0 to 4
            {
                Digit = "";          // clears the string variable Digit
                Digit += getChar();  // Makes the string Digit what ever getChar() is holding
                  if (Digit ==".")          // ******   This looks for the "." *************
            {
        Digit = "";           // If we find the "." we clear it and get the next digit
        Digit += getChar();  //  Because we know where the "." always goes.
            }
                Nav1Stby[Count] = Digit.toInt(); // Turns the string Digit into an Integer and
                                                   // then stores it in the Array AP_alt_set[] 0,1,2,3,4
                Count++;
            }

            led_Display_1.setDigit(2,4,Nav1Stby[0],false);   // First digit of Alt Set is displayed
            led_Display_1.setDigit(2,3,Nav1Stby[1],false);   // Second digit of Alt Set is displayed
            led_Display_1.setDigit(2,2,Nav1Stby[2],true);   // Third digit of Alt Set is displayed
            led_Display_1.setDigit(2,1,Nav1Stby[3],false);   // Fourth digit of Alt Set is displayed
            led_Display_1.setDigit(2,0,Nav1Stby[4],false);   // Fifth digit of Alt Set is displayed
                                                               // false denotes NO decimal point
    }
}


  void INPUTS(){ // Simconnect codes and "Keys" section
  Kstringnewstate = "";
  for (int KpinNo = 22; KpinNo <=70; KpinNo++){ //set this to the input pins. (pins 10 to 19)
    KpinStateSTR = String(digitalRead(KpinNo)); //read the state of the pin
    KoldpinStateSTR = String(Kstringoldstate.charAt(KpinNo - 22));// gets the 'old' state of the pin from string
    if (KpinStateSTR != KoldpinStateSTR){// checks if it's different to the last reading of that pinNo
      if (KpinNo != 13){ // avoid using pin 13 as an input unless you know the tricks.
        if (KpinNo == 28 && KpinStateSTR == "0"){Serial.println ("A06");} //sets gear handle up
        if (KpinNo == 29 && KpinStateSTR == "0"){Serial.println ("A18");} //sets gear handle up

        //  Add more here but remember to also change the figure in the next line down. (the 14)
      }//end of 'its not pin 13'
    }//end of 'its different'
    Kstringnewstate += KpinStateSTR;
  }//end of 'for' loop (read the pins)
  Kstringoldstate = Kstringnewstate;
}//end of INPUTS void