// slightly cleaned up from harvey moon's drawing machine
// http://www.kickstarter.com/projects/notever/the-drawing-machine/posts
// http://unanything.com/
// https://files.me.com/sami6877/wg01bu
// http://www.creativeapplications.net/maxmsp/drawing-machine-maxmsp-processing/
// 
// 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

// 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 processing.serial.*;  // serial communication
import java.io.*; //Java
import geomerative.*; // svg parsing

// configuration
XML xml;

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

String filename="/Users/rtwomey/Documents/blood_bag_bottom_seperation_13_17.svg";

PrintWriter writer;


//---------------------------------------------------
// 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 = 13.0;//36.0;//17.0;//22.0;//44.0;  // width
float PAGEH = 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

//subdividing lines segments 
float MAXLENGTH;
float MAXARCLENGTH;
float SVGSEGMENTLENGTH=0.1;

// 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);

String portName;
int portBaud;

//-----------------------------------
// 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;
boolean jumping = true;
boolean atPoint = true; // if the arduino returns that it is at the point, turn true.
boolean atHome=true;


//-----------------------------------
// image file setup and coordinates
RShape grp;
float grpW, grpH, grpX, grpY;
RPoint[][] pointPaths;
Path[] paths;

// global counters
int j = 0;
int i = 0;

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


//------------------------------------
// main program
void setup() {
  
  loadSettings();
  
  // graphics
  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();


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


  writer = createWriter(filename.split(".svg")[0]+".txt");
  machine.setupWriting(writer);

  background(255);

  // load image and create point paths
  //readSVGFile("/Users/rtwomey/Pictures/"+filename);
  println("about to read svg file");
  
  readSVGFile(filename);
  // where should we start drawing?
  i=0;//79s;  // path num to start with

  currpath = paths[i].num;  // index to path in pointPaths array

  if (paths[i].flipped) {
    currpoint=pointPaths[currpath].length-1;
    pointinc=-1;
    stoppoint=-1;
  } 
  else {
    currpoint=0;
    pointinc=1;
    stoppoint=pointPaths[currpath].length;
  }

  // set coords for current point
  Current = new PVector(pointPaths[currpath][currpoint].x, pointPaths[currpath][currpoint].y, 0);
  Last = new PVector(pointPaths[currpath][currpoint].x, pointPaths[currpath][currpoint].y, 1);


  // calc home values for X and Y (upper left corner of page)
  //HOMEA = sqrt(pow(PAGE0X, 2)+pow(PAGE0Y, 2));
  //HOMEB = sqrt(pow(MOTORSEP-(PAGE0X), 2)+pow(PAGE0Y, 2));
  if(printToConsole)
    println("(HOME: "+HOMEA + " " +HOMEB+")");

  machine.storeHome();
  machine.penUp();
  jumping=true;
}  

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

  // serial port
  XML serial = xml.getChild("port");
  portName = serial.getContent();
  portBaud = int(xml.getChild("baud").getContent());
  
  // machine geometry
  XML geometry = xml.getChild("geometry");
  HOMEA = float(geometry.getChild("homea").getContent())*INCH_TO_MM;
  HOMEB = float(geometry.getChild("homeb").getContent())*INCH_TO_MM;
  MOTORSEP = float(geometry.getChild("separation").getContent())*INCH_TO_MM;

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

  
  println("(motor separation "+MOTORSEP+")");
  println("(page zero "+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 (On && (i < pointPaths.length)) {

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

    // update to screen
    //print("point#:  " + currpoint + " / " + pointPaths[currpath].length+"\t"); 
    //println("path#:   " + i + "/ " +  pointPaths.length);

    currpoint+=pointinc;  // advance to next point

    // store old point
    Last.set(Current.x, Current.y, Current.z);

    if (currpoint != stoppoint) {  // still working on current path

      // set coords for new point
      Current.set(pointPaths[currpath][currpoint].x, pointPaths[currpath][currpoint].y, 1);

      // draw line from last to current if not end of current path
      //if (currpoint!=(stoppoint-pointinc)) {
        drawScaledSegment(Last.x, Last.y, Current.x, Current.y);
      //}

      if (jumping) {  // did we jump?

        Current.z = 1.0;
        jumping = false;  // done jumping

        machine.penDown();  // put pen down
      }
    } 
    else { // finished with current path

      jumping = true;   //jumping to next path     
      i++;   //increment path index

      machine.penUp();  // lift pen

      // have we hit every path?
      if (i<pointPaths.length) {
        currpath = paths[i].num;
        if (paths[i].flipped) {
          currpoint=pointPaths[currpath].length-1;
          pointinc=-1;
          stoppoint=-1;
        } 
        else {
          currpoint=0;
          pointinc=1;
          stoppoint=pointPaths[currpath].length;
        }
        if(printToConsole) {
          println("(**** moved to next path ****)");
          println("(path#:   " + i + "/ " +  pointPaths.length+")");
        }
        
        // set coords for new point
        Current.set(pointPaths[currpath][currpoint].x, pointPaths[currpath][currpoint].y, 0);

        // send the first point of next path twice
        // to guarantee that we have moved all the way to the next point before lifting pen back up
        //drawScaledSegment(Last.x, Last.y, Current.x, Current.y);
        
        machine.sendVal(Current.x/PPI, Current.y/PPI, Current.z);
        drawCrossHairs(Current.x, Current.y);
        if (serialOn)machine.waitForOk();

        //Current.set(pointPaths[currpath][currpoint].x+0.00001, pointPaths[currpath][currpoint].y, 1);
        Current.set(pointPaths[currpath][currpoint].x, pointPaths[currpath][currpoint].y, 1);
      } 
      else {  // done
        On=false;
        if(printToConsole) println("(DONE!)");
        machine.goHome();
      }
    }
  }
}


// keyboard interface

void keyPressed() {
  if (key == 'g') { 
    // start movement
    On = true;
    if(printToConsole)println("(GO!)");
    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;
    }
}
}

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();
};

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

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