/****************************************************************/
/* cs4496/7496: Computer Animation, Instructor: Jarek Rossignac */
/* Project P4: Morph                                            */
/*                                                              */
/* Solution by                                                  */
/*             Florian Hecht                                    */
/*                                                              */
/* November 2006                                                */
/****************************************************************/

class Twist {
  // parameters for the twist
  pt center;
  float a;
  
  // control points
  pt A;
  pt B;
  vec aDir;
  vec bDir;
  
  // control point for the area of influence
  vec sigmaVec;
  
  // are we doing a simple translation
  boolean simpleTrans = false;
  
  void createTwist(pt P1, vec p1Dir, pt P2, vec p2Dir) {
    A = P1.make();
    B = P2.make();
    aDir = p1Dir.make();
    bDir = p2Dir.make();
    
    vec U = aDir.make(); U.makeUnit();
    vec V = bDir.make(); V.makeUnit();
    a=angle(U,V);
    pt M = midPt(A,B);   
    float d=A.disTo(B);
    float h = d/2/tan(a/2);
    vec W = A.vecTo(B); W.makeUnit();
    vec L = W.left();   L.mul(h);
    M.addVec(L);
    
    center = M;
  }
  void drawTwist() {
    
    float d=A.disTo(B);
    float h = d/2/tan(a/2);
    vec W = A.vecTo(B); W.makeUnit();
    vec L = W.left();   L.mul(h);
    
    L.back();
    L.makeUnit(); 
    L.mul(abs(h+sq(d)/4/h)); 
    pt N = center.make(); N.addVec(L);
    
    noFill();
    stroke(0, 255, 0);
    if (!simpleTrans) {
      center.showCircle(15);
      drawArc(A, N, B, 5);
    } else {
      line(A.x, A.y, B.x, B.y);
    }
    
    fill(0, 0, 255);
    stroke(0);
    
    pt tmp;
   
    if (!simpleTrans) {
      tmp = A.make();
      tmp.addVec(aDir);
      line(A.x, A.y, tmp.x, tmp.y);
      tmp.showCircle(6);
    }
    A.showCircle(8);
    
    fill(255, 0, 0);
    
    if (!simpleTrans) {
      tmp = B.make();
      tmp.addVec(bDir);
      line(B.x, B.y, tmp.x, tmp.y);
      tmp.showCircle(6);
    }
    B.showCircle(8);
    
    noFill();
    stroke(0, 255, 0);
    float sigma = sigmaVec.norm();
    A.showCircle(2*sigma);
    tmp = A.make();
    tmp.addVec(sigmaVec);
    fill(0, 255, 0);
    tmp.showCircle(8);
  }
  pt applyTwist(float s, pt p) {
    pt P = p.make();
    
    if (!simpleTrans) {
      P.turnAround(center, -s*a);
    } else {
      P.addScaledVec(s, A.vecTo(B));
    }
    
    return P;
  }
  float influence(pt p) {
    float d = A.disTo(p);
    float sigma = sigmaVec.norm();
    float i = 1;
    
    float e = 2.718282;
    float t1 = 1.0f; //(1.0 / sigma*sqrt(TWO_PI));
    float t2 = - ((d*d) / (2*sigma*sigma));
      
    i = t1*pow(e, t2);
    
    return i;
  }
  // incluence round point B for the backward warp
  float influenceB(pt p) {
    float d = B.disTo(p);
    float sigma = sigmaVec.norm();
    float i = 1;
    
    float e = 2.718282;
    float t1 = 1.0f; //(1.0 / sigma*sqrt(TWO_PI));
    float t2 = - ((d*d) / (2*sigma*sigma));
      
    i = t1*pow(e, t2);
    
    return i;
  }
}




// the list of our twists
ArrayList twists = new ArrayList();


void resetTwists() {
  twists.clear();
  
  // create a default twist
  Twist tw = new Twist();
  
  pt p1 = new pt(width/2, height/2, 0);
  pt p2 = new pt(p1.x + 100, p1.y - 30, 0);
  vec v1 = new vec(0, -10, 0);
  vec v2 = new vec(0, -10, 0);
  tw.createTwist(p1, v1, p2, v2);
  tw.sigmaVec = new vec(100, 0, 0);
  tw.simpleTrans = true;
  
  twists.add(tw);
  
}

void drawTwists() {
  int c = twists.size();
  
  for (int i = 0; i < c; i++) {
    Twist tw = (Twist)twists.get(i);
    tw.drawTwist();
  }
}


/*

calc how the twists affect the mesh point at (x, y)

This is the work horse of the algorithm. It finds all the twists that influence this point
and applies them in order of their influence on the the point. The point is moved by each
twist and the results are blended together. The weight of each twist depends on it's influence
strength. We make sure that if the strongest twist has an influence of nearly 1 then the point
will completely follow that twist, but in case of a 0.5 and 0.5 influence of two twists we get
a nice interpolation between the two influences.

*/

