// HOW TO USE:
//   1. Copy bluetooth drivers and set your clock to 2006 to work with the license
//   2. Set the framerate
//   3. Press play, then press both the 1+2 buttson on the wiimote at the same time.  wait 10 seconds
//      for it to connect.  if it doesnt, redo this step.
 
// These are the Bluetooth drivers:
// the libraries should sit in your processing library directory
// IMPORTANT, set the date of your computer to 2006.  A licensing issue.
import lll.wrj4P5.*;
import lll.Loc.*;
import processing.serial.*;

// the serial port:
Serial port;
// start mark
boolean debut = false;
// variable to hold input values:
int thisByte = -1;
float X, Y, Z, valX, valY, valZ = 0;                   // 3 Coordinates of the 3 axis accelero
int[] serialInArray = new int[28];   // Where we'll put what we receive
int serialCount = 0;                 // A count of how many bytes we receive

Loc acc;
Wrj4P5 wii;
// This is the framerate.  24 is normal but it makes
// connecting the wiimote more dificult.  So for testing
// and debugging use a lower framerate.
int FRAMERATE = 24;
 
// just some variables we use
Accelometer wiicube;
int screenwidth;
int screenheight;
int screenmidx;
int screenmidy;

PrintWriter logfile;
 
// setup our enviornment:
void setup() {
  logfile = createWriter( nf(year(),4) + nf(month(),2) + nf(day(),2) + nf(hour(),2) + nf(minute(),2) + nf(second(),2) + "_serial_accelerometer.log"); 
  
  size(800,800,P3D);       // setup the screen size.  I use the P3D engine for
                           // certain 3d features.  Lets me rotate junk more easily.
  // align junk:
  screenwidth = width;
  screenheight = height;
  screenmidx = width/2;
  screenmidy = height/2;
  // here we set the framerate:
  frameRate(FRAMERATE);
//  // Connect to the wiiremote:
//  wii=new Wrj4P5(this);
//  wii.connect();
 
  // this comes from the accelometer class (see added file).  Set to "ball" to see a ball instad of "cube"
  wiicube = new Accelometer("ball");

  // list all the available serial ports:
  println(Serial.list());
                                                 
  String device = Serial.list()[0];              
  port = new Serial(this, device, 9600);        
  println("Connecting to port: " + device);  
}
 
// Now do all of the following on every frame redraw (FRAMERATE/60 seconds)
float accx, accy,accz;
void draw() {
  //if (wii.isConnecting()) return; // some wait code
  background(200);         // background color
  stroke(255,50);          // default color for drawn lines
 
  while (port.available() > 0) {
    thisByte = port.read();
    processByte(char(thisByte));  // Process the input buffer (see below)
  }   
  
    accx = atan2(Y,Z); // Convert G to angular accel
    accy = atan2(X,Z); // Convert G to angular accel
    accz = Z;

//    Loc acc = new Loc(accx*100, accy*100, accz*100);
    Loc acc = new Loc(valX*100, valY*100, valZ*100);  // Print the bar graph in Gs  

    
  // A "Matrix" is geometric area defined inside of the 3d space.
  // Everything inside of pushMatrix() ... popMatrix() can be rotated and edited in
  // a global manner but in a way that wont effect the items outside of this Matrix
  pushMatrix();
  translate(screenmidx, screenmidy, 0);  // move the object to the center of the screen
  wiicube.setXYZ(accx,accy,accz);        // see the accelometer class.  this does the work of rotating
                                         // according to the current xyz from the accelerometers
  wiicube.draw();
  popMatrix();
 
  // here I draw the little colored lines that give you a bar graph of the xyz
  pushMatrix();
  translate(screenmidx, screenmidy, 0);
  wiicube.drawAccDebug(acc);  
  popMatrix();
 
}
class Accelometer
{
  PFont font = loadFont("Arial-Black-14.vlw");
  PFont font22 = loadFont("ArialMT-22.vlw");
 
  String shape;
  int maxsize;
  int xsize;
  int ysize;
  int zsize;
  color fillcolor = color(0,0,0,102);
  String debugMsgLn1;
  String debugMsgLn2;
   
  float averageroll; // used to normalize movements
  float averagey;
 
   
  Accelometer()
  {     
  }
 
  // the initialization routine for the class.  see other file: "wiicube = new Accelometer("cube");"
  Accelometer(String s)
  {
    shape = s;
    if (height < width)
      maxsize = height;
    else
      maxsize = width;
  }
 
  // this gets called every FRAMERATE/60 seconds
  void draw()
  {
    if (shape == "cube")
      cube();
    else if (shape == "ball")
      ball();
  }
 
  void cube() {
      zsize = maxsize/2;
      xsize = maxsize/6;
      ysize = maxsize/6;
 
    fill(fillcolor);
    box(xsize, ysize, zsize);   
    textFont(font22);
    fill(0,0,0,102);
    text("top",-10,-(ysize/2)-8,-zsize/2);
 
  }
   
