import g4p_controls.*;
import processing.serial.*;

class IMU {
  public int accX, accY, accZ;
  public int gyroX, gyroY, gyroZ;
  public int tempRaw;
  public int compAngleX, compAngleY, tempCal;
  public int serialCount = 0;                 // A count of how many bytes we receive
  public boolean firstContact = false;        // Whether we've heard from the microcontroller
}

IMU myIMU1;
IMU myIMU2;

Serial[] myPorts = new Serial[2];

float[][] dataIn = new float[2][10];          // a list to hold data from the serial ports
boolean communicationStarted = false;
float value;

void setup()
{
  //  Take care of the GUI
  size(600, 400);
  createGUI();
  customGUI();

  //  Create IMU objects
  myIMU1 = new IMU();
  myIMU2 = new IMU();

  // initial parameters for 2D/3D animation
  initAnimation();
}

void draw() {
  background(160, 150, 150);
  if (communicationStarted == true) {
    printValues();
  }
  // Every 10th frame update the sketchpad graphic
  if (frameCount % 10 == 0) {
    clearGraphic();
    updateGraphic();
  }
}

void printValues() {
}

void startCommunication() {
  communicationStarted = true;

  textarea_console.appendText("Communication Started");
}

void stopCommunication() {
  communicationStarted = false;

  textarea_console.appendText("Communication Stopped");
}

void findSerials() {
  String SerialList[] = Serial.list();
  String s;

  s = join(SerialList, "\n");
  // Print a list of the serial ports, for debugging purposes:
  printArray(Serial.list());

  textarea_console.appendText("Available Serial Ports:");
  textarea_console.appendText(s);
  textarea_console.appendText(" ");
}

void autoConnect() {
  String portOne = Serial.list()[0];
  String portTwo = Serial.list()[1];
  myPorts[0] = new Serial(this, portOne, 9600);
  myPorts[1] = new Serial(this, portTwo, 9600);

  textarea_console.appendText("Auto-connecting.");
  textarea_console.appendText("IMU #1 Connected.");
  textarea_console.appendText("    Port: " + portOne + "  BaudRate: " + 9600);
  textarea_console.appendText("IMU #2 Connected.");
  textarea_console.appendText("    Port: " + portTwo + "  BaudRate: " + 9600);
  textarea_console.appendText(" ");
}

void connectIMU1() {
  String port = dropList_com1.getSelectedText();
  //int baudRate = dropList_com1.getSelectedText();
  int baudRate = 9600;
  myPorts[0] = new Serial(this, port, baudRate);

  textarea_console.appendText("IMU #1 Connected.");
  textarea_console.appendText("    Port: " + port + "  BaudRate: " + baudRate);
}

void connectIMU2() {
  String port = dropList_com2.getSelectedText();
  //int baudRate = dropList_com1.getSelectedText();
  int baudRate = 9600;
  myPorts[1] = new Serial(this, port, baudRate);

  textarea_console.appendText("IMU #2 Connected.");
  textarea_console.appendText("    Port: " + port + "  BaudRate: " + baudRate);
}

/**
 * When SerialEvent is generated, it'll also give you
 * the port that generated it.  Check that against a list
 * of the ports you know you opened to find out where
 * the data came from
 */

void serialEvent(Serial thisPort) {
  // variable to hold the number of the port:
  int portNumber = -1;

  // iterate over the list of ports opened, and match the
  // one that generated this event:
  for (int p = 0; p < myPorts.length; p++) {
    if (thisPort == myPorts[p]) {
      portNumber = p;
    }
  }

  // read a byte from the port:
  int inByte = thisPort.read();

  if (portNumber == 0) {
    if (myIMU1.firstContact == false) {
      if (inByte == 'A') {
        myPorts[0].clear();          // clear the serial port buffer
        myIMU1.firstContact = true;     // you've had first contact from the microcontroller
        myPorts[0].write('A');       // ask for more
      }
    }
    if (myIMU1.firstContact == true ) {
      String message = myPorts[0].readStringUntil('\n');
      if (message != null) {
        value = float(message);
        println(value);
      }

      // Add the latest byte from the serial port to array:
      dataIn[0][myIMU1.serialCount] = value;
      myIMU1.serialCount++;

      if (myIMU1.serialCount > 9) {
        // Send a capital A to request new sensor readings:
        thisPort.write('A');
        // Reset serialCount:
        myIMU1.serialCount = 0;
      }

      /*
      // Add the latest byte from the serial port to array:
       dataIn[0][myIMU1.serialCount] = inByte;
       myIMU1.serialCount++;

       // If we have 17 bytes:
       if (myIMU1.serialCount > 16) {
       myIMU1.accX        = ((dataIn[0][0] << 8)  | dataIn[0][1]);
       myIMU1.accY        = ((dataIn[0][2] << 8)  | dataIn[0][3]);
       myIMU1.accZ        = ((dataIn[0][4] << 8)  | dataIn[0][5]);
       myIMU1.tempRaw     = ((dataIn[0][6] << 8)  | dataIn[0][7]);
       myIMU1.gyroX       = ((dataIn[0][8] << 8)  | dataIn[0][9]);
       myIMU1.gyroY       = ((dataIn[0][10] << 8) | dataIn[0][11]);
       myIMU1.gyroZ       = ((dataIn[0][12] << 8) | dataIn[0][13]);
       myIMU1.compAngleX  = dataIn[0][14];
       myIMU1.compAngleY  = dataIn[0][15];
       myIMU1.tempCal     = dataIn[0][16];

       // Send a capital A to request new sensor readings:
       thisPort.write('A');
       // Reset serialCount:
       myIMU1.serialCount = 0;
       }
       */
    }
  }

  // tell us who sent what:
  //println("Got " + inByte + " from serial port " + portNumber);
}

/**
 * Use this method to add additional statements
 * to customise the GUI controls
 */

void customGUI() {
  textarea_console.setText   ("##############################");
  textarea_console.appendText("GUI for xMotionPlus");
  textarea_console.appendText("IMU developed by PXL");
  textarea_console.appendText("Version 1.0, May 2014");
  textarea_console.appendText("");
  textarea_console.appendText("Glenn Kerselaers & Devlin Voets");
  textarea_console.appendText("##############################");
  textarea_console.appendText(" ");

  window_graph.setVisible(false);
  window_animation.setVisible(false);
}