/*
TODO:
   - Various shtick and example sequences
   - Show string of the sequence we are recording
   - Sidestepping
   - Flip/backflip, spin, cartwheel
   - One-cycle-only moves
   - Add incidental sounds
       - Footfalls
       - Peep noises
   - Feet should tilt properly when not on the ground
       - use height to interpolate between vertical and oriented peep to leg
   - Things to customize:
       - Leg thickness
       - Foot size, shape
       - Feathers or arms on side
       - Feather or crest on top
       - Tail
   - Try adding a twist to the midsection, to better follow pollymorph
   - More organic/continuous body and limb shape
   - Try hitting tiles with feet in a dance step combination
   - Drag feet with cursor
*/

import render.*;

public class Peep extends PollymorphActor
{
   Material bodyColor, mouthColor, eyelidColor, eyeColor, white, black, red;
   Geometry peep, root, body, head, mouth, mouthCavity, leftEyelid, rightEyelid;
   Geometry eyes, leftEye, rightEye, leftIris, rightIris, leftPupil, rightPupil;
   Geometry leftThigh, rightThigh, leftLeg, rightLeg, leftFoot, rightFoot;
   Geometry leftKnee, rightKnee;
   Geometry leftCalf, rightCalf;
   Geometry mouthRef, mouthCavityRef;
   Geometry exoskeleton;
   double FH = .1; // FOOT HEIGHT

////////////////////////////////////////////////////
/////////                         //////////////////
/////////  INITIALIZE EVERYTHING  //////////////////
/////////                         //////////////////
////////////////////////////////////////////////////

   public Peep(RenderApplet applet) {
      super(applet);
      Geometry h;

      polly.setSize(2);

      // ALL THE DIFFERENT COLORS AND MATERIALS

      mouthColor = new Material();
      mouthColor.setAmbient(.4,0,.1);
      mouthColor.setDiffuse(.0,.0,0);

      eyelidColor = new Material();
      eyelidColor.setAmbient(.4,0,.1);
      eyelidColor.setDiffuse(.5,.4,0,.5);
      eyelidColor.setSpecular(.5,.5,.5, 10);
      eyelidColor.setDoubleSided(true);

      eyeColor = new Material();
      eyeColor.setAmbient(.2,.1,.4);
      eyeColor.setDiffuse(.1,.2,.4);
      eyeColor.setSpecular(1,1,1, 20);

      white = new Material();
      white.setAmbient(.6,.6,.6);
      white.setDiffuse(.4,.4,.4);
      white.setSpecular(1,1,1, 30);

      black = new Material();
      black.setAmbient(0,0,0);
      black.setDiffuse(0,0,0);
      black.setSpecular(0,0,0, 1);

      red = new Material();
      red.setAmbient(1,0,0);
      red.setDiffuse(0,0,0);
      red.setSpecular(0,0,0, 1);

      // THE FEET ARE INDEPENDENT

      leftFoot  = add();
      rightFoot = add();

      makeFoot(leftFoot, -.04);
      makeFoot(rightFoot, .04);

      // EVERYTHING ELSE IS ROOTED AT THE PEEP BODY

      peep = add();

      leftThigh  = peep.add();
      rightThigh = peep.add();
      root       = peep.add();

      // THE THIGHS AND LEGS

      h = leftThigh.add().globe(16,8);
      push();
         rotateX(Math.PI/2);
         scale(HW,HW,HW);
         transform(h);
      pop();


      leftThigh.add().globe(8,8,0,1,.5,.8);

      leftLeg = leftThigh.add();

      leftKnee = leftLeg.add();
      leftCalf = leftLeg.add();

      push();
         rotateX(Math.PI/2);
         transform(leftKnee);
         transform(leftCalf);
      pop();

      leftKnee.add().globe(8,8);
      leftCalf.add().globe(8,8,0,1,.5,1);


      h = rightThigh.add().globe(16,8);
      push();
         rotateX(Math.PI/2);
         scale(HW,HW,HW);
         transform(h);
      pop();

      rightThigh.add().globe(8,8,0,1,.5,.8);

      rightLeg = rightThigh.add();

      rightKnee = rightLeg.add();
      rightCalf = rightLeg.add();

      push();
         rotateX(Math.PI/2);
         transform(rightKnee);
         transform(rightCalf);
      pop();

      rightKnee.add().globe(8,8);
      rightCalf.add().globe(8,8,0,1,.5,1);


      // THE MAIN BODY

      body = root.add();

      head           = body.add().add().globe(24,12,0,1,0,.8);
      mouth          = body.add().globe(24,8,0,1,.8,.94);
      mouthRef       = body.add().globe(24,8,0,1,.8,.94);
      mouthCavity    = body.add().globe(24,3,0,1,.8,1);
      mouthCavityRef = body.add().globe(24,3,0,1,.8,1);

      for (int i = 0 ; i < mouthCavityRef.vertices.length ; i++) {
         double v[] = mouthCavityRef.vertices[i];
	 v[2] = 1.59 - v[2];
      }

      push();
         scale(0,0,0);
         transform(mouthRef);
         transform(mouthCavityRef);
      pop();


      // THE EYES

      push();

         translate(0,TY,TZ);

         leftEyelid  = makeEyelid(root, .19 * Math.PI);
         rightEyelid = makeEyelid(root,-.19 * Math.PI);

         eyes = root.add();

         leftEye  = makeEyeball(eyes, .19 * Math.PI);
         rightEye = makeEyeball(eyes,-.19 * Math.PI);
      pop();

      leftIris = leftEye.add().ball(8);
      leftPupil = makePupil(leftIris);

      rightIris = rightEye.add().ball(8);
      rightPupil = makePupil(rightIris);

      // BUILD THE OPTIONALLY VISIBLE POLLYMORPHIC EXOSKELETON

      exoskeleton = add().setMaterial(red);
      for (int i = 0 ; i < 6 ; i++)
         exoskeleton.add().ball(4);

      setColor(3);
   }