  void ball() {
    fill(fillcolor);
    textFont(font22);
    fill(0,0,0,102);
    text("top",-10,-(maxsize/PI)-8,0);
    sphere(maxsize/(PI)); 
  }
   
 
  void setXYZ(float x, float y, float z) {
    //radians measurements:  x, y and z are passed in radians
    float radius = sqrt(x*x + y*y + z*z);
    float roll = atan2(x,z);
    float pitch = acos(y/radius);
    //degrees measurements:
    float Xdeg = degrees(x); 
    float Ydeg = degrees(y); 
    float Zdeg = degrees(z);
    float Radius = degrees(radius);
    float Rolldeg = degrees(atan2(x,z));      // / PI * 180.0;
    float Pitchdeg = degrees(acos(y/radius)); // / PI * 180.0;
 
    textFont(font22);
    fill(0,0,0,200);
    debugMsgLn1 = "x: "+nf(x,1,2) + " y: "+ nf(y,1,2) + " z: " + nf(z,1,2);
    debugMsgLn2 = "radius: "+nf(radius,1,2)+" roll: "+nf(roll,1,2)+" pitch: "+nf(pitch,1,2);
    println(debugMsgLn1+debugMsgLn2);
    text(debugMsgLn1,-width/2+10,-height/2+30,0);
    text(debugMsgLn2,-width/2+10,-height/2+60,0);
    text("<-- radians",-width/2+300,-height/2+30,0);
    // IN DEGREES:
    debugMsgLn1 = "X: "+int(Xdeg) + " Y: "+ int(Ydeg) + " Z: " + int(Zdeg);
    debugMsgLn2 = "Radius: "+int(Radius)+" Roll: "+int(Rolldeg)+" Pitch: "+int(Pitchdeg);
    println(debugMsgLn1+debugMsgLn2);
    text(debugMsgLn1,-width/2+10,-height/2+90,0);
    text(debugMsgLn2,-width/2+10,-height/2+120,0);
    text("<-- degrees",-width/2+300,-height/2+120,0);
     
 
    // THIS DOES THE WORK:
    //rotateZ(z);
    averageroll=(averageroll+roll)/2; // smooth out the movements a little
    averagey=(averagey+y)/2;
    rotateZ(averageroll);
    rotateX(averagey);
     
     
  }   
 

  // draw the graph bars showing x,y,z
  void drawAccDebug (Loc acc) {
    textFont(font);
    fill(0,0,0,102);
 
    stroke(255,204,204);  // pink = z
    line(4,0, 4, -acc.z);
    text(acc.z/100, 0, -acc.z,0);
    stroke(255,0,0);     // red = x
    line(0,0, acc.x, -acc.z);
    text(acc.x/100, acc.x-30, -acc.z);
    stroke(51,102,51);   // green = y
    line(0,0, 0, acc.y);
    text(acc.y/100, 15, acc.y);
  }

   
   
}


void processByte(char inByte) {
  
  // 
  // Well, it is a bit of a balagan to explain what i do here but it kinda works ;)
  // Basically, the WiTilt ASCII mode sends series of 28 decimals that represents the data :
  //
  // 88 61 45 48 46 51 56 49 32 89 61 32 49 46 48 52 50 32 90 61 45 48 46 56 48 52 10 13 
  //  X  =  -  0  .  3  8  1 SP  Y =  SP  1  .  0  4  2 SP Z   =  -  0  .  8  0 4  \n \r
  //
  // SP : space
  // \n : Line feed
  // \r  : Carriage return
  //
  // Every serie is separated by a \r char meaning 13 in decimal. I localize it , then do an array of the 28
  // next values and then parse 3 substring with my X, Y, and Z values. Then i turn this string into an number
  // with the Float.parseFloat method and that's ok. I've added also error checking code, because sometimes,
  // i do not have just numbers in my strings but also weired stuff like = or letters (because the synchro between 
  // the Mac and the WiTilt is random sometimes...).
  // 
  // Of course, any comments on how to improve this is welcomed. So far, it works fine of my machine, almost real time...
  //
  //
  
  // localize the Carriage return byte to know where we are in the data stream
  if (inByte == 13){
  debut = true;
  }
  
  if (debut) {
  // Add the latest byte from the serial port to array:
  serialInArray[serialCount] = inByte;
  serialCount++;
  
  if (serialCount > 27 ) {  
    serialCount = 0;
    String serialInString = new String(char(serialInArray));
    
    // parsing of the serialInString into three different substring
    
    String strX = new String(serialInString.substring(4, 10));
    //print("X:"+strX);
    valX = parser(strX);
    X = valX;
    //X = int(1000*valX);
    //print(" : "+X);
    
    String strY = new String(serialInString.substring(13, 19));
    //print("  |  Y:"+strY);
    valY = parser(strY);
    Y = valY;
    //Y = int(1000*valY);
    //print(" : "+Y);
    
    String strZ = new String(serialInString.substring(22, 28));
    //print("  |  Z:"+strZ);
    valZ = parser(strZ);
    Z = valZ;
    //Z = int(1000*valZ);
    //println(" : "+Z);
    
    // Log the data for future use
    Date d = new Date();
    logfile.println(d.getTime() + "," + X + "," + Y + "," + Z);
    logfile.flush();    
    
    debut = false;
    }
  }
}

float parser(String val){
  
   // check where is the dot to be sure that we gonna parse a coherent value and not an outlier
   // mmm, this is not the best, sometimes it does not work, but overall it is pretty stable ;)
   // any comments on this point are welcomed !
   
   int indexpoint = val.indexOf(".");
   
   float valparsed = 0.0;
   
   if (indexpoint == 2){ 
     valparsed = Float.parseFloat(val);
     indexpoint = 0;
   } 
   return valparsed;
}