import Jama.util.*;
import Jama.*;

Mesh g_mesh;
Slider g_slides[];

int MODE_FE = 0;
int g_mode = 0;

PFont fontA; // display font

void initFEMode() {
  g_slides = new Slider[4];
  //Slider(float xpercent, float iy, float xmin, float xmax, float itarget, float irad)
  g_slides[0] = new Slider(.1, 100, 450, 495, 5, .05, 1.5);
  g_slides[1] = new Slider(.1, 130, 450, 495, 5, .05, 1.5);
  g_slides[2] = new Slider(.1, 160, 450, 495, 5, .05, 1.5);
  g_slides[3] = new Slider(.1, 190, 450, 495, 5, .05, 1.5);
}

boolean pressedLast[];
boolean keyHit(char c) {
  //println ("pressedLast[c] == " + pressedLast[c]);
  if (keyPressed && key == c) {
    //println("down");
    if (!pressedLast[c]) {
      pressedLast[c] = true;
      return true;
    } else {
      return false;
    }
  }
  pressedLast[c] = false;
  return false;
}

class Mesh {
  ArrayList nodes;
  ArrayList tris;
  PVector cm;
  
  Node n(int i) {
    return (Node)nodes.get(i);
  }
  Tri t(int i) {
    return (Tri)tris.get(i);
  }
  
  
  void drawWire() {
    for (int ti = 0; ti < tris.size(); ti++) {
      Tri t = (Tri)tris.get(ti);
      stroke(255);
      for (int i = 2, j = 0; j < 3; i = j++) {
        line(n(t.ni[i]).p.x, n(t.ni[i]).p.y, n(t.ni[j]).p.x, n(t.ni[j]).p.y);
      }
    }
  }

  Mesh(String file) {
    nodes = new ArrayList();
    tris = new ArrayList();
    
    String line;
    BufferedReader reader;
    reader = createReader(file);
    

    
    try {
      while ( (line = reader.readLine()) != null ) {
        println(line);
        String[] sa = split(line, ' ');
        if (sa.length == 0)
          continue;
        println("sa length = " + sa.length);
        if (sa[0].length() > 0 && sa[0].charAt(0) == 'n' && sa.length >= 3) {
          nodes.add(new Node(float(sa[1]), float(sa[2])));
          println("added node ... " + nodes.size());
        }
        if (sa[0].length() > 0 && sa[0].charAt(0) == 't' && sa.length >= 4) {
          tris.add(new Tri(this, int(sa[1]), int(sa[2]), int(sa[3])));
          println("added tri ... " + tris.size());
        }
      }
      reader.close();
    } catch (IOException e) {
      e.printStackTrace();
    }
    
    // masses start at zero; add the area of each tri to the masses
    // density is one everywhere because I said so.
    for (int ti = 0; ti < tris.size(); ti++) {
      float mass = .001 * t(ti).area/3;
      for (int ii = 0; ii < 3; ii++) {
        n(t(ti).ni[ii]).mass += mass;
      }
    }
    
    cm = new PVector(0,0);
    float count = 0;
    for (int i = 0; i < nodes.size(); i++) {
      cm.add(n(i).m);
      count++;
    }
    cm.mult(1.0/count);
  }
  
  float dsq(int i, float x, float y) {
    float dx = n(i).p.x-x, dy = n(i).p.y-y;
    return dx*dx+dy*dy;
  }
  
  int getClosest(float x, float y) {
    if (nodes.size() == 0) {
      return -1;
    }
    int closest = 0;
    float bestdsq = dsq(0,x,y);
    for (int i = 1; i < nodes.size(); i++) {
      float dsq = dsq(i,x,y);
      if (dsq < bestdsq) {
        bestdsq = dsq;
        closest = i;
      }
    }
    return closest;
  }
  
  void setForces(float fx, float fy) {
    for (int i = 0; i < nodes.size(); i++) {
      n(i).f = new PVector(fx,fy);
    }
  }
  