pt getTwistedPoint(int x, int y, float t) {
  pt p = mesh[y*WIDTH + x].make();
  
  p.x *= width;
  p.y *= height;
  
  if (pinned[y*WIDTH + x])
    return p;
  
  int c = twists.size();
  Twist[] influencingTwists = new Twist[c];
  float[] weights = new float[c];
  float totalWeight = 0;
  float maxWeight = 0;
  int infTw = 0;
  
  for (int i = 0; i < c; i++) {
    Twist tw = (Twist)twists.get(i);
    
    float inf;
    if ( t >= 0)
      inf = tw.influence(p);
    else
      inf = tw.influenceB(p);
      
    if (inf < 0.00001)
      continue;
      
    influencingTwists[infTw] = tw;
    weights[infTw] = inf;
    totalWeight += inf;
    maxWeight = max(inf, maxWeight);
    infTw++;
  }
  
  if (totalWeight < 0.00001)
    return p;
  
  if (totalWeight < 1.0)
    totalWeight = 1.0;
    
  pt acc = new pt(0, 0, 0);
  float w = 1.0;
  for (int i = 0; i < infTw; i++) {
    // find the strongest influence
    Twist tw = influencingTwists[0];
    float highest = -1;
    int k = -1;
    for (int j = 0; j < infTw; j++) {
      if (weights[j] > highest) {
        tw = influencingTwists[j];
        k = j;
        highest = weights[j];
      }
    }
    
    // calc how the strongest twist would a affect the point alone
    pt tmp = tw.applyTwist(t, p);
    
    // add it to the accumulator with a weight
    acc = linear_ipol(acc, tmp, w);
    // update the weight
    w = w*(w - weights[k]);
    
    // if there is no weight left, the following twist will have no effect, so break
    if (w <= 0) {
      w = 0;
      break;
    }
    
    
    weights[k] = -1; // kill that influence
  }
  
  // the remaining weight is used to blend between the original position and the accumulator
  pt tp = linear_ipol(p, acc, 1 - w);
  
  if (/*infTw == 1 && */influencingTwists[0].simpleTrans) {
    if (maxWeight > 0.99)
      pinned[y*WIDTH + x] = true;
  } else {
    if (maxWeight > 0.9)
      pinned[y*WIDTH + x] = true;
  }
  
  return tp;
}

void releaseCenterPins() {
  for (int x = 2; x < WIDTH-2; x++) {
    for (int y = 2; y < HEIGHT-2; y++) {
      pinned[y*WIDTH + x] = false;
    }
  }
}

// calc the the displacement for each point on the mesh
void generateTwistedMesh(float t) {
  for (int y = 2; y < HEIGHT-2; y++) {
    for (int x = 2; x < WIDTH-2; x++) {
      pt p = getTwistedPoint(x, y, t);
      pt op = mesh[y*WIDTH + x].make();
      op.x *= width;
      op.y *= height;
      
      displacement[y*WIDTH + x].setTo((p.x - op.x) / width, (p.y - op.y) / height, 0);
    }
  }
}

pt getDispPoint(int x, int y) {
  int i = y*WIDTH + x;
  pt op = mesh[i];
  vec d = displacement[i];
  
  return new pt((op.x + d.x) * width, (op.y + d.y) * height, 0);
}

void drawTwistedMesh(float t, boolean constTransparency) {
  float du = (float)1.0 / (float)WIDTH;
  float dv = (float)1.0 / (float)HEIGHT;
  
  noStroke();
  if (drawSecond) {
    if (!constTransparency) {
      fill(255, 255*((float)1 - t));
    } else {
      fill(255, 128);
    }
  } else {
    fill(255, 255);
  }
  textureMode(NORMALIZED);
  
  // release the mesh
  releaseCenterPins();
  // calculate the deformation by applying the twists
  generateTwistedMesh(t);
  // relax them mesh a couple of times to smooth it
  for (int i = 0; i < 10; i++) {
    relaxMesh(true);
  }
  
  beginShape(TRIANGLES);
  texture(image1);
  
  for (int y = 0; y < HEIGHT-1; y++) {
    for (int x = 0; x < WIDTH-1; x++) {
      
      pt p1 = getDispPoint(x, y);
      pt p2 = getDispPoint(x, y+1);
      pt p3 = getDispPoint(x+1, y+1);
      pt p4 = getDispPoint(x+1, y);
      
      float u = x*du;
      float v = y*dv;
      
      p1.vertext(u     , v     );
      p2.vertext(u     , v + dv);
      p3.vertext(u + du, v + dv);
      
      p4.vertext(u + du, v     );
      p1.vertext(u     , v     );
      p3.vertext(u + du, v + dv);
    }
  }
  
  endShape();
  
}

void drawReverseTwistedMesh(float t, boolean doTwist) {
  float du = (float)1.0 / (float)WIDTH;
  float dv = (float)1.0 / (float)HEIGHT;
  
  
  noStroke();
  if (doTwist) {
    fill(255); //, 255*t);
  } else {
    fill(255, 128);
    t = 1;
  }
  textureMode(NORMALIZED);
  
  // release the mesh
  releaseCenterPins();
  // calculate the deformation by applying the twists
  generateTwistedMesh(-(1-t));
  // relax them mesh a couple of times to smooth it
  for (int i = 0; i < 10; i++) {
    relaxMesh(true);
  }
  
  beginShape(TRIANGLES);
  texture(image2);
  
  for (int y = 0; y < HEIGHT-1; y++) {
    for (int x = 0; x < WIDTH-1; x++) {
      
      pt p1 = getDispPoint(x, y);
      pt p2 = getDispPoint(x, y+1);
      pt p3 = getDispPoint(x+1, y+1);
      pt p4 = getDispPoint(x+1, y);
      
      float u = x*du;
      float v = y*dv;
      
      p1.vertext(u     , v     );
      p2.vertext(u     , v + dv);
      p3.vertext(u + du, v + dv);
      
      p4.vertext(u + du, v     );
      p1.vertext(u     , v     );
      p3.vertext(u + du, v + dv);
    }
  }
  
  endShape();
}

void drawTwistedGrid(float t) {
  stroke(0, 255, 0);
  noFill();
  
  beginShape(LINES);
  
  for (int y = 0; y < HEIGHT-1; y++) {
    for (int x = 0; x < WIDTH-1; x++) {
      pt p1 = getDispPoint(x, y);
      pt p2 = getDispPoint(x, y+1);
      pt p3 = getDispPoint(x+1, y+1);
      pt p4 = getDispPoint(x+1, y);
      
      p1.vert();
      p2.vert();
      
      p1.vert();
      p3.vert();
      
      p1.vert();
      p4.vert();
    }
  }
  
  endShape();
  
}


/* mouse control for the twists */

Twist selectedTwist;
int selectedPart = -1;

void checkTwistSelection() {
  int c = twists.size();
  
  for (int i = 0; i < c; i++) {
    Twist tw = (Twist)twists.get(i);
    if (checkSelection(tw))
      return;
  }
  
  // if we could find a twist at the mouse pos, we create a new twist
  selectedTwist = new Twist();
  twists.add(selectedTwist);
  
  // if CTRL is pressed we create a simple translation
  if (keyPressed && key==CODED && keyCode==CONTROL) {
    selectedTwist.createTwist(Mouse, new vec(0, -10, 0), Mouse, new vec(0, -10, 0));
    selectedTwist.simpleTrans = true;
  } else {
    selectedTwist.createTwist(Mouse, new vec(0, -10, 0), Mouse, new vec(10, -10, 0));
  }
  selectedTwist.sigmaVec = new vec(100, 0, 0);
  selectedPart = 1;
  
}

void releaseTwistSelection() {
  if (keyPressed && key==CODED && keyCode==CONTROL) {
    twists.remove(selectedTwist); 
  }
  selectedPart = -1;
  selectedTwist = null;
}

boolean checkSelection(Twist twist) {
  pt C = twist.A.make();
  C.addVec(twist.aDir);
  pt D = twist.B.make();
  D.addVec(twist.bDir);
  pt E = twist.A.make();
  E.addVec(twist.sigmaVec);
  
  if (twist.A.disTo(Mouse) < 7) {
    selectedTwist = twist;
    selectedPart = 0;
  } else if (twist.B.disTo(Mouse) < 7) {
    selectedTwist = twist;
    selectedPart = 1;
  } else if (C.disTo(Mouse) < 7) {
    selectedTwist = twist;
    selectedPart = 2;
  } else if (D.disTo(Mouse) < 7) {
    selectedTwist = twist;
    selectedPart = 3;
  } else if (E.disTo(Mouse) < 7) {
    selectedTwist = twist;
    selectedPart = 4;
  } else {
    selectedTwist = null;
    selectedPart = -1;
    
    return false;
  }
  
  if ((selectedPart == 2 || selectedPart == 3) && twist.simpleTrans) {
    selectedTwist = null;
    selectedPart = -1;
    
    return false;
  }
  
  return true;
}

void dragTwist() {
  
  if (selectedPart == -1)
    return;
  
  switch (selectedPart) {
    case 0: selectedTwist.A.setToMouse(); break;
    case 1: selectedTwist.B.setToMouse(); break;
    case 2: selectedTwist.aDir = selectedTwist.A.vecTo(Mouse); break;
    case 3: selectedTwist.bDir = selectedTwist.B.vecTo(Mouse); break;
    case 4: selectedTwist.sigmaVec = selectedTwist.A.vecTo(Mouse); break;
  }
  
  selectedTwist.createTwist(selectedTwist.A, selectedTwist.aDir, selectedTwist.B, selectedTwist.bDir);
}