// Pantograph
// By Patrick Hebron

Pantograph pgraph;
ArrayList drawPts, drawSty;

void setup() {
  size(800, 800);
  background(0);
  strokeWeight(1);

  pgraph = new Pantograph(width/2, height/2, 2);
  drawPts = new ArrayList();
  drawSty = new ArrayList();
}

void draw() {
  background(0);

  if(mousePressed == true) {
    pgraph.newStylusPos(mouseX, mouseY);
    pgraph.display();
    
    drawPts.add(new PPoint(pgraph.penPt.X(), pgraph.penPt.Y()));
    drawSty.add(new PPoint(mouseX, mouseY));
    int numDrawPts = drawPts.size();
    if(numDrawPts > 0) {
      for(int i = 0; i < numDrawPts-1; i++) {
        PPoint currDraw = (PPoint) drawPts.get(i);
        PPoint nextDraw = (PPoint) drawPts.get(i+1);
        
        PPoint currDrawS = (PPoint) drawSty.get(i);
        PPoint nextDrawS = (PPoint) drawSty.get(i+1);
        
        if(currDraw.X() != -999 && nextDraw.X() != -999) {
          stroke(255,0,0);
          line(currDraw.X(), currDraw.Y(), nextDraw.X(), nextDraw.Y());
          stroke(0,255,0);
          line(currDrawS.X(), currDrawS.Y(), nextDrawS.X(), nextDrawS.Y());
        }
      }
    } 
    else {
      drawPts.add(new PPoint(pgraph.penPt.X(), pgraph.penPt.Y()));
      drawSty.add(new PPoint(mouseX, mouseY));
    }
  }
  else {
    pgraph.display();
    PPoint lastPoint = new PPoint(-999,-999);
    PPoint lastPointS = new PPoint(-999,-999);
    int numDrawPts = drawPts.size();
    if(numDrawPts > 0) {
      for(int i = 0; i < numDrawPts-1; i++) {
        PPoint currDraw = (PPoint) drawPts.get(i);
        PPoint nextDraw = (PPoint) drawPts.get(i+1);
        
        PPoint currDrawS = (PPoint) drawSty.get(i);
        PPoint nextDrawS = (PPoint) drawSty.get(i+1);
        
        if(currDraw.X() != -999 && nextDraw.X() != -999) {
          stroke(255,0,0);
          line(currDraw.X(), currDraw.Y(), nextDraw.X(), nextDraw.Y());
          stroke(0,255,0);
          line(currDrawS.X(), currDrawS.Y(), nextDrawS.X(), nextDrawS.Y());
        }
        
        if(i == numDrawPts-2) {
          lastPoint = nextDraw;
          lastPointS = nextDrawS;
        }
      }
      stroke(255);
      
      if(lastPoint.X() != -999) {
        drawPts.add(new PPoint(-999, -999));
        drawSty.add(new PPoint(-999, -999));
      }
    } 
  }
}

class Pantograph {
  // Bar Lengths
  float shortBar = 30;
  float midBar = 60;
  float longBar = 90;
  float pantoSize;
  // Pantograph Junctions
  PPoint anchorPt, ancStyPt, ancConnPt, stylusPt, connPt, penPt;

  Pantograph(float aX, float aY, float pS) {
    pantoSize = pS;
    shortBar *= pantoSize;
    midBar *= pantoSize;
    longBar *= pantoSize;

    anchorPt = new PPoint(aX, aY);
    ancStyPt = new PPoint(aX, aY+midBar);
    ancConnPt = new PPoint(aX, aY+midBar*2);
    stylusPt =  new PPoint(aX+longBar, aY+midBar);
    connPt =  new PPoint(aX+longBar, aY+midBar*2);
    penPt = new PPoint(aX+longBar+shortBar, aY+midBar*2);
  }  

  void display() { 
    stroke(255);
    fill(255);
    line(anchorPt.X(), anchorPt.Y(), ancStyPt.X(), ancStyPt.Y());
    line(ancStyPt.X(), ancStyPt.Y(), ancConnPt.X(), ancConnPt.Y());
    line(ancStyPt.X(), ancStyPt.Y(), stylusPt.X(), stylusPt.Y());
    line(stylusPt.X(), stylusPt.Y(), connPt.X(), connPt.Y());
    line(ancConnPt.X(), ancConnPt.Y(), connPt.X(), connPt.Y());
    line(connPt.X(), connPt.Y(), penPt.X(), penPt.Y());
    
    ellipse(anchorPt.X(), anchorPt.Y(), 2, 2);
    ellipse(ancStyPt.X(), ancStyPt.Y(), 2, 2);
    ellipse(ancConnPt.X(), ancConnPt.Y(), 2, 2);
    ellipse(connPt.X(), connPt.Y(), 2, 2);
    stroke(255,0,0);
    fill(255,0,0);
    ellipse(penPt.X(), penPt.Y(), 2, 2);
    stroke(0,255,0);
    fill(0,255,0);
    ellipse(stylusPt.X(), stylusPt.Y(), 2, 2);
  }

  void newStylusPos(int nx, int ny) {
      stylusPt = new PPoint(nx, ny);
      ancStyPt = circInter(anchorPt.X(), anchorPt.Y(), stylusPt.X(), stylusPt.Y(), midBar, longBar); 
      ancConnPt = extendLine(anchorPt.X(), anchorPt.Y(), ancStyPt.X(), ancStyPt.Y(), midBar);
      connPt = circInter(ancConnPt.X(), ancConnPt.Y(), stylusPt.X(), stylusPt.Y(), longBar, midBar);
      penPt = extendLine(ancConnPt.X(), ancConnPt.Y(), connPt.X(), connPt.Y(), shortBar);
  }
}

class PPoint {
  float X, Y;

  PPoint(float x, float y) {
    X = x;
    Y = y;
  }

  float X() {
    return X;
  }
  float Y() {
    return Y;
  }
}

PPoint circInter(float cenAx, float cenAy, float cenBx, float cenBy, float radA, float radB) {
  float e = cenBx - cenAx;
  float f = cenBy - cenAy;
  float p = sqrt(sq(e) + sq(f)); 
  float k = (sq(p) + sq(radA) - sq(radB))/(2*p);

  float x = cenAx + (e*k)/p - (f/p)*sqrt(sq(radA) - sq(k));
  float y = cenAy + (f*k)/p + (e/p)*sqrt(sq(radA) - sq(k));
    // float x = cenAx + (e*k)/p + (f/p)*sqrt(sq(radA) - sq(k)); 
    // float y = cenAy + (f*k)/p - (e/p)*sqrt(sq(radA) - sq(k));

  PPoint forReturn =  new PPoint(x,y);
  return forReturn;
}

float ptDistance(PPoint a, PPoint b) {
  float deltaX = a.X() - b.X();
  float deltaY = a.Y() - b.Y();

  return sqrt(sq(deltaX) + sq(deltaY));
}

PPoint extendLine(float x1, float y1, float x2, float y2, float extension) {
  float deltaX = x2-x1;
  float deltaY = y2-y1;
  float deltaH = sqrt(sq(deltaX)+sq(deltaY));
  float ang = asin(deltaX/deltaH);
  float newdeltX = sin(ang)*extension;
  float newdeltY = sqrt(sq(extension)-sq(newdeltX));
  if(deltaY < 0)
    newdeltY *= -1;
  
  return new PPoint(x2+newdeltX,y2+newdeltY);
}