   Geometry makeEyelid(Geometry root, double angle) {
      Geometry eyelid = root.add().globe(24,8,0,1,0,.68);
      push();
         rotateY(angle);
         rotateX(-.035 * Math.PI);
         translate(0,0,.75);
         rotateX(-.5);
         rotateZ(.2);
         scale(.5,.5,.2);
         transform(eyelid);
      pop();
      return eyelid;
   }

   Geometry makeEyeball(Geometry eyes, double angle) {
      Geometry eyeball = eyes.add().ball(8);
      push();
         rotateY(angle);
         rotateX(-.055 * Math.PI);
         translate(0,0,.85);
         scale(.47,.45,.2);
         transform(eyeball);
      pop();
      return eyeball;
   }

   Geometry makePupil(Geometry iris) {
      Geometry pupil = iris.add().ball(8);
      push();
         translate(0,0,.85);
         scale(.65,.65,.25);
         transform(pupil);
      pop();
      return pupil;
   }

   void makeFoot(Geometry parent, double turn) {
      push();
         rotateY(turn);
         push();
            rotateY(-.2);
	    transform(makeToe(parent));
         pop();

         transform(makeToe(parent));

         push();
            rotateY(.2);
	    transform(makeToe(parent));
         pop();
      pop();
   }

   Geometry makeToe(Geometry parent) {
      Geometry toe = parent.add();
      Geometry toetop = toe.add().globe(16,8,0,1,.25,1);
      Geometry toepad = toe.add().disk(16);
      push();
         translate(0,0,.55);
         scale(.15,.15,.6);
         push();
            rotateX(-Math.PI/2);
            transform(toetop);
         pop();
         push();
            translate(0,-.707,0);
            rotateX(Math.PI/2);
            scale(.707,.707,1);
            transform(toepad);
         pop();
      pop();
      return toe;
   }

   void setColor(int i) {
      setMaterial(colors[i]);

      leftEye.setMaterial(white);
      rightEye.setMaterial(white);

      mouthCavity.setMaterial(mouthColor);

      leftIris.setMaterial(eyeColor);
      rightIris.setMaterial(eyeColor);

      leftPupil.setMaterial(black);
      rightPupil.setMaterial(black);

      exoskeleton.setMaterial(red);
   }

   void setEyecolor(int i) {
      eyeColor = eyeColors[i];
      leftIris.setMaterial(eyeColor);
      rightIris.setMaterial(eyeColor);
      leftPupil.setMaterial(black);
      rightPupil.setMaterial(black);
   }

   double leftIk[] = {0,0,0}, rightIk[] = {0,0,0}, V[] = {0,0,0}, L[] = {0,0,0}, R[] = {0,0,0};

   double time = 0, x = 0, y = 0, z = 0;
   double ly = 0, ry = 0, dly = 0, dry = 0, ddly = 0, ddry = 0;

//////////////////////////////////////////////////
/////////                       //////////////////
/////////  ANIMATE EVERY FRAME  //////////////////
/////////                       //////////////////
//////////////////////////////////////////////////

