/*READ ME:
 This is an example sketch for the PMDG 737 NGX MCP. This sketch controls the Speed, Heading and altitude control of the MCP, as well as the C/O button. I may expand these functionalities in the future to include all buttons, 7 segment displays and encoders on the MCP.

 BEFORE YOU START:
 This sketch only works with arduino Mega. To make it work with UNO, please adjust the pin on the ENCODERS section (just above void setup()), void Setup(), and void ButtonsAP()
 Please make sure that you installed Encoder library from:http://www.pjrc.com/teensy/td_libs_Encoder.html
 YOU MUST HAVE the latest version of FSUIPC INSTALLED!
 YOU MUST START FSX BEFORE YOU START LINK2FS for this to work properly

 SETUP:
 This sketch will only work on Link2fs Experts. It will not function using the basic link2fs.
 Follow these steps to set up Link2fs. Note these offsets only work for PMDG 737 NGX. For use with other airplanes, please adjust offsets accordingly.
 You only need to do this once, provided that you save the profile (step 3)
 1)Go to Expert tab, FSUIPC(1) tab
 Identifier   Reminder          Offsets Type    Hex?          Format    card1
  #g          MCP_IASMach       6524    Single                000.00    V
 then press reset
 2) Go to Expert tab, Controls tab
 Ident  Reminder        Control   Hex?
 Z04    MCP_IAS_SET     81434
 Z05    MCP_HDG_SET     81436
 Z06    MCP_ALT_SET     81437
 Z07    MCP_MACH_SET    81435
 3)Press 'Save all settings for all cards and startup' box. (should be on the right, under the saved profile and profile in use)

 After this, upload the sketch to your arduino.

 WIRING:
 Encoders are wired as follows:
 IAS/Mach is wired to pin 2 and 4
 Heading is wired to pin 3 and 6
 Altitude is wired to pin 18 and 17

 Buttons are wired as follows:
 C/O is wired to pin 52

 NOTE FOR ENCODERS:
 if you want to rewire your encoder, please keep this in mind:
 1) Best Performance: Both signals connect to interrupt pins.
 2) Good Performance: First signal connects to an interrupt pin, second to a non-interrupt pin.
 3) Low Performance: Both signals connect to non-interrupt pins
 Interrupt pins are on the following pins:
 Arduino Mega: 2, 3, 18, 19, 20, 21
 Arduino Uno: 2,3
 Arduino Leonardo: 0,1,2,3


 Final note:
 all improvement and constructive input to improve this sketch is highly welcomed. In no way is this sketch the most effective in doing its job, so I'm looking at ways to improve it. Please go to link2fs forum and PM me (barrizqi) :)
*/


#include <Encoder.h> //include encoder library. Download from:http://www.pjrc.com/teensy/td_libs_Encoder.html
//void serialCheck()
int CodeIn;
  //IAS or Mach?
  String MCPIN_IASMachStr;
  float MCPIN_IASMach, MCPIN_IASMachOld;

//void EncoderAP()
int MCP_IAS = 100, MCP_IASOld = 0, MCP_Mach=0.50, MCP_MachOld=0.49, MCP_IASEncOld, MCP_IASEnc, MCP_IASEncDiff;  //IAS and Mach
int MCP_Heading = 0, MCP_HeadingOld=359, MCP_HeadEncOld, MCP_HeadEnc, MCP_HeadEncDiff;  //Heading
int MCP_Altitude = 100, MCP_AltitudeOld=0, MCP_AltEncOld, MCP_AltEnc, MCP_AltEncDiff; //Altitude
long MCP_AltLong;
long encPreviousMillis = 0; //for Output delay


//void ButtonsAP()
int MCP_COBtn=0, MCP_COBtnOld;

//ENCODERS
Encoder IASEnc(2,4); //IAS/Mach
Encoder HeadingEnc(3,6);  //Heading
Encoder AltitudeEnc(18,17); //Altitude

void setup() {
  Serial.begin(115200);

  //Input buttons
  pinMode(52,INPUT); //C/O
}

void loop() {
  serialCheck(); // gather information from link2fs
  EncoderAP();  //Codes for Encode: IAS/Mach, Heading, Altitude
  ButtonsAP();  //Codes for Buttons: C/O
}

char getChar()// Get a character from the serial buffer, Do not change!
{
  while(Serial.available() == 0);
  return((char)Serial.read());
}

void serialCheck(){
  if (Serial.available()) {// Check if serial data has arrived from PC
    CodeIn = getChar();
    if (CodeIn == '#') {Hashtag();} //the first identifier is "#"
/*  if (CodeIn == '=') {EQUALS();} // The first identifier is "="
    if (CodeIn == '<') {LESSTHAN();}// The first identifier is "<"
    if (CodeIn == '?') {QUESTION();}// The first identifier is "?"
    if (CodeIn == '/') {SLASH();}// The first identifier is "/" (Annunciators) */
  }
}