  void addGrav(float gy) {
    for (int i = 0; i < nodes.size(); i++) {
      n(i).f.y += gy*n(i).mass;
    }
  }
  
  void addMassDamping(float k) {
    for (int i = 0; i < nodes.size(); i++) {
      Node n = n(i);
      n.f.x -= n.mass*n.v.x*k;
      n.f.y -= n.mass*n.v.y*k;
    }
  }
  
  void addSpring(int ni, float k, float kd, float maxd, float sx, float sy) {
    PVector sep = new PVector(sx-n(ni).p.x,sy-n(ni).p.y);
    float magsq = sep.x*sep.x+sep.y*sep.y;
    if (magsq > maxd*maxd) {
      float mag = sqrt(magsq);
      sep.mult(maxd/mag);
    }
    // add spring force
    n(ni).f.x += (sep.x)*(k);
    n(ni).f.y += (sep.y)*(k);
    // add stiffness proportional damping
    
    float dotp = -sep.dot(n(ni).v);
    sep.mult(dotp/magsq);
    n(ni).f.add(sep);
  }
  
  void addTriFEForces(float lambda, float mu, float k3, float k4) {
    for (int i = 0; i < tris.size(); i++) {
      t(i).fe.addForcesToNodes(lambda, mu, k3, k4);
    }
  }
  
  void addPenaltyBox(float xl, float xr, float yf, float yc, float kp) {
    for (int i = 0; i < nodes.size(); i++) {
      float y = n(i).p.y;
      float x = n(i).p.x;
      if (y > yf) {
        n(i).f.y += (yf-y)*kp;
      } else if (y < yc) {
        n(i).f.y += (yc-y)*kp;
      }
      if (x > xr) {
        n(i).f.x += (xr-x)*kp;
      } else if (x < xl) {
        n(i).f.x += (xl-x)*kp;
      }
    }
  }
  
  void integrateForcesExplicit(float dt, float damp) { // here damp is for that 'scale down v' damping
    for (int i = 0; i < nodes.size(); i++) {
      PVector a = PVector.mult(n(i).f, 1.0/n(i).mass);
      //println("a = " + a.x + ", " + a.y);
      n(i).v.x += dt*a.x; n(i).v.y += dt*a.y;
      n(i).v.x *= damp; n(i).v.y *= damp;
      float dtsq = dt*dt;
      n(i).p.x += n(i).v.x * dt + a.x * dtsq;
      n(i).p.y += n(i).v.y * dt + a.y * dtsq;
    }
  }
  
