//
// gridding off space available in the image plane
// repeatibility
// accuracy
// precision
//
// 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

// configuration
boolean serialOn = true;
boolean writeToFile = true;
boolean printToConsole = true;

// fast forward to resume previous job
boolean rununtil=false;
int runtoX=8;
int runtoY=10;
int runtoZoom=4;


PrintWriter writer;
//---------------------------------------------------
// physical setup
float MOTORSEP = 96.0;//72.0; // how far apart are the two steppers

// offsets
// line A, at limit switch, is 2.0 inches out. 
// line B, at limit switch, is 1.25 inches out. 

// paper size
float PAGEW = 66.0;//86.0;//36.0;//17.0;//22.0;//44.0;  // width
float PAGEH = 48;//60.0;//14.0;//30;//30.0;  // height
float MARGIN = 6.0;//12.0;

// page location relative to motors
float PAGE0X = (MOTORSEP-PAGEW)/2.0;  // upper left
float PAGE0Y = 12.0;//3.0;//12.0;//6.0-PAGEH;  // top of paper //12.0;//40.0;

// home position of pen carriage. where it is when the program starts
float HOMEA = 81.0;//86.5;//82.0;// 58.5225650924496; // 108
float HOMEB = 81.0;//86.5;//82.0;//56.02580320709378; // 108

// scale image to screen for display
int windowH = 900;
float onScreenScale=windowH/PAGEH;
int windowW = int(PAGEW*onScreenScale);

// feed rates and gcode stuff
float FASTFEED = 175.0;//200.0;
float SLOWFEED = 100.0;//15.0;

float MAXLENGTH=1.0;

static int PEN_UP = 0;
static int PEN_DOWN = 1;

int WAITTIME = 0;//1500;

// limit of the algorithmic process
int MAXDEPTH=10;

int FORWARD = 0;
int BACKWARD= 1;

int START = 0;
int PATTERN_A=1;
int PATTERN_B=2;

//-----------------------------------
// state variables
boolean On = false;// USED SO THAT THE KEY 'S' AND 'G' START AND STOP THE DRAWING SEQUENCE, starts off to calibrate
boolean done = false;
boolean jumping = true;
boolean atPoint = true; // if the arduino returns that it is at the point, turn true.
boolean atHome=true;

int zoomLevel=0;
int x=0;
int y=0;
int inc=1;

int state=START;
int dir=FORWARD;
int count=0;

PVector Last;

//------------------------------------
// main program
void setup() {
  // 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("test.txt");
  machine.setupWriting(writer);

  background(255);

  // 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));
  //HOMEA = 63.261147;
  //HOMEB = 58.017002;

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

  machine.storeHome();
  machine.penUp();
  Last = new PVector(MOTORSEP/2, sqrt(81*81-(MOTORSEP/2*MOTORSEP/2)), 0);
  println("(at "+Last.x+","+Last.y+" relative to PAGE(0,0))");

  noCursor();
}


void draw() {  
  stroke(120);
  strokeWeight(2);
  fill(220);

  if (On && !done) {
    if (drawGridAtScale(MARGIN, MARGIN, PAGEW-MARGIN, PAGEH-MARGIN, int(pow(2, zoomLevel)))) {
      if (incrementPosition(int(pow(2, zoomLevel))))
      {
        zoomLevel++;

        if (printToConsole)
          println("(grid "+zoomLevel+":"+MAXDEPTH+")");

        if (zoomLevel>MAXDEPTH) {
          done=true;
        }
      }
    }
    if (doneFFWDing())
      delay(WAITTIME);
  }
}

boolean doneFFWDing() {
  if (!rununtil)return true;

  if ((x==runtoX) && (y==runtoY) && (zoomLevel==runtoZoom)) {
    print("found it.\n");
    serialOn=true;
    rununtil=false;
  }
  return false;
}

boolean incrementLine(int scale) {
  y++;

  if (printToConsole)
    println("(line "+y+":"+zoomLevel+")");
  if (y>=scale) {
    y=0;
    return true;
  }
  return false;
}

int toggleState(int state) {
  if (state==PATTERN_A)
    return PATTERN_B;
  return PATTERN_A;
}

boolean incrementPosition(int scale) {

  if (state==START) {
    state=PATTERN_A;
    return true;
  }

  state = toggleState(state);

  if (dir==FORWARD) {
    x++;
    if (x>=scale)
    {
      x=scale-1;
      dir=BACKWARD;
      return incrementLine(scale);
    }
  } 
  else {
    x--;
    if (x<0) {
      x=0;
      dir=FORWARD;
      return incrementLine(scale);
    }
  }
  return false;
}