void Hashtag(){
  CodeIn = getChar();
  switch(CodeIn) {
  //IAS or Mach
    case 'g':
    MCPIN_IASMachStr = " ";
    MCPIN_IASMachStr += getChar();
    MCPIN_IASMachStr += getChar();
    MCPIN_IASMachStr += getChar();
    MCPIN_IASMachStr += getChar();
    MCPIN_IASMachStr += getChar();
    MCPIN_IASMachStr += getChar();  //string should be in form 000.00
    MCPIN_IASMach = MCPIN_IASMachStr.toFloat();
    //now check if there is a change in IAS or Mach mode. if there is a change, synchronise new value directly from link2fs
    if (MCPIN_IASMach <=99 && MCPIN_IASMachOld >=99){ //change from IAS to mach
      MCP_Mach = (MCPIN_IASMach*100);}
    if (MCPIN_IASMach >=99 && MCPIN_IASMachOld <=99){ //change from mach to IAS
      MCP_IAS = MCPIN_IASMach;}
    MCPIN_IASMachOld = MCPIN_IASMach;
  break;
  }
}



void EncoderAP(){
  //IAS/Mach control
  MCP_IASEnc = IASEnc.read(); //read encoder
  MCP_IASEncDiff = (MCP_IASEnc - MCP_IASEncOld); //is the encoder being rotated?
  //Check if in IAS or Mach mode
  if (MCPIN_IASMach >=099.00){  //is in IAS mode?
  if (MCP_IASEncOld != MCP_IASEnc){ //is the encoder being rotated?
    if (MCP_IASEncDiff ==-1){ //rotated to the right
      (MCP_IAS=MCP_IAS+1);} //increase Value
    if (MCP_IASEncDiff == 1){ //rotated to the left
      (MCP_IAS=MCP_IAS-1);} //decrease value
    if (MCP_IAS < 100){ //set lower limit, in this case 100 for IAS in PMDG
      (MCP_IAS = 100);}
    if (MCP_IAS > 340){ //set upper limit, in this case 340 for IAS in PMDG
      (MCP_IAS = 340);}
  MCP_IASEncOld = MCP_IASEnc;} // store old encoder value
  }
  if (MCPIN_IASMach <=099.00) {  //if not in IAS, then must be Mach
  if (MCP_IASEncOld != MCP_IASEnc){
    if (MCP_IASEncDiff ==-1){
      (MCP_Mach=MCP_Mach+1);}
    if (MCP_IASEncDiff == 1){
      (MCP_Mach=MCP_Mach-1);}
    if (MCP_Mach < 60){
      (MCP_Mach = 60);}
    if (MCP_Mach > 82){
      (MCP_Mach = 82);}
  MCP_IASEncOld = MCP_IASEnc;}
  }

  //Heading control
  MCP_HeadEnc = HeadingEnc.read();
  MCP_HeadEncDiff = (MCP_HeadEnc - MCP_HeadEncOld);
  if (MCP_HeadEncOld != MCP_HeadEnc){
    if (MCP_HeadEncDiff ==-1){
      (MCP_Heading=MCP_Heading+1);}
    if (MCP_HeadEncDiff == 1){
      (MCP_Heading=MCP_Heading-1);}
    if (MCP_Heading > 359){
      (MCP_Heading = 000);}
    if (MCP_Heading < 0){
      (MCP_Heading = 359);}
  MCP_HeadEncOld = MCP_HeadEnc;
  }

  //Altitude control
  MCP_AltEnc = AltitudeEnc.read();
  MCP_AltEncDiff = (MCP_AltEnc - MCP_AltEncOld);
  if (MCP_AltEncOld != MCP_AltEnc){
    if (MCP_AltEncDiff ==-1){
      (MCP_Altitude=MCP_Altitude+1);}
    if (MCP_AltEncDiff == 1){
      (MCP_Altitude=MCP_Altitude-1);}
    if (MCP_Altitude > 500){
      (MCP_Altitude = 500);}
    if (MCP_Altitude < 0){
      (MCP_Altitude = 00000);}
  MCP_AltLong = MCP_Altitude; //convert to long, because int only ranges from -32,768 to 32,767
  MCP_AltEncOld = MCP_AltEnc;
  }



  //now for the output data to lin2fs
  unsigned long encCurrentMillis = millis();    // get processor running time in milli seconds
  if(encCurrentMillis - encPreviousMillis > 50) {  // check 50miliseconds has passed sinced last check(fiddle the 50 to suit)
  encPreviousMillis = encCurrentMillis;
    //IAS/Mach Output
    //if in IAS mode
    if  (MCP_IASOld != MCP_IAS){  //is there any change in IAS from last value?
      Serial.print("Z04");
      Serial.println(MCP_IAS);
    MCP_IASOld = MCP_IAS; //store old IAS value
    }
    //if in Mach mode
    if  (MCP_MachOld != MCP_Mach){
      Serial.print("Z07");
      Serial.println(MCP_Mach);
    MCP_MachOld = MCP_Mach;
    }

    //Heading Output
    if  (MCP_HeadingOld != MCP_Heading){
      Serial.print("Z05");
      Serial.println(MCP_Heading);
    MCP_HeadingOld = MCP_Heading;
    }
    //Altitude Output
    if  (MCP_AltitudeOld != MCP_Altitude){
      Serial.print("Z06");
      Serial.println(MCP_AltLong*100);
    MCP_AltitudeOld = MCP_Altitude;
    }
  }
}

void ButtonsAP(){
  //C/O button - IAS or mach button
  MCP_COBtn = digitalRead(52);
  if ((MCP_COBtn != MCP_COBtnOld) && (MCP_COBtnOld !=1)){
  if (MCPIN_IASMach >=099.00){  //is changing from IAS to Mach
    Serial.println("Z031");}
  if (MCPIN_IASMach <=099.00){  //is changing from Mach to IAS
    Serial.println("Z030");}
  }
  MCP_COBtnOld = MCP_COBtn;
}