   public void animate(double time) {

      super.animate(time);

///////////////// PLACE THE PEEP AND ITS FEET IN THE WORLD

      // GET POLLYMORPHIC COORDINATES

      double x0 = polly.getX(0), y0 = polly.getY(0), z0 = polly.getZ(0);
      double x1 = polly.getX(1), y1 = polly.getY(1), z1 = polly.getZ(1);
      double x2 = polly.getX(2), y2 = polly.getY(2), z2 = polly.getZ(2);
      double x3 = polly.getX(3), y3 = polly.getY(3), z3 = polly.getZ(3);
      double x4 = polly.getX(4), y4 = polly.getY(4), z4 = polly.getZ(4);
      double x5 = polly.getX(5), y5 = polly.getY(5), z5 = polly.getZ(5);

      double px = (x0 + x2 + x3 + x5) / 4;
      double py = (y0 + y2 + y3 + y5) / 4;
      double pz = (z0 + z2 + z3 + z5) / 4;

      // COMPUTE THE FLAP ANGLE OF THE FEET AS THEY GO UP AND DOWN

      double dly = misc.lerp(.5, this.dly, y1 - this.ly) ; this.ly = y1;
      double dry = misc.lerp(.5, this.dry, y4 - this.ry) ; this.ry = y4;
      ddly = misc.lerp(.5, ddly, dly - this.dly) ; this.dly = dly;
      ddry = misc.lerp(.5, ddry, dry - this.dry) ; this.dry = dry;

      // PLACE LEFT FOOT

      push();
         translate(x1, y1+FH, z1);
	 rotateY(polly.getLeftFootTwist());
         rotateX(3 * ddly);
         if (!showActor || !showLegs)
            scale(0,0,0);
         transform(leftFoot);
      pop();

      // PLACE RIGHT FOOT

      push();
         translate(x4, y4+FH, z4);
	 rotateY(polly.getRightFootTwist());
         rotateX(3 * ddry);
         if (!showActor || !showLegs)
            scale(0,0,0);
         transform(rightFoot);
      pop();

      // PLACE PEEP

      push();
         translate(px, py, pz);

	 double lx = (x0 + x2) / 2, ly = (y0 + y2) / 2, lz = (z0 + z2) / 2;
	 double rx = (x3 + x5) / 2, ry = (y3 + y5) / 2, rz = (z3 + z5) / 2;
	 double fx = (x0 + x3) / 2, fy = (y0 + y3) / 2, fz = (z0 + z3) / 2;
	 double bx = (x2 + x5) / 2, by = (y2 + y5) / 2, bz = (z2 + z5) / 2;
	 rotateY(Math.atan2(lz-rz, rx-lx));
	 rotateZ(Math.atan2(y3-y0, x3-x0));
	 rotateX(Math.atan2(by-fy, fz-bz));

         if (!showActor)
            scale(0,0,0);
         transform(root);
      pop();

   ///////////////// DO MOVEMENTS INTERNAL TO THE PEEP //////////////////////

      // RIGID TRANSFORMATIONS OF THE BODY

      push();
         translate(0,TY,TZ);
         scale(1,1,1.1);
         transform(body);
      pop();

      push();
         translate(0,0,-.1);
         rotateX(.23 * Math.PI);
         transform(head);
         transform(mouth);
         transform(mouthCavity);
      pop();

      // SIZE ADJUSTMENTS OF THE LEGS

      push();
         rotateX(Math.PI/2);
         scale(1.4*LW,1.4*LW,1.2);
         if (!showLegs)
            scale(0,0,0);
         transform(leftThigh.child(1));
         transform(rightThigh.child(1));
      pop();

      push();
         scale(LW,LW,LW);
         transform(leftKnee.child(0));
         transform(rightKnee.child(0));
      pop();

      push();
         scale(LW,LW,1);
         transform(leftCalf.child(0));
         transform(rightCalf.child(0));
      pop();

      // PLACE THE LEGS USING INVERSE KINEMATICS

      Vec.set(V, -.8,-.4+TY,0);
      Vec.set(L, 0,0,0);
      misc.transform(V, root.getMatrix());
      misc.transform(L, leftFoot.getMatrix());
      computeIkAngles(V, L, leftIk);

      push();
         translate(V[0],V[1],V[2]);
         rotateZ(leftIk[2]);
         rotateX(leftIk[0] + leftIk[1]);
         if (!showActor || !showLegs)
            scale(0,0,0);
         transform(leftThigh);
      pop();

      push();
         translate(0,-1,0);
         rotateX(-2*leftIk[1]);
         if (!showLegs)
            scale(0,0,0);
         transform(leftLeg);
      pop();

      Vec.set(V, .8,-.4+TY,0);
      Vec.set(R, 0,0,0);
      misc.transform(V, root.getMatrix());
      misc.transform(R, rightFoot.getMatrix());
      computeIkAngles(V, R, rightIk);

      push();
         translate(V[0],V[1],V[2]);
         rotateZ(rightIk[2]);
         rotateX(rightIk[0] + rightIk[1]);
         if (!showActor || !showLegs)
            scale(0,0,0);
         transform(rightThigh);
      pop();

      push();
         translate(0,-1,0);
         rotateX(-2*rightIk[1]);
         if (!showLegs)
            scale(0,0,0);
         transform(rightLeg);
      pop();


   /////////////////// EYE MOVEMENTS


      double sy = Noise.noise(2.5 * time + 10) > 0 ? .02 : -.02;
      double sx = Noise.noise(2.5 * time     ) > 0 ? .02 : -.02;
      push();
         rotateY(sx-.4);
         rotateX(sy);
         translate(0,0,1.1-.3*IW);
         scale(.6*IW,.6*IW,.3);
         transform(leftIris);
      pop();

      push();
         rotateY(sx+.4);
         rotateX(sy);
         translate(0,0,.8 + .4 * (1-IW));
         scale(.6*IW,.6*IW,.3);
         transform(rightIris);
      pop();

      push();
         double s = time % 1 < .1 ? 0 : 1;
         scale(s,s,s);
         transform(eyes);
      pop();

   //////////////////// MOUTH MOVEMENTS

      animateMouth(time, mouthRef, mouth);
      animateMouth(time, mouthCavityRef, mouthCavity);

   //////////////////// OPTIONALLY VISIBLE POLLYMORPHIC EXOSKELETON

      if (showExoskeleton) {
         push();
	    scale(1,1,1);
	    transform(exoskeleton);
         pop();
	 for (int i = 0 ; i < 6 ; i++) {
	    push();
	       translate(polly.getX(i), polly.getY(i), polly.getZ(i));
	       scale(.3,.3,.3);
	       transform(exoskeleton.child(i));
	    pop();
	 }
      }
      else {
         push();
	    scale(0,0,0);
	    transform(exoskeleton);
         pop();
      }
   }

