/*


          e-Gizmo PS Controller Sample Code

      This sample code is in the public domain.

      Read an output data from the PS controller and prints the 
      x, y values of the analog joystick and whatever the key is
      pressed.

      by
      e-Gizmo Mechatronix Central
      October 5, 2015
      http://www.e-gizmo.com

*/

byte serial_rxcounter=0;
byte serial_state=0;
boolean serial_available = false;  //true when data packet is available

#define SER_SYNC1 0
#define SER_SYNC2 1
#define SER_FETCHDATA 2

byte  inputData[15];

typedef  struct cont cont;
struct cont{
  boolean  up;
  boolean  down;
  boolean  left;
  boolean  right;
  boolean  triangle;
  boolean  circle;
  boolean  cross;
  boolean  square;
  boolean  left1;
  boolean  left2;
  boolean  right1;
  boolean  right2;
  boolean  start;
  byte  leftx;
  byte  lefty;
  byte  rightx;
  byte  righty;
};

cont  controller;


  

void setup() {
  // Serial 0 is used in this proto. Change if so desired
  Serial.begin(9600);
}


void loop() {

  if (serial_available==true) {
        serial_available = false;
        serial_state=SER_SYNC1;

/*
    // Put your program here

*/

    // Example program
    // This will display the x,y values of the analog joystick
    // and whatever key is pressed
    
    Serial.print("Left JS ");
    Serial.print(controller.leftx);
    Serial.print(",");
    Serial.println(controller.lefty);
    Serial.print("Right JS ");
    Serial.print(controller.rightx);
    Serial.print(",");
    Serial.println(controller.righty);
    if(controller.up==true) Serial.println("UP");
    if(controller.down==true) Serial.println("DOWN");
    if(controller.left==true) Serial.println("LEFT");
    if(controller.right==true) Serial.println("RIGHT");
    if(controller.triangle==true) Serial.println("TRIANGLE");
    if(controller.circle==true) Serial.println("CIRCLE");
    if(controller.square==true) Serial.println("SQUARE");
    if(controller.cross==true) Serial.println("CROSS");
    if(controller.left1==true) Serial.println("LEFT1");
    if(controller.left2==true) Serial.println("LEFT2");
    if(controller.right1==true) Serial.println("RIGHT1");
    if(controller.right2==true) Serial.println("RIGHT2");
    if(controller.start==true) Serial.println("SELECT");
    Serial.println("*******************");
    


  }
}













/*****************************************************************/
/*     Do not modify anything past this line
        Unless you are absolutely sure you know
        what you are doing
*/

/*
  SerialEvent occurs whenever a new data comes in the
 hardware serial RX.  This routine is run between each
 time loop() runs, so using delay inside loop can delay
 response.  Multiple bytes of data may be available.
 */
 

void serialEvent() {
  while (Serial.available()) {

    // get the new byte:
    byte inChar = (byte)Serial.read(); 
      
    switch(serial_state){
      case SER_SYNC1:
        // Look for first sync byte 0x5A
        serial_rxcounter=0;
        if(inChar==0x5A)serial_state=SER_SYNC2;
        break;
      
      case SER_SYNC2:
        // Verify if valid packet by checking second byte = 0xA5
        if(inChar==0xA5) 
          serial_state=SER_FETCHDATA;
          else
          serial_state=SER_SYNC1;
          break;
          
      case  SER_FETCHDATA:
        // Store data packet
        inputData[serial_rxcounter]= inChar;
        serial_rxcounter++;
        // Process when 10 bytes is completed
        if(serial_rxcounter>=10){
          //checksum should match

          if(checksum()==inputData[9]){
            refresh_controller();
            serial_available=true;    // data packet is ready
          }
          else{
            // bad checksum, disregard packet and wait for a new packet
            serial_state=SER_SYNC1;
            serial_available=false;
          }
        }
      
    }
    
  }
}

/* Translate raw gamepad controller data
  into something more meaningful for you
*/

void  refresh_controller(void){
  controller.up=false;
  if((inputData[3]&0x08)==0) controller.up=true;
  controller.down=false;
  if((inputData[3]&0x02)==0) controller.down=true;
  controller.left=false;
  if((inputData[3]&0x01)==0) controller.left=true;
  controller.right=false;
  if((inputData[3]&0x04)==0) controller.right=true;
  controller.triangle=false;
  if((inputData[4]&0x08)==0) controller.triangle=true;
  controller.circle=false;
  if((inputData[4]&0x04)==0) controller.circle=true;
  controller.cross=false;
  if((inputData[4]&0x02)==0) controller.cross=true;
  controller.square=false;
  if((inputData[4]&0x01)==0) controller.square=true;
  controller.left1=false;
  if((inputData[4]&0x20)==0) controller.left1=true;
  controller.left2=false;
  if((inputData[4]&0x80)==0) controller.left2=true;
  controller.right1=false;
  if((inputData[4]&0x10)==0) controller.right1=true;
  controller.right2=false;
  if((inputData[4]&0x40)==0) controller.right2=true;
  controller.start=false;
  if((inputData[3]&0x10)==0) controller.start=true;
  controller.leftx=inputData[7];
  controller.lefty=inputData[8];
  controller.rightx=inputData[5];
  controller.righty=inputData[6];
}

// calculate checksum

byte  checksum(void){
  int  i;
  byte chk= 0x5A+0xA5;
  for(i=0;i<9;i++) chk += inputData[i];
  return(chk);
  
}