import processing.serial.*;
import controlP5.*;
import processing.opengl.*;

// Controller
ControlP5 MyController;

// 2D Boot
PImage Boot2D;

// Vibrators
int[] VibratorStatus = {0,0,0,0,0,0,0,0};
float[] VibratorX = {70,50,125,80,150,150,80,150};      //relative to top left location of Boot2D
float[] VibratorY = {50,150,150,250,250,350,450,450};   //relative to top left location of Boot2D
float VibratorDiameter = 40;
int VibratorClicks = 0; 
int VibratorMessage;
String VibratorWords;
byte[] Message = {0,0,0,0,0,0,0,0};

// Temp and Strain
ControlGroup messageBox;
Textlabel tempTextlabel, strainTextlabel;
Textlabel tempTextvalue, strainTextvalue;
String temperature = "14";
String strain = "n/a";
int straintest = 0;

// Title
Button b1, b2;
DropdownList d1;

// 3D Boot Variables
int val = 0; // To store data from serial port, used to color background
int xoffl = 1;
int xoffh = 1;
int xonl = 1;
int xonh = 1;
int yoffl = 1;
int yoffh = 1;
int yonl = 1;
int yonh = 1;
int count = 1;

int byte2 = 0;
int byte3 = 0;
int byte4 = 0;

float HighX =.4;
float LowX = .4;

float degreesx = 0;
float degreesy = 0;

// Serial Port
Serial port3; // com3, left of laptop
Serial port6;    // com6, right of laptop

// SETUP
void setup() {
  size(1000,850,P3D);
  background(0);
  lights();
  
  // 2D Boot variables
  Boot2D = loadImage("footprint.png");  //load the footprint onto the program
  
  // Vibrators
  drawVibrators();
  
  // Slider
  noStroke();
  MyController = new ControlP5(this);
  MyController.addSlider("PWM", 0, 255, 127, width*3/4, height/4+550, 200, 20)
    .setNumberOfTickMarks(10);
  //MyController.addSlider("name of slider", min range, max range, starting balue, x location, y location, width, height);
  
  // Temp and Strain
  createMessageBox();
 
 //Title
  d1 = MyController.addDropdownList("myList-dl")
    .setPosition(100,100);
  customize(d1);
  
  // Serial
  println(Serial.list());
  port3 = new Serial(this, Serial.list()[0], 9600);   //use the first available port
  port6 = new Serial(this, Serial.list()[1], 9600);   //use second port
}

// DRAWING
void draw() {
  hint(ENABLE_DEPTH_TEST);
  pushMatrix();

  clear();
  draw3DBoot();  //
  popMatrix();
  hint(DISABLE_DEPTH_TEST);
}

void mousePressed() {  // things that only need to change when I click something
  if(overVibrator()){
    drawVibrators();
    VibratorClicks = VibratorClicks + 1;
    createVibratorMessage();
    //portout.write(Message);
    port3.write(Message);

  }
}

// Vibrators
boolean overVibrator() {
  boolean answer = false;
  for(int i = 0; i < VibratorStatus.length; i++){
    float disX = width*3/4+VibratorX[i] - mouseX;
    float disY = height/4+VibratorY[i] - mouseY;
    if (sqrt(sq(disX) + sq(disY)) < VibratorDiameter/2 ) {
      if (VibratorStatus[i] == 1){
        VibratorStatus[i] = 0;
      } else {
        VibratorStatus[i] = 1;
      }
      answer = true;
    } 
  }
  return answer;
}
void drawVibrators(){
  stroke(255); 
  for (int i = 0; i < VibratorStatus.length; i++){
    fill(VibratorStatus[i]*255,0,0);
    stroke(VibratorStatus[i]*255,0,0);
    ellipse(width*3/4+VibratorX[i], height/4+VibratorY[i], VibratorDiameter, VibratorDiameter);  
  }
}
void createVibratorMessage(){
 // Message = {0,0,0,0,0,0,0,0};
 for (int i = 0; i < VibratorStatus.length; i++){
    Message[i] = (byte)VibratorStatus[i];  
  }
  //A = board1 or 2
//B = vibrator number 0-7
//C = on (1) or off (2)
//D = pwm hundreds value
//E = pwm tens value
//F = pwm ones value
//G = 
//H = 

  println(Message);               
}

// TEMPERATURE AND STRAIN
void createMessageBox(){
  // create a group to store the messageBox elements
  messageBox = MyController.addGroup("messageBox", width*3/4, height/4-70, 175);
  messageBox.setBackgroundHeight(60);
  messageBox.setBackgroundColor(color(37,94,253));
  messageBox.hideBar();
  
  // add static TextLabels to messageBox
  tempTextlabel = MyController.addTextlabel("tempTextlabel", "Temp (F): ", 10, 15);  //
  tempTextlabel.moveTo(messageBox);
  strainTextlabel = MyController.addTextlabel("strainTextlabel", "Strain: ", 10, 35);
  strainTextlabel.moveTo(messageBox);
  
  // add variable TextLabels to message Box
  tempTextvalue = MyController.addTextlabel("tempTextvalue", temperature, 80, 15);
  tempTextvalue.moveTo(messageBox);
  strainTextvalue = MyController.addTextlabel("strainTextvalue", strain, 80, 35);
  strainTextvalue.moveTo(messageBox);
}