   double sgn(double t) { return t > 0 ? 1 : -1; }

   double C[] = {0,0,0};

   void computeIkAngles(double A[],double B[], double angles[]) {
      C[0] = A[0] - B[0];
      C[1] = A[1] - B[1];
      C[2] = A[2] - B[2];
      double norm = Vec.norm(C);
      angles[0] = Math.atan2(C[2], Math.sqrt(C[1]*C[1]+C[0]*C[0]));
      angles[1] = Math.acos(.5 * norm);
      angles[2] = Math.atan2(-C[0], C[1]);
      if (norm >= 2)
         angles[1] = 0;
   }

////////////////////////// ANIMATE MOUTH EXPRESSIONS ///////////////////////////

   void animateMouth(double time, Geometry src, Geometry dst) {
      double dx, dy, dz;
      for (int i = 0 ; i < src.vertices.length ; i++) {

         double x = src.vertices[i][0]/.6, y = src.vertices[i][1]/.6, z = src.vertices[i][2]/.6;

         double d = dsqr(p, src.vertices[i]);
         double w = Math.max(0, 1 - 4 * d);
         w = w * w;

         dx = -.5 * x * pucker;
         dy = .45 * y * (open - .8) +
              .4 * (.1 + lump(x,-1.9,.1) * leftSmile + lump(x,-.1,1.9) * rightSmile) +
              ( y<0 ? 0 : 1.4 * y * ( lump(x,-1 ,.1) * leftGrimace +
                                      lump(x,-.5,.5) * (leftGrimace + rightGrimace) * .3 +
                                      lump(x,-.1,1 ) * rightGrimace ));
         dz = (.1 * Math.max(0,pucker) + .05 * y + .2 - Math.max(0, w-.5)) * (1-x*x*.35);

         dst.vertices[i][0] = src.vertices[i][0] + w * dx;
         dst.vertices[i][1] = src.vertices[i][1] + w * dy;
         dst.vertices[i][2] = src.vertices[i][2] + w * dz;
      }
      dst.recomputeMeshNormals();
   }

   double lump(double t, double lo, double hi) {
      if (t <= lo || t >= hi)
         return 0;
      double mid = (lo + hi) / 2;
      t = 1 - Math.abs(t - mid) / (hi - mid);
      return t * t * (3 - t - t);
      
   }

   double dsqr(double a[], double b[]) {
      double x = a[0] - b[0], y = a[1] - b[1], z = a[2] - b[2];
      return x * x + y * y + z * z;
   }

   double p[] = {0,0,1};
   boolean showLegs = true;
}