//Programmer: Joe Morrow
//Contact: jmmorrow@mit.edu
//Date: Nov 26, 2011
//Description: Provides GUI and visualization for gimbal_code (arduino)

import toxi.geom.*;
import toxi.geom.mesh.*;
import toxi.processing.*;

import controlP5.*;
ControlP5 controlP5; 
PFont font;
PFont font2;
PFont font3;

TriangleMesh mesh;
ToxiclibsSupport gfx;

import processing.serial.*;
Serial myPort;

ControlWindow controlWindow;

int myColorBackground = color(100);
float[] data = new float[3];
int width = 600;
int height = 500;
int locY = 175 - 50;
int offY = 50;

void setup() {
  size(width, height, P3D);
  println(Serial.list());
  myPort = new Serial(this, Serial.list()[0], 115200);
  frameRate(30);
  //  smooth();
  font = loadFont("CourierNew36.vlw");
  font2 = loadFont("ArialNarrow-12.vlw");
  font3 = loadFont("CourierNew28.vlw");

  controlP5 = new ControlP5(this);

  controlWindow = controlP5.addControlWindow("controlP5window", 70, 10, 950, 175);
  controlWindow.setTitle("Control_Window");
  controlWindow.setBackground(myColorBackground);

  controlP5.addSlider("Inner Angle", -180, 180, 2, 950/12, locY - 2*offY, 800, 20);
  controlP5.addSlider("Middle Angle", -180, 180, 2, 950/12, locY - offY, 800, 20);
  controlP5.addSlider("Outer Angle", -180, 180, 2, 950/12, locY, 800, 20);
  Slider s1 = (Slider)controlP5.controller("Inner Angle");
  s1.setNumberOfTickMarks(181);
  s1.setSliderMode(Slider.FLEXIBLE);
  s1.setWindow(controlWindow);
  Slider s2 = (Slider)controlP5.controller("Middle Angle");
  s2.setNumberOfTickMarks(181);
  s2.setSliderMode(Slider.FLEXIBLE);
  s2.setWindow(controlWindow);
  Slider s3 = (Slider)controlP5.controller("Outer Angle");
  s3.setNumberOfTickMarks(181);
  s3.setSliderMode(Slider.FLEXIBLE);
  s3.setWindow(controlWindow);
  Controller t1 = controlP5.addTextfield("Inner_Value", 950/50, locY - 2*offY, 30, 20);
  t1.setWindow(controlWindow);
  Controller t2 = controlP5.addTextfield("Middle_Value", 950/50, locY - offY, 30, 20);
  t2.setWindow(controlWindow);
  Controller t3 = controlP5.addTextfield("Outer_Value", 950/50, locY, 30, 20);
  t3.setWindow(controlWindow);
}

void draw() {

  background(0);
  data[0] = int(controlP5.controller("Inner Angle").value());
  data[1] = int(controlP5.controller("Middle Angle").value());
  data[2] = int(controlP5.controller("Outer Angle").value());
  textFont(font, 30);
  fill(255);
  textMode(SCREEN);
  textAlign(RIGHT);
  text("All Units Degrees", width - 25, 40);
  textFont(font2, 12);
  textAlign(LEFT);
  text("Programmer: Joe Morrow \n" +
    "Contact: jmmorrow@mit.edu \n" +
  "Description: Graphical User Interface for \n MAS863 final project. " +
  "The three sliders \n control servo angles on an external 3-axis \n gimbal. " +
  "Press 'Space Bar' to reset all \n angles to 0. 'qweasd' step angles. " +
  "Angle \n data exported to COM0." ,10,15);
  
  if (((data[1] >= 85) & (data[1] <= 95)) | ((data[1] <= -85) & (data[1] >= -95)) ) {  
    fill(255, 255, 0);
    ellipse(width/1.9, height/6-6, 35, 35);
    textFont(font3, 30);
    fill(255);
    textMode(SCREEN);
    textAlign(RIGHT);
    text("Gimbal Lock", width - 50, height/6);
  } 
  else {
    fill(30);
    ellipse(width/1.9, height/6-6, 35, 35);
    textFont(font3, 30);
    fill(255);
    textMode(SCREEN);
    textAlign(RIGHT);
    text("Gimbal Lock", width - 50, height/6);
  }



  fill(125);
  lights();
  translate(width/2, height/1.8, 0);
  mesh =(TriangleMesh)new STLReader().loadBinary(sketchPath("F4_Phantom.stl"), STLReader.TRIANGLEMESH);  
  gfx=new ToxiclibsSupport(this);
  mesh.scale(0.04);
  mesh.translate(new Vec3D());
  mesh.rotateZ(PI/180*90);
  mesh.rotateX(PI/180*90);
  //  mesh.transform(new Matrix4x4().rotateY(mouseX*0.01));
  mesh.rotateY(-PI/180*(controlP5.controller("Inner Angle").value()));
  mesh.rotateZ(-PI/180*(controlP5.controller("Middle Angle").value()));
  mesh.rotateX(-PI/180*(controlP5.controller("Outer Angle").value()));
  gfx.origin(new Vec3D(), 200);
  noStroke();
  gfx.mesh(mesh, false, 0);
  mesh.clear();

  myPort.write(0); //tag
  myPort.write(0);
  for (int i = 0; i < 3; i++) {
    if(i == 0) {
      data[i] = map(data[i],-180,180,127,1);
    } else {
      data[i] = map(data[i],-180,180,1,127);
    }
    myPort.write(byte(data[i]));
//    data[i] = data[i] + 200; //make positive
//    if (data[i] > 255) {
//      myPort.write(data[i]);
//    } 
//    else {
//      myPort.write(0);
//      myPort.write(data[i]);
//    }
  }

  //  print(data[0]);
  //  print(" ");
}
///////////////////////////////////

//void controlEvent(ControlEvent theEvent) {
//  println("[" + int(controlP5.controller("Inner Angle").value()) +
// " " + int(controlP5.controller("Middle Angle").value()) + " " +
// int(controlP5.controller("Outer Angle").value()) + "]");
//}

void keyPressed() {
  if (key == ' ') {
    println("###RESET###");
    //    delay(100);
    controlP5.controller("Inner Angle").setValue(2.0);
    controlP5.controller("Middle Angle").setValue(2.0);
    controlP5.controller("Outer Angle").setValue(2.0);
  }
  if (key == 'q') {
    controlP5.controller("Inner Angle").setValue(int(controlP5.controller("Inner Angle").value())+6);
  }
  if (key == 'a') {
    controlP5.controller("Inner Angle").setValue(int(controlP5.controller("Inner Angle").value())-2);
  }
  if (key == 'w') {
    controlP5.controller("Middle Angle").setValue(int(controlP5.controller("Middle Angle").value())+6);
  }
  if (key == 's') {
    controlP5.controller("Middle Angle").setValue(int(controlP5.controller("Middle Angle").value())-2);
  }
  if (key == 'e') {
    controlP5.controller("Outer Angle").setValue(int(controlP5.controller("Outer Angle").value())+6);
  }
  if (key == 'd') {
    controlP5.controller("Outer Angle").setValue(int(controlP5.controller("Outer Angle").value())-2);
  }
}

public void Inner_Value(String theValue) {
  controlP5.controller("Inner Angle").setValue(float(theValue)+2);
}
public void Middle_Value(String theValue) {
  controlP5.controller("Middle Angle").setValue(float(theValue)+2);
}
public void Outer_Value(String theValue) {
  controlP5.controller("Outer Angle").setValue(float(theValue)+2);
}