boolean drawGridAtScale(float left, float top, float right, float bot, int scale) {

  if (state==START) {
    return drawFirstBox(MARGIN, MARGIN, PAGEW-MARGIN, PAGEH-MARGIN);
  } 
  else {

    float ux=(right-left)/(float)scale;
    float uy=(bot-top)/(float)scale;

    if (dir==FORWARD) {
      switch (count) {
      case 0:
        if (state!=PATTERN_A)  
          drawScaledSegment(left+ux*x, top+uy*y, left+ux*(x+1), top+uy*(y+1));
        if (state!=PATTERN_B)
          drawScaledSegment(left+ux*x, top+uy*(y+1), left+ux*(x+1), top+uy*y);
        break;
      case 1:      
        drawScaledSegment(left+ux*(x+1), top+uy*(y+0.5), left+ux*x, top+uy*(y+0.5));
        break;
      case 2:
        if (state!=PATTERN_A)  
          drawScaledSegment(left+ux*(x+0.5), top+uy*y, left+ux*(x+0.5), top+uy*(y+1));
        if (state!=PATTERN_B)  
          drawScaledSegment(left+ux*(x+0.5), top+uy*(y+1), left+ux*(x+0.5), top+uy*y);

        break;
      };
    } 
    else {
      //BACKWARD
      switch (count) {
      case 0:
        if (state!=PATTERN_A)  
          drawScaledSegment(left+ux*(x+1), top+uy*(y+1), left+ux*x, top+uy*y);
        if (state!=PATTERN_B)
          drawScaledSegment(left+ux*(x+1), top+uy*y, left+ux*x, top+uy*(y+1));
        break;
      case 1:
        drawScaledSegment(left+ux*x, top+uy*(y+0.5), left+ux*(x+1), top+uy*(y+0.5));
        break;
      case 2:
        if (state!=PATTERN_A)  
          drawScaledSegment(left+ux*(x+0.5), top+uy*(y+1), left+ux*(x+0.5), top+uy*y);
        if (state!=PATTERN_B)  
          drawScaledSegment(left+ux*(x+0.5), top+uy*y, left+ux*(x+0.5), top+uy*(y+1));
        break;
      }
    }
    count++;
    if (count>2) {
      count=0;
      return true;
    }
  }
  return false;
}

boolean drawFirstBox(float left, float top, float right, float bot) {
  float ux=(right-left);
  float uy=(bot-top);
  float x=0;
  float y=0;

  switch (count) {
  case 0:
    drawScaledSegment(left, top, right, top);
    break;
  case 1:
    drawScaledSegment(right, top, right, bot);
    break;
  case 2:
    drawScaledSegment(right, bot, left, bot);
    break;
  case 3:
    drawScaledSegment(left, bot, left, top);
    break;
  case 4:
    drawScaledSegment(left, top, right, bot);
    break;
  case 5:
    drawScaledSegment(right, top, left, bot);
    break;
  case 6:      
    drawScaledSegment(left, top+uy*0.5, right, top+uy*0.5);
    break;
  case 7:
    drawScaledSegment(left+ux*0.5, top, left+ux*0.5, bot);
    break;
  }
  count++;
  if (count>7) {
    count=0;
    return true;
  }
  return false;
}


void drawScaledSegment(float x1, float y1, float x2, float y2) {

  if (printToConsole)
    println("("+x+","+y+":"+zoomLevel+")");
  if (writeToFile) {
    writer.write("("+x+","+y+":"+zoomLevel+")\n");
    writer.flush();
  }

  // if((x1!=Last.x)&&(y1!=Last.y)) {
  // jogging to a new position
  stroke(240, 0, 0, 24);
  line(Last.x*onScreenScale-2, Last.y*onScreenScale-2, x1*onScreenScale-2, y1*onScreenScale-2);
  if (serialOn && !rununtil) {
    machine.penUp();
    machine.sendVal(x1, y1, PEN_UP);
    machine.waitForOk();
  }
  //drawCrossHairs(x1*onScreenScale-2, y1*onScreenScale-2);
  //  }

  // drawing a line
  stroke(0);
  float d = dist(x1, y1, x2, y2);
  if (d>MAXLENGTH) {
    float lx=x1;
    float ly=y1;
    float steps = d/MAXLENGTH;
    for (int i=1;i<steps;i++) {
      float tx=lerp(x1, x2, (float)i/steps);
      float ty=lerp(y1, y2, (float)i/steps);
      line(lx*onScreenScale, ly*onScreenScale, tx*onScreenScale, ty*onScreenScale);
      if (serialOn && !rununtil) {  
        machine.penDown();
        machine.sendVal(tx, ty, PEN_DOWN);
        machine.waitForOk();
      }
      lx=tx;
      ly=ty;
    }
    line(lx*onScreenScale, ly*onScreenScale, x2*onScreenScale, y2*onScreenScale);

    if (serialOn && !rununtil) {  
      machine.penDown();
      machine.sendVal(x2, y2, PEN_DOWN);
      machine.waitForOk();
    }
  } 
  else {
    line(x1*onScreenScale, y1*onScreenScale, x2*onScreenScale, y2*onScreenScale);

    if (serialOn && !rununtil) {  
      machine.penDown();
      machine.sendVal(x2, y2, PEN_DOWN);
      machine.waitForOk();
    }
  }
  //drawCrossHairs(x2, y2);

  // store current position
  Last.x=x2;
  Last.y=y2;
};

void drawCrossHairs(float x, float y) {
  float px, py;
  px=x*onScreenScale;
  py=y*onScreenScale;
  //  stroke(128, 0, 0, 128);
  //  line(px-3, py, px+3, py);
  //  line(px, py-3, px, py+3);

  line(px-5, py-5, px+5, py+5);
  line(px+5, py-5, px-5, py+5);
};

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


// 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 (!On) {
    switch(key) {
    case '1':
      // top left
      //machine.sendVal(0, 0, 0);
      machine.sendVal(MARGIN, MARGIN, 0);
      break;
    case '2':
      // top right
      //machine.sendVal(PAGEW, 0, 0);
      machine.sendVal(PAGEW-MARGIN, MARGIN, 0);
      break;
    case '3':
      // lower right
      //machine.sendVal(PAGEW, PAGEH, 0);
      machine.sendVal(PAGEW-MARGIN, PAGEH-MARGIN, 0);
      break;
    case '4':
      // lower left
      //machine.sendVal(0, PAGEH, 0);
      machine.sendVal(MARGIN, PAGEH-MARGIN, 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);
      break;
    case 'd':
      machine.penDown();
      break;
    case 'u':
      machine.penUp();
      break;
    default:
      break;
    }
  }
}