//
// gridding off space available in the image plane
// repeatibility
// accuracy
// precision
//
// rtwomey@u.washington.edu
//
//---------------------------------------------------
// 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 = 17.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 = 86.5;//81.0;//82.0;// 58.5225650924496; // 108
float HOMEB = 86.5;//81.0;//82.0;//56.02580320709378; // 108

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

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; // what zoom level
int x=0; // which grid x position
int y=0;  // which grid y position
int count=0; // which stroke
int s=0;  // which segment of stroke
int inc=1;


int state=START;
int dir=FORWARD;
float segs=0.0; // how many segments in current line
boolean segDone=false;

float lx, ly; // fractional x,y positions
PVector Endpoint; // end of current line
PVector Last; // end of last line

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

// fast forward to doResume previous job
boolean doResume=false;
int runtoX=0;
int runtoY=3;
int runtoLine=2;
int runtoZoom=2;
float startX=65.20017;
float startY=90.21675;



//------------------------------------
// main program
void setup() {
  // graphics
  size(1237,900);
  
  // load a font for text display
  PFont font;
  font = loadFont("Serif-24.vlw"); 
  textFont(font, 12); 

  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));
  if(doResume) {
    HOMEA = startX;
    HOMEB = startY;
  }
  
  if (printToConsole)
    println("(HOME: "+HOMEA + " " +HOMEB+")");

  Last = new PVector(HOMEA, HOMEB);
  Endpoint = new PVector(Last.x, Last.y);
  
  println("(at "+Last.x+","+Last.y+" relative to PAGE(0,0))");

  noCursor();
  
  On = true;

}


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 (!doResume)return true;

  if ((x==runtoX) && (y==runtoY) && (zoomLevel==runtoZoom) && (count==runtoLine)) {
    println("(found doResume point)");
    serialOn=true;
    doResume=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;
    //println("(ux "+ux+" uy "+uy+" scale "+scale+" count "+count+")");
    
    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;
      }
    }
    if (isLineDone()) {
     count++;
     println("(line "+(count-1)+" done. current line in pattern is line "+count+")");
     if (count>2) {
       println("(pattern "+x+","+y+":"+scale+" done )");
        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;
  }

  if (isLineDone()) {
    count++;
    if (count>7) {
      count=0;
      return true;
    }
  }

  return false;

  //  count++;
  //  if (count>7) {
  //    count=0;
  //    return true;
  //  }
  //  return false;
}

boolean isLineDone() {
  //println("(checking segment "+(s-1)+" of line "+count+", at pattern "+x+","+y+"...)");
  if (s>=segs){
    // finish multi-part line
    if(segs>1) {
      
      line(lx*onScreenScale, ly*onScreenScale, Endpoint.x*onScreenScale, Endpoint.y*onScreenScale);
      //println("finished long line");
      if (serialOn && !doResume) {  
//        machine.penDown();
//        machine.sendVal(Endpoint.x, Endpoint.y, PEN_DOWN);
//        machine.waitForOk();
      }
    };
    
    s=0;
    // store current position
    segDone=true;
    
    //Last=Endpoint;
        Last.x=Endpoint.x;
        Last.y=Endpoint.y;
    //println("(done.)");
    return true;
  }
  //println("(not done.)");
  return false;
}

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

  if (s==0) {
    // starting a new line
    if (printToConsole)
      //println("("+s+":"+count+":"+x+","+y+":"+zoomLevel+")");
      println("("+zoomLevel+":"+x+","+y+":"+count+":"+s+")");

    // jog 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 && !doResume) {
//      machine.penUp();
//      machine.sendVal(x1, y1, PEN_UP);
//      machine.waitForOk();
    }
    //println("\tdid jog");
    
    segDone=false;
    float d = dist(x1, y1, x2, y2);
    if (d>MAXLENGTH) {
      // line length needs to be broken down
      lx=x1;
      ly=y1;
      segs = d/MAXLENGTH;
      s=1;
      Endpoint.x=x2;
      Endpoint.y=y2;
      //println("\tthis is a long line");
    } 
    else {
      // line length is ok, draw it
      stroke(0);
      line(x1*onScreenScale, y1*onScreenScale, x2*onScreenScale, y2*onScreenScale);
      //println("(this is a short line)");
      //println("\tdrew short line");
      if (serialOn && !doResume) {  
//        machine.penDown();
//        machine.sendVal(x2, y2, PEN_DOWN);
//        machine.waitForOk();
      }
      
      // signal we drew a line, and we are done
      s=1;
      segs=1;
      segDone=true;
      
      // store current position
      Endpoint.x=x2;
      Endpoint.y=y2;
      Last.x=x2;
      Last.y=y2;
    }
  }

  if ((s>0) && !segDone) { 
    // continuing a line segment
    if (printToConsole)
      println("("+zoomLevel+":"+x+","+y+":"+count+":"+s+")");

    // draw segment
    stroke(0);
    float tx=lerp(x1, x2, (float)s/segs);
    float ty=lerp(y1, y2, (float)s/segs);

    line(lx*onScreenScale, ly*onScreenScale, tx*onScreenScale, ty*onScreenScale);
    if (serialOn && !doResume) {  
//      machine.penDown();
//      machine.sendVal(tx, ty, PEN_DOWN);
//      machine.waitForOk();
    }
    //println("\tdrew segment");
    lx=tx;
    ly=ty;

    s++;
  }
}

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