void updateMessageBox(){
  //tempTextvalue.setValue(strain);
  //straintest++;
  //tempTextvalue.setValue(str(straintest));
}

// TITLE
void customize(DropdownList ddl) {
  // a convenience function to customize a DropdownList
  ddl.setItemHeight(20);
  ddl.setBarHeight(40);

  ddl.captionLabel().style().marginTop = 3;
  ddl.captionLabel().style().marginLeft = 3;
  ddl.valueLabel().style().marginTop = 3;
  ddl.addItem("HTM(A)A Astronaut Smart Boot", 0);
  ddl.addItem("MASSAGE MODE!", 1);

  ddl.setColorBackground(color(0));
  ddl.setColorActive(color(255, 128));
  ddl.setIndex(0);
  ddl.setWidth(300);
}

// 3D BOOT
void draw3DBoot(){
  // to have both 2d and 3d, boot and vibrators must be drawn within 3Dboot function.
  image(Boot2D, width*3/4, height/4, Boot2D.width*.75, Boot2D.height*.75); //img, x, y, scalex, scaley
  drawVibrators();
  translate(width/4, height/2, 0);
  stroke(255);
  rotateX(radians(90));
  rotateX(radians(degreesy));
  rotateY(radians(degreesx));
  
  // base
  fill(153,204,255); // light blue
  beginShape();
  vertex(-100, 0, 0);
  vertex(100, 0,0);
  vertex(100, 50, 0);
  vertex(-100,50,0);
  endShape(CLOSE);
  
  // base walls
  fill(153,204,255);
  beginShape();
  vertex(-100, 0, 0);
  vertex(-100, 0, 20);
  vertex(100, 0,20);
  vertex(100, 0,0);
  endShape(CLOSE);
  beginShape();
  vertex(-100, 50, 0);
  vertex(-100, 50, 20);
  vertex(100, 50,20);
  vertex(100, 50,0);
  endShape(CLOSE);
  
  fill(160,160,160);
  beginShape();
  vertex(100, 0,20);
  vertex(100, 50, 0);
  vertex(100, 50, 20);
  vertex(-100,50,0);
  vertex(-100,50,20);
  vertex(-100, 0, 0);
  vertex(-100, 0, 20);
  vertex(100, 0,0);
  vertex(100, 0,20);
  vertex(100, 50, 0);
  vertex(100, 50, 20);
  vertex(-100,50,0);
  vertex(-100,50,20);
  
  // Top of Toes
  vertex(-100, 0, 20);
  vertex(100, 0,20);
  vertex(100, 50, 20);
  vertex(-100,50,20);
  vertex(-100,0,20);
  
  // Top of Ankle
  vertex(20, 0, 50);
  vertex(100, 0,50);
  vertex(100, 50, 50);
  vertex(20,50,50);
  vertex(20,0,50);
  
  // Calf
  vertex(20, 0, 80);
  vertex(100, 0,80);
  vertex(100, 50, 80);
  vertex(20,50,80);
  vertex(20,0,80);

  endShape(CLOSE);
}


// SERIAL - Called whenever there is something available to read
void serialEvent(Serial port6) {
  // Data from the Serial port is read in serialEvent() using the read() function and assigned to the global variable: val
  int byte1 = byte2;
  byte2 = byte3;
  byte3 = byte4;
  byte4 = port6.read();
  
  
  if (count == 1){
    xoffl = byte4;
    count = 2;
    //println("--count:" + count);
    println("xoffl:" + xoffl);
  }
  else if (count == 2){
    xoffh = byte4;
    count = 3;
    //println("--count:" + count);
    println("xoffh:" + xoffh);
  }
  else if (count == 3){
    xonl = byte4;
    count = 4;
   // println("--count:" + count);
    println("xonl:" + xonl);
  }
  else if (count == 4){
    xonh = byte4;
    count = 5;
    //println("--count:" + count);
    println("xonh:" + xonh);
  }
  else if (count == 5){
    yoffl = byte4;
    count = 6;
    //println("--count:" + count);
    println("yoffl:" + yoffl);
  }
  else if (count == 6){
    yoffh = byte4;
    count = 7;
    //println("--count:" + count);
    println("yoffh:" + yoffh);
  }
  else if (count == 7){
    yonl = byte4;
    count = 8;
    //println("--count:" + count);
    println("yonl:" + yonl);
  }
  else if (count == 8){
    yonh = byte4;
    count = 9;
    //println("--count:" + count);
    println("yonh:" + yonh);
  }
  
  if ((count==9) && ((byte1==1) && (byte2==2) && (byte3==3) && (byte4==4))){
    println("YAAAAAAAY");
    count = 1;
  }
  
  float xoff = 256*xoffh+xoffl;
  float xon = 256*xonh+ xonl;
  float xvalue = xoff/(xon+xoff);
  
  float yoff = 256*yoffh+yoffl;
  float yon = 256*yonh+ yonl;
  float yvalue = yoff/(yon+yoff);
  
  degreesx = map(xvalue, .3, .55, -90, 90);
  degreesy = map(yvalue, .3, .55, -90, 90);
  // .3 to .55
  println("------------------XVALUE " + degreesx);
  println("------------------YVALUE " + degreesy);
  
  //println("------------------High " + HighX);
  //println("------------------Low " + LowX);
}