String KSFSVersion = "KSFS.1";
bool commandComplete = false;
String command = "";
String commandID = "";
String commandData = "";

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

  // Navigation
  pinMode(53,INPUT_PULLUP); // Maneuver
  pinMode(52,INPUT_PULLUP); // Kill Rotation
  pinMode(51,INPUT_PULLUP); // Retrograde
  pinMode(50,INPUT_PULLUP); // Prograde
  pinMode(49,INPUT_PULLUP); // Anti-normal
  pinMode(48,INPUT_PULLUP); // Normal
  pinMode(47,INPUT_PULLUP); // Radial Out
  pinMode(46,INPUT_PULLUP); // Radial In
  pinMode(45,INPUT_PULLUP); // Anti-target
  pinMode(44,INPUT_PULLUP); // Target

  // Action Groups
  pinMode(31,INPUT_PULLUP); // AG01
  pinMode(29,INPUT_PULLUP); // AG02
  pinMode(28,INPUT_PULLUP); // AG03
  pinMode(27,INPUT_PULLUP); // AG04

  // Utilities
  pinMode(43,INPUT_PULLUP); // Brakes
  pinMode(42,INPUT_PULLUP); // Gear
  pinMode(41,INPUT_PULLUP); // Lights

  // SAS/RCS
  pinMode(40,INPUT_PULLUP); // SAS
  pinMode(39,INPUT_PULLUP); // RCS
  pinMode(38,OUTPUT); // SAS LED
  pinMode(37,OUTPUT); // SAS LED

  // Stage
  pinMode(36,INPUT_PULLUP); // Stage
  pinMode(35,OUTPUT); // Stage LED
  pinMode(34,INPUT_PULLUP); // Stage Lock

  // Abort
  pinMode(33,INPUT_PULLUP); // Abort
  pinMode(32,OUTPUT); // Abort LED
  pinMode(30,INPUT_PULLUP); // Abort Lock

  // Translation
  pinMode(26,INPUT_PULLUP); // Translation Down
  pinMode(25,INPUT_PULLUP); // Translation Up
  pinMode(24,INPUT_PULLUP); // Translation Back
  pinMode(23,INPUT_PULLUP); // Translation Forward
  pinMode(22,INPUT_PULLUP); // Translation Right
  pinMode(21,INPUT_PULLUP); // Translation Left

  // Throttle
  pinMode(20,INPUT_PULLUP); // Throttle Full/Max
  pinMode(19,INPUT_PULLUP); // Throttle Cut

  //Data
  pinMode(18,INPUT_PULLUP); // Connect
  pinMode(17,INPUT_PULLUP); // Connect LED
}

bool CONN = false;

void internals()
{
    if(CONN){
      digitalWrite(38,HIGH);
    }else{
      digitalWrite(38,LOW);
    }
    //if(digitalRead(40) == 0){digitalWrite(38,HIGH);}else{digitalWrite(38,LOW);} // SAS LED
    if(digitalRead(39) == 0){digitalWrite(37,HIGH);}else{digitalWrite(37,LOW);} // RCS LED
    if(digitalRead(34) == 0){digitalWrite(35,HIGH);}else{digitalWrite(35,LOW);} // Stage Lock LED
    if(digitalRead(30) == 0){digitalWrite(32,HIGH);}else{digitalWrite(32,LOW);} // Abort Lock LED
}

void serialOut()
{
  Serial.print("MANU"+String(digitalRead(53))+";");
  Serial.print("KLRT"+String(digitalRead(52))+";");
  Serial.print("RGRD"+String(digitalRead(51))+";");
  Serial.print("PGRD"+String(digitalRead(50))+";");
  Serial.print("ANRM"+String(digitalRead(49))+";");
  Serial.print("NORM"+String(digitalRead(48))+";");
  Serial.print("ROUT"+String(digitalRead(47))+";");
  Serial.print("RAIN"+String(digitalRead(46))+";");
  Serial.print("ATRG"+String(digitalRead(45))+";");
  Serial.print("TRGT"+String(digitalRead(44))+";");

  Serial.print("AG01"+String(digitalRead(31))+";");
  Serial.print("AG02"+String(digitalRead(29))+";");
  Serial.print("AG03"+String(digitalRead(28))+";");
  Serial.print("AG04"+String(digitalRead(27))+";");

  Serial.print("BRKS"+String(!digitalRead(43))+";");
  Serial.print("GEAR"+String(!digitalRead(42))+";");
  Serial.print("LITE"+String(!digitalRead(41))+";");

  Serial.print("SASS"+String(!digitalRead(40))+";");
  Serial.print("RCSS"+String(!digitalRead(39))+";");

  if(digitalRead(34) == 0){Serial.print("STAG"+String(digitalRead(36))+";");}else{Serial.print("STAG0;");}
  if(digitalRead(30) == 0){Serial.print("ABRT"+String(digitalRead(33))+";");}else{Serial.print("ABRT0;");}

  Serial.print("TDWN"+String(digitalRead(26))+";");
  Serial.print("TUPP"+String(digitalRead(25))+";");
  Serial.print("TBCK"+String(digitalRead(24))+";");
  Serial.print("TFWD"+String(digitalRead(23))+";");
  Serial.print("TRIT"+String(digitalRead(22))+";");
  Serial.print("TLFT"+String(digitalRead(21))+";");

  Serial.print("THFU"+String(digitalRead(20))+";");
  Serial.print("THCT"+String(digitalRead(19))+";");

  Serial.print("CONN"+String(digitalRead(18))+";");

  internals();
}

void loop()
{

  if(commandComplete){

    commandComplete = false;

    if(command.length() > 0)
    {
      if(command.equals("C1NN"))
      {
        CONN = true;
      }
      else if(command.equals("C0NN"))
      {
        CONN = false;
      }
    }

    command = "";

  }

  Serial.print(KSFSVersion + ";");
  serialOut();

}

void serialEvent()
{
  while(Serial.available())
  {
    char data = (char)Serial.read();
    if(data == ';')
    {
      commandComplete = true;
    }
    else if(data != '?')
    {
      command += data;
    }
  }
}