  void integrateShapeMatching(float dt, float a) {
    // calc current centroid
    float count = 0;
    PVector cmx = new PVector(0,0);
    for (int i = 0; i < nodes.size(); i++) {
      cmx.add(n(i).p);
      count++;
    }
    cmx.mult(1.0/count);
    Matrix Apq = new Matrix(2,2);
    float p[] = new float[2];
    float q[] = new float[2];
    for (int i = 0; i < nodes.size(); i++) {
      p[0] = n(i).p.x-cmx.x;
      p[1] = n(i).p.y-cmx.y;
      q[0] = n(i).m.x-cm.x;
      q[1] = n(i).m.y-cm.y;
      for (int ii = 0; ii < 2; ii++) {
        for (int jj = 0; jj < 2; jj++) {
          Apq.set(ii,jj,p[ii]*q[jj]); // * mass also but eh
        }
      }
    }
    SingularValueDecomposition svd = Apq.svd();
    Matrix R = svd.getU().times(svd.getV().transpose());
    //Matrix R = svd.getV().times(svd.getU().transpose());
    double svs[] = svd.getSingularValues();
    for (int i = 0; i < svs.length; i++) {
       println("sv " + i + ": " + svs[i]);
    }
    //QRDecomposition qr = new QRDecomposition(Apq); 
    //Matrix R = qr.getQ();
    
    for (int i = 0; i < nodes.size(); i++) {
      PVector acc = PVector.mult(n(i).f, 1.0/n(i).mass);
      float qx = n(i).m.x-cm.x;
      float qy = n(i).m.y-cm.y;
      //float gx = qx+cmx.x;//(float)(R.get(0,0)*qx + R.get(0,1)*qy + cmx.x);
      //float gy = qy+cmx.y;//(float)(R.get(1,0)*qx + R.get(1,1)*qy + cmx.y);
      float gx = (float)(-R.get(0,0)*qx + R.get(0,1)*qy + cmx.x);
      float gy = (float)(-R.get(1,0)*qx + R.get(1,1)*qy + cmx.y);
      line(gx,gy,n(i).p.x,n(i).p.y);
      fill(255,0,255);
      ellipse(gx,gy,3,3);
     // n(i).v.x += dt*acc.x + a*(gx-n(i).p.x)*.001;//a * (gx-n(i).p.x) / dt + dt * acc.x;
     // n(i).v.y += dt*acc.y + a*(gy-n(i).p.y)*.001;//a * (gy-n(i).p.y) / dt + dt * acc.y;
      n(i).v.x += a * (gx-n(i).p.x) / dt + dt * acc.x;
      n(i).v.y += a * (gy-n(i).p.y) / dt + dt * acc.y;
      n(i).v.mult(.99);
      n(i).p.x += dt * n(i).v.x;
      n(i).p.y += dt * n(i).v.y;
    }
  }
}

class Node {
  Node(float mx, float my) {
    m = new PVector(mx, my);
    p = new PVector(mx,my);
    v = new PVector(0,0);
    mass = 0;
  }
  
  PVector m,p,v;
  float mass;
  PVector f;
}

class FEdata {
  Matrix Beta;
  Matrix P, V;
  Matrix eps, nu, sigma;
  Mesh m;
  Tri t;
  
  FEdata(Mesh m, Tri t) {
    this.m = m;
    this.t = t;
    // Beta stays constant ... compute once here
    Matrix Betai = new Matrix(3,3);
    for (int ii = 0; ii < 3; ii++) {
      PVector mm = t.n(ii).m;
      Betai.set(0, ii, mm.x);
      Betai.set(1, ii, mm.y);
      Betai.set(2, ii, 1.0);
    }
    Beta = Betai.inverse();
    // just allocate space for the rest, they change with time
    P = new Matrix(2,3);
    V = new Matrix(2,3);
    eps = new Matrix(2,2);
    nu = new Matrix(2,2);
    sigma = new Matrix(2,2);
  }
  
  float dotm(Matrix a, int i, Matrix b, int j) {
    return (float)(a.get(0,i)*b.get(0,j) + a.get(1,i)*b.get(1,j));
  }
  
  void addForcesToNodes(float lambda, float mu, float k3, float k4) {
    for (int ii = 0; ii < 3; ii++) {
      PVector p = t.n(ii).p;
      PVector v = t.n(ii).v;
      P.set(0, ii, p.x);
      P.set(1, ii, p.y);
      V.set(0, ii, v.x);
      V.set(1, ii, v.y);
    }
    Matrix dx = P.times(Beta); // dxdu
    Matrix dv = V.times(Beta); // dvdu
    // pop eps and nu
    for (int i = 0; i < 2; i++) {
      for (int j = 0; j < 2; j++) {
        float d = 1; // kronecker delta
        if (i != j)
          d = 0;
        eps.set(i,j,dotm(dx,i,dx,j) - d);
        nu.set(i,j,dotm(dx,i,dv,j)+dotm(dv,i,dx,j));
      }
    }
    // pop sigma
    for (int i = 0; i < 2; i++) {
      for (int j = 0; j < 2; j++) {
        float acc = 0;
        if (i == j) {
          for (int k = 0; k < 2; k++) {
            acc += lambda * eps.get(k,k);
            acc += k3 * nu.get(k,k);
          }
        }
        acc += 2*mu*eps.get(i,j);
        acc += 2*k4*nu.get(i,j);
        sigma.set(i,j,acc);
      }
    }
    // add forces
    for (int ii = 0; ii < 3; ii++) {
      for (int jj = 0; jj < 3; jj++) {
        float acc = 0;
        for (int k = 0; k < 2; k++) {
          for (int l = 0; l < 2; l++) {
            acc += Beta.get(jj,l) * Beta.get(ii,k) * sigma.get(k,l);
          }
        }
        t.n(ii).f.x += -.5*t.area*acc*t.n(jj).p.x;
        t.n(ii).f.y += -.5*t.area*acc*t.n(jj).p.y; 
      }
    }
  }
}

class Tri {
  int ni[];
  float area;
  FEdata fe;
  Mesh m;
  
  Tri(Mesh m, int i, int j, int k) {
    this.m = m;
    ni = new int[3];
    ni[0] = i; ni[1] = j; ni[2] = k;
    PVector d1 = PVector.sub(m.n(j).m, m.n(i).m);
    PVector d2 = PVector.sub(m.n(k).m, m.n(i).m);
    area = .5 * (d1.cross(d2)).mag();
    fe = new FEdata(m, this);
  }
  
  Node n(int ii) {
    return m.n(ni[ii]);
  }
}

void setup() {
  size(500, 500);
  frameRate(30);

  g_mesh = new Mesh("mesh2.txt");
  pressedLast = new boolean[256];
  
  initFEMode();
    
  fontA = createFont("FFScala", 15);
  textFont(fontA, 10);
}

float lastMx, lastMy;

void draw() {
  background(0);
  
  if (keyHit('1'))
    g_mesh = new Mesh("mesh.txt");
  if (keyHit('2'))
    g_mesh = new Mesh("mesh2.txt");
  if (keyHit('3'))
    g_mesh = new Mesh("mesh3.txt");
  

  int closeInd = g_mesh.getClosest(mouseX, mouseY);
  Node closeNode = g_mesh.n(closeInd);   // assume closeNode always exists b/c zero node cases are dumb
  
  boolean overAny = false;
  for (int i = 0; i < g_slides.length; i++) {
    if (g_slides[i].update(6, mouseX-lastMx, mouseY-lastMy))
      overAny = true;
  }
  
  for (int i = 0; i < 100; i++) {
    g_mesh.setForces(0,0);
    g_mesh.addGrav(.1);
    if (mousePressed && !overAny) {
//      g_mesh.addSpring(closeInd, 3, 2, 15, mouseX, mouseY);
      g_mesh.addSpring(closeInd, .4, 2, 15, mouseX, mouseY);
    }
    g_mesh.addMassDamping(.01);
    //g_mesh.addMassDamping(.01);
    g_mesh.addTriFEForces(g_slides[0].getVal(),
                          g_slides[1].getVal(),
                          g_slides[2].getVal(),
                          g_slides[3].getVal());
    
    g_mesh.addPenaltyBox(10, 440, 450, 10, 10);
    g_mesh.integrateForcesExplicit(.1, 1);
    //g_mesh.integrateShapeMatching(.8, 1);
  }
  
  g_mesh.drawWire();
  
  if (mousePressed)
    fill(255,255,0);
  if (!overAny)
    ellipse(closeNode.p.x, closeNode.p.y, 10,10);
  fill(255);
  //draw the penalty box
  line(10,10,440,10); // ceiling 
  line(10,450,440,450); // floor
  line(10,10,10,450); // left wall
  line(440,10,440,450); // right wall
  
  for (int i = 0; i < g_slides.length; i++) {
    g_slides[i].display();
  }
  
  lastMx = mouseX;
  lastMy = mouseY;
}