//package dynsim;
import vrml.*;
import vrml.node.*;
import vrml.field.*;
import Mat;
import Rotation;


// A rotational dynamics class with momentum and energy conservation
public class dynamics2 extends Script{
   // Define all the eventOuts and fields as class variables.
     private SFRotation rotation;
     private SFRotation rotation_changed;
     private SFVec3f Vomega;
     private SFVec3f Vinertia;
     private SFVec3f Energy;
     private SFBool running;
     private float oldtime;
     private float R[][];
     private float RT[][];
     private float newR[][];
     private float tempv[];
     private float tempv1[][];
     private float tempv2[][];
     private float Iinv[][];
     private float II[][];
     private float AA[][];
     private float omega[][];
     private float p[][];
     private float pt[][];
     private float pskew[][];
     private float val[][];
     private float E0;
     private Rotation Rot;

     // This method is called when the script is loaded
     public void initialize(){
       // Get field and event handles
       rotation = (SFRotation) getField("rotation");
       rotation_changed = (SFRotation) getEventOut("rotation_changed");
       Energy = (SFVec3f) getEventOut("Energy");
       Vomega = (SFVec3f) getField("omega");
       Vinertia = (SFVec3f) getField("inertia");
       running = (SFBool) getField("running");

       // Allocate all the arrays
       R = new float[3][3];
       RT = new float[3][3];
       newR = new float[3][3];
       tempv = new float[4];
       tempv1 = new float[3][1];
       tempv2 = new float[3][1];
       Iinv = new float[3][3];
       II = new float[3][3];
       AA = new float[4][1];
       omega = new float[3][1];
       p = new float[3][1];
       pt = new float[1][3];
       pskew = new float[3][3];
       val = new float[1][1];
       oldtime = 0.0f;

       // Read in the field values from VRML
       Vomega.getValue(tempv);
       Mat.VtoM(tempv, omega);  // omega as a 3x1 matrix

       rotation.getValue(tempv);
       // Convert the rotation to an R matrix
       Mat.VtoM(tempv, AA);
       Rot = new Rotation();
       Rot.setAA(AA);
       Rot.getR(R);

       Vinertia.getValue(tempv); // Read the inertia matrix
	 Mat.Ident(II);            // Put it in the matrix II
       II[0][0] = tempv[0];
       II[1][1] = tempv[1];
       II[2][2] = tempv[2];
       Mat.Ident(Iinv);           // Make its inverse
       Iinv[0][0] = 1.0f / tempv[0];
       Iinv[1][1] = 1.0f / tempv[1];
       Iinv[2][2] = 1.0f / tempv[2];

       // Compute the angular momentum p = R * I * RT * omega
       Mat.Transpose(R, RT);
       Mat.Mult(RT, omega, tempv1);
       Mat.Mult(II, tempv1, tempv2);
       Mat.Mult(R, tempv2, p);
       Mat.Transpose(p, pt);    // Compute the transpose of p
       Mat.Skew(p, pskew);      // and make a skew matrix from p
       Mat.Mult(pt, omega, val); // pt * omega = energy
       E0 = val[0][0];           // Save it away for later
   }

  // The main method, called at every time step
   private void set_time (float time) {
     if ((oldtime != 0.0f) && running.getValue()) {
       float dt = time - oldtime;    // Compute the time step
       if (dt < 0.0f) dt = dt + 1.0f;  // Correct wrap-around of time signal
       dt = Math.min(dt, 0.1f);      // Dont allow long time steps

       // Compute new omega  = R * Iinv * RT * p
       Mat.Transpose(R, RT);
       Mat.Mult(RT, p, tempv1);
       Mat.Mult(Iinv, tempv1, tempv2);
       Mat.Mult(R, tempv2, omega);

       // Compute the new rotation by (omega * dt)
       float n = Mat.Norm(omega);
       Mat.Scale(1.0f/n, omega, AA);
       AA[3][0] = n * dt;
       Rot.setAA(AA);
       Mat.Mult(Rot.getR(), R, newR);
       Rot.setR(newR);

       // Compute the new energy = pt * omega
       Mat.Mult(pt, omega, val);
       tempv[1] = val[0][0]/E0;  // Ratio of new and original energy
       tempv[0] = 1.0f;
       tempv[2] = 1.0f;
       Energy.setValue(tempv);  // Output to scale the meter

       // Make an energy correction
       float dE =  val[0][0] - E0;
       Mat.Mult(pskew, omega, tempv1);
       n = Mat.Norm(tempv1);
       if (n > (2.0f * Math.abs(dE))) {
	 Mat.Scale(1.0f / n, tempv1, AA);
	 AA[3][0] = dE/(2.0f * n);
	 Rot.setAA(AA);
	 Mat.Mult(Rot.getR(), newR, R);
	 Rot.setR(R);
       }

       // Get the current rotation and output it to VRML
       Rot.getAA(AA);
       Mat.MtoV(AA, tempv);
       rotation_changed.setValue(tempv);
       Rot.setAA(AA);  // Also use it to recompute R, to make sure it
       Rot.getR(R);    // remains a valid rotation matrix.
     }
     oldtime = time;
   } 

   private void stop_start(boolean bval) {
     if (bval == true)
       running.setValue(!running.getValue());
   }

  private void set_omega (ConstSFRotation RR) {
    RR.getValue(tempv);
    Mat.VtoM(tempv, AA);
    Rot.setAA(AA);
    Rot.getR(newR);
    omega[0][0] = 5.0f * newR[0][1];
    omega[1][0] = 5.0f * newR[1][1];
    omega[2][0] = 5.0f * newR[2][1];

    Mat.Transpose(R, RT);
    Mat.Mult(RT, omega, tempv1);
    Mat.Mult(II, tempv1, tempv2);
    Mat.Mult(R, tempv2, p);
    Mat.Transpose(p, pt);    // Compute the transpose of p
    Mat.Skew(p, pskew);      // and make a skew matrix from p
    Mat.Mult(pt, omega, val); // pt * omega = energy
    E0 = val[0][0];           // Save it away for later
}
  
   public void processEvent (Event e) {
      String EventName = e.getName();
      
      if (EventName.equals("set_time"))
         set_time(((ConstSFFloat)e.getValue()).getValue()); 

      else if (EventName.equals("stop_start"))
         stop_start(((ConstSFBool)e.getValue()).getValue());

      else if (EventName.equals("set_omega"))
         set_omega((ConstSFRotation)e.getValue());
   }
}