// v 0.2 - added servo to left pen from page. controlled with G100 S255 (pen down) and G100 S100 (pen up).
// v 0.3 - added xml config file for drawing machine parameters
// v 0.4 - added OSC interface and proper path buffer

// rtwomey@u.washington.edu

// Key 1 = 0,0
// Key 2 = width,0
// Key 3 = width,height
// Key 4 = 0,height
// Keys 5-8, corners of svg image
// Key 0 = Home
// Key H = home device
// Key h = set origin and init coords.
// Key S = stop
// Key G = go

Bot machine;

import oscP5.*;        //  Load OSC P5 library
import netP5.*;        //  Load net P5 library

import processing.serial.*;  // serial communication
import java.io.*; //Java
import geomerative.*; // svg parsing

// configuration
XML xml;

boolean serialOn = true;
boolean writeToFile = true;
boolean printToConsole = true;
boolean doRepeat = true;

PrintWriter writer;

// osc messages
OscP5 oscP5;            //  Set oscP5 as OSC connection
float oscX, oscY, oscSx, oscSy;
float oscTheta=-1*PI;
boolean changed=false;

//---------------------------------------------------
// configuration read from XML configuration file

// machine geometry
float MOTORSEP = 96.0;//72.0; // how far apart are the two steppers
float HOMEA = 86.5;//81.0;//82.0;// 58.5225650924496; // 108
float HOMEB = 86.5;//81.0;//82.0;//56.02580320709378; // 108

// page setup
float PAGEW = 48.0;//13.0;//36.0;//17.0;//22.0;//44.0;  // width
float PAGEH = 36.0;//17.0;//24.0;//14.0;//30;//30.0;  // height
float PAGE0Y = 32.0;//6.0-PAGEH;
float PAGE0X = (MOTORSEP-PAGEW)/2.0;  // upper left

float MARGIN = 0.5;

// feed rates and gcode
float MAXLENGTH=1.0;
float MAXARCLENGTH=0.1;//0.5;

// output settings
float PPI = 90.0; // SVG (Pt) to plotter space (inch) coordinate conversion
float multiplier = 1.0; // global scale

// on-screen scale 
// scale width to full height
int windowH = 900;
float onScreenScale=windowH/(PAGEH*PPI);
int windowW = int((PAGEW*PPI)*onScreenScale);

// scale height to full width
//int windowW = 1400;
//float onScreenScale=windowW/(PAGEW*PPI);
//int windowH = int((PAGEH*PPI)*onScreenScale);

//-----------------------------------
// state variables
boolean On = false;// USED SO THAT THE KEY 'S' AND 'G' START AND STOP THE DRAWING SEQUENCE, starts off to calibrate

PVector Current;
PVector Last;
PVector Endpoint; // end of current line

boolean jumping = true;
boolean atPoint = true; // if the arduino returns that it is at the point, turn true.
boolean atHome=true;
boolean done = false;

float zoom=1.0;
int repeats=0;

//-----------------------------------
// path buffer
PathBuffer pathbuff;
float bufferTheta=-1*PI;

// state of point/path iterators
int currpath=0;
int currpoint=0;
int pointinc=1;
int stoppoint;


//------------------------------------
// main program
void setup() {

  loadSettings();

  pathbuff=new PathBuffer();

  // graphics
  //windowW=900;
  println(windowW);
  size(windowW, windowH);

  // load a font for text display
  PFont font;
  font = loadFont("Serif-24.vlw"); 
  textFont(font, 12); 

  //  // store progress images
  //  Photo = new OutputImage();

  // osc interface
  oscP5 = new OscP5(this, 8000);  // Start oscP5, listening for incoming messages at port 8000

    // arduino g-code interface
  machine = new Bot(this);
  machine.waitForStart();

  if(writeToFile) {
    String filename="circle"+PAGEW+"_"+PAGEH+".txt";
    writer = createWriter(filename);
    machine.setupWriting(writer);
  }
  
  background(255);

  if (printToConsole)
    println("(HOME: "+HOMEA + " " +HOMEB+")");

  machine.storeHome();
  machine.penUp();

  Last = new PVector(MOTORSEP/2, sqrt(81*81-(MOTORSEP/2*MOTORSEP/2)), 0);
  Current= new PVector();
  Endpoint = new PVector();

  println("(at "+Last.x+","+Last.y+" relative to PAGE(0,0))");

  noCursor();

  println(onScreenScale);
}  

void loadSettings() {
  xml = loadXML("settings.xml");

  // machine geometry
  XML geometry = xml.getChild("geometry");
  HOMEA = float(geometry.getChild("homea").getContent());
  HOMEB = float(geometry.getChild("homeb").getContent());
  MOTORSEP = float(geometry.getChild("separation").getContent());

  // feed rates
  XML rates = xml.getChild("rates");
  SLOWFEED = float(rates.getChild("slowfeed").getContent());
  
  // page setup
  XML page = xml.getChild("page");
  PAGEW = float(page.getChild("width").getContent());
  PAGEH = float(page.getChild("height").getContent());
  PAGE0Y = float(page.getChild("pagezeroy").getContent());
  PAGE0X = (MOTORSEP-PAGEW)/2.0;  // upper left

  println(MOTORSEP);
  println(PAGE0X+", "+PAGE0Y);

  // output scaling
  XML output = xml.getChild("output");
  PPI = float(output.getChild("ppi").getContent()); // SVG (Pt) to plotter space (inch) coordinate conversion
  multiplier = float(output.getChild("scale").getContent()); // global scale
  MAXARCLENGTH = float(output.getChild("maxarclength").getContent()); // global scale
  MAXLENGTH = float(output.getChild("maxlength").getContent()); // global scale

  // on-screen display
  XML screen = xml.getChild("screen");

  if (PAGEH>PAGEW) {  
    // portrait: scale width to full height
    windowH = int(screen.getChild("screenh").getContent());
    float onScreenScale=windowH/(PAGEH*PPI);
    windowW = int((PAGEW*PPI)*onScreenScale);
  } 
  else {
    // landscape: scale height to full width
    windowW = int(screen.getChild("screenw").getContent());
    onScreenScale=windowW/(PAGEW*PPI);
    windowH = int((PAGEH*PPI)*onScreenScale);
  }
}

void draw() {  

  stroke(120);
  strokeWeight(2);
  fill(220);

  if (changed) {

    // load up path buffer
    float left=MARGIN+((PAGEW-PAGEH)/2.0);
    float top=MARGIN;
    float right=PAGEW-(MARGIN+((PAGEW-PAGEH)/2.0));
    float bot=PAGEH-MARGIN;

    float midx=(left+right)/2.0;
    float midy=(top+bot)/2.0;

    float tl, tr, tt, tb;

    tl=lerp(midx, left, zoom);
    tr=lerp(midx, right, zoom);
    tt=lerp(midy, top, zoom);
    tb=lerp(midy, bot, zoom);

    fillPathBuffer((tl+tr)/2.0, (tt+tb)/2.0, (tr-tl)/2.0);
    changed=false;
  }

  if (On && !done) {
    if (drawNext()) {
      println("(done.)");

      println("(starting over)");
      //background(255);
      fill(255, 192);
      noStroke();
      rect(0, 0, windowW, windowH);
      repeats++;
      println("(pattern iteration "+repeats+")");
      zoom=1.0;

      println("(stopping)");
      done=true;
      //On=false;
    }
  }

  fill(255);
  noStroke();
  rect(0,0,50,50);
  fill(0);
  text(repeats, 10, 20);
  text(oscTheta, 10, 30);
  text(bufferTheta, 10, 40);

  stroke(0, 0, 128, 62);
  strokeWeight(1);
  ellipse(oscSx, oscSy, 1, 1);
  line(100, 100, 100+100*cos(oscTheta), 100+100*sin(oscTheta));
  noStroke();
}

// -------- striving for the pope's commission -------- //
// -------- drawing routines --------//
boolean drawNext() {
  if (pathbuff.getLength()>0) {
    Current.x=pathbuff.getFrontX();
    Current.y=pathbuff.getFrontY();
    pathbuff.removeFront();

    drawScaledSegment(Last.x, Last.y, Current.x, Current.y); 
    drawCrossHairs(Current.x, Current.y);
    
    machine.sendVal(Current.x/PPI, Current.y/PPI, 1.0);//Current.z); // send current point
    drawCrossHairs(Current.x, Current.y);
    if (serialOn) machine.waitForOk();

    Last.x=Current.x;
    Last.y=Current.y;
    return false;
  }
  return true;
}

void stop() {
  machine.penUp();
  //machine.DirectCommand("G1 X"+HOMEA+" Y"+HOMEB+" Z0.0 F"+FASTFEED);
  machine.DirectCommand("G0 X"+HOMEA+" Y"+HOMEB+" Z0.0");
  machine.stop();
};

void fillPathBuffer(float xcenter, float ycenter, float radius) {
  
 
  float dtheta=oscTheta-bufferTheta;
  if ((PI-bufferTheta<=0.1*PI)&&(oscTheta<=-0.9*PI))
    dtheta+=TWO_PI;
 
  float arclength=dtheta*radius;
  int numsegments = int(arclength/MAXARCLENGTH);
  float theta_inc=dtheta/numsegments;
  //println(arclength+" "+numsegments+" "+arc_inc);

  if (numsegments>0) {
    int len=pathbuff.getLength()-1;
    if(len<0)len=0;
//    println("FILL BUFFER: ");
//    println("\tRADIUS: "+radius);
//    println("\tnum segments: "+numsegments);
//    println("\toscTheta: "+oscTheta);
//    println("\tbufferTheta: "+bufferTheta);
//    println("\tarc length: "+arclength);

    for (int i=0;i<numsegments;i++) {
      float theta=bufferTheta+theta_inc*i;

      float x1, y1;
      x1 = xcenter + radius * cos(theta);
      y1 = ycenter + radius * sin(theta);
      //print("\t"+i);
      //println(": "+x1+","+y1);
      pathbuff.addPoint(x1*PPI, y1*PPI);
    }

    //println("drawme");
    //println(pathbuff.getLength());
    pathbuff.draw(len, pathbuff.getLength());
    bufferTheta=oscTheta;
    done=false;
  }
}

// -------- osc messaging to control advance of drawing -------- //
void oscEvent(OscMessage theOscMessage) {   //  This runs whenever there is a new OSC message

    String addr = theOscMessage.addrPattern();  //  Creates a string out of the OSC message

  //println(addr);
  if (addr.indexOf("/3/toggle") !=-1) {   // Filters out any toggle buttons
    int i = int((addr.charAt(9) )) - 0x30;   // returns the ASCII number so convert into a real number by subtracting 0x30
    println(i);
    switch(i) {
    case 1:
      // start
      On = true;
      if (printToConsole)println("(GO!)");

      machine.DirectCommand("G100 S100"); //moves servo, push pen off page
      machine.waitForOk();
      jumping=true;
      break;
    case 2:
      // stop movement
      On = false;
      if (printToConsole)println("(STOP)");
      break;
    case 3:
      // go home
      machine.penUp();
      On=false;
      if (printToConsole)println("(going to stored home position)");
      //machine.DirectCommand("G1 X"+HOMEA+" Y"+HOMEB+" Z0.0 F"+FASTFEED);
      machine.DirectCommand("G0 X"+HOMEA+" Y"+HOMEB+" Z0");
      break;
    }
    //led[i]  = int(theOscMessage.get(0).floatValue());     //  Puts button value into led[i]
    // Button values can be read by using led[0], led[1], led[2], etc.
  }

  if (addr.indexOf("3/xy")!=-1) {
    oscX=(float)theOscMessage.get(0).floatValue();
    oscY=(float)theOscMessage.get(1).floatValue();
    oscX=oscX-0.5;
    oscY=oscY-0.5;
    oscSx=100+oscX*200;
    oscSy=100+oscY*200;

    float temptheta=atan2(oscY, oscX);//+PI;
    
    if (((temptheta>oscTheta)&&(temptheta-oscTheta<=0.1*PI))||((PI-oscTheta<=0.1*PI)&&(temptheta<=-0.9*PI))) {
      oscTheta=temptheta;
      changed=true;
    }

    //println(temptheta+" "+oscTheta);
  }
}


// -------- keyboard interface -------

void keyPressed() {
  if (key == 'g') { 
    // start movement
    On = true;
    if (printToConsole)println("(GO!)");

    machine.DirectCommand("G100 S100"); //moves servo, push pen off page
    machine.waitForOk();
    jumping=true;
  }

  if (key == 's') {
    // stop movement
    On = false;
    if (printToConsole)println("(STOP)");
  }

  if (key=='h') {
    machine.goHome();
  }

  if (key == ' ') {
    background(255);
  }

  if (key=='p') {
    //drawSortedPointPaths();
  }

  if (!On) {
    switch(key) {
    case '1':
      // top left
      machine.sendVal(0, 0, 0);
      break;
    case '2':
      // top right
      machine.sendVal(PAGEW, 0, 0);
      break;
    case '3':
      // lower right
      machine.sendVal(PAGEW, PAGEH, 0);
      break;
    case '4':
      // lower left
      machine.sendVal(0, PAGEH, 0);
      break;
      //    case '5':
      //      // top left
      //      machine.sendVal(grpX/PPI, grpY/PPI, 0);
      //      break;
      //    case '6':
      //      // top right
      //      machine.sendVal((grpX+grpW)/PPI, grpY/PPI, 0);
      //      break;
      //    case '7':
      //      // lower right
      //      machine.sendVal((grpX+grpW)/PPI, (grpY+grpH)/PPI, 0);
      //      break;
      //    case '8':
      //      // lower left
      //      machine.sendVal(grpX/PPI, (grpY+grpH)/PPI, 0);
      //      break;      
    case '0':
      machine.penUp();
      if (printToConsole)println("(going to stored home position)");
      //machine.DirectCommand("G1 X"+HOMEA+" Y"+HOMEB+" Z0.0 F"+FASTFEED);
      machine.DirectCommand("G0 X"+HOMEA+" Y"+HOMEB+" Z0");
      break;
    case 'd':
      machine.penDown();
      break;
    case 'u':
      machine.penUp();
      break;
    default:
      break;
    }
  }
}


// -------- terminal output and logging -------- // 

void doMessage(String message) {
  if (printToConsole)
    println(message);
  if (writeToFile) {
    writer.write(message+"\n");
    writer.flush();
  }
}