import vrml.* ;
import vrml.field.* ;
import vrml.node.* ;
import java.lang.Math ;

public class Animator extends Script {

private float theta =  (float) ( 15 * Math.PI/180 );


private float omega = (float) ( theta * 2 )  ;
//private float omega2 = (float)( ( theta-1*Math.PI/180 ) * 2 ) ;

private float curr_thigh_angle, curr_calf_angle , max_calf_angle ;

                                             
private float thighLength = 6;
private float calfLength = 9;
private float footLength = 4;
private float torsoHeight, curr_torso_height ; 
private float toeDistance ;
private float t, dt ;

private SFRotation current_thigh_angle, current_calf_angle ;

// Event Outs
private SFRotation rotate_thigh, rotate_calf ;
private SFVec3f adjust_torso ;
private SFVec3f adjust_thigh ;
private SFVec3f translate_body;

// State variables
private int state = 0;
private float angle3start, angle4start;
private float angle3delta, angle4delta;
private float torsoPosX, torsoPosY, torsoPosZ;
private float thighPosX, thighPosY, thighPosZ;

private float heelDistance;
private float torsoDistance;

private float hipToToeSegment;
private float hipToToeAngle;
private float deltaF = 0;   //(float) 1.5;   // Used in calculation for part 4


private float hipDistance;
private float hipHeight;


// Variables for adjust height 2
private float start_frac = 0;
private float deltaFrac = 0;
private float l3max = 0;

//variable for hand movements
private float bicep_fwd_angle = ( float ) ( -15 * Math.PI/180 ) ; 
private float bicep_back_angle = ( float ) ( 10 * Math.PI/180 ) ; 
private float arm_fwd_angle   = ( float ) ( -15 * Math.PI/180 ) ; 
private float arm_back_angle   = ( float ) ( 2 * Math.PI/180 ) ; 
private float curr_bicep_angle = 0 ; 
private float curr_arm_angle   = 0 ; 
private float bicep_omega = -( bicep_fwd_angle - bicep_back_angle ) ;
private float arm_omega   = -( arm_fwd_angle - arm_back_angle ) ;
private float offset = 0 ;

//Event Outs
private SFRotation rotate_bicep ;
private SFRotation rotate_arm ;

//EventOuts for touchsensor and view point ;
private SFVec3f translate_touch_sensor , translate_view ;
private float sensor_offX = -10 ;
private float sensor_offY = 0 ;
private float sensor_offZ = 10 ;
private float view_offX = -5 ;
private float view_offY = 0 ;
private float view_offZ = 40 ;

 public void initialize( )
 {
  t    = 0  ;
  dt   = (float)0.04 ;

  SFFloat tmp =  ( SFFloat ) getField( "theta" )     ;
  theta       =   tmp.getValue(  ) ;

  curr_thigh_angle = theta ;

  if (theta < 0) {
    theta = -theta;
  }


  // Get intial value of state
  SFInt32 tmpState = (SFInt32) getField( "state" );
  state = tmpState.getValue();

  SFFloat tmp1 =  ( SFFloat ) getField( "time" )     ;
  t =   tmp1.getValue(  ) ;

  System.out.println( "Theta: " + theta ) ;

  torsoHeight = (float)( ( thighLength + calfLength ) * Math.cos( theta ) + footLength * Math.sin( theta ) ) ;
  toeDistance = (float)( ( thighLength + calfLength ) * Math.sin( theta ) - footLength * Math.cos( theta ) ) ;
  curr_torso_height = torsoHeight ;

  curr_calf_angle  = 0 ;

  current_thigh_angle = new SFRotation( ) ;
  current_calf_angle  = new SFRotation( ) ;

  current_thigh_angle = ( SFRotation ) getField( "current_thigh_angle" ) ;
  current_calf_angle =  ( SFRotation ) getField( "current_calf_angle" ) ;


  // getting handles to event outs
  rotate_thigh = ( SFRotation ) getEventOut( "rotate_thigh" ) ;
  rotate_calf  = ( SFRotation ) getEventOut( "rotate_calf"  ) ;
  adjust_torso = ( SFVec3f    ) getEventOut( "adjust_torso"  ) ;
  translate_body = (SFVec3f) getEventOut( "translate_body" );
  adjust_thigh   = (SFVec3f) getEventOut( "adjust_thigh" );
  rotate_thigh.setValue( new SFRotation( 0, 0, 1, curr_thigh_angle ) ) ;
  translate_touch_sensor = (SFVec3f) getEventOut( "translate_touch_sensor" ) ;
  translate_view = (SFVec3f) getEventOut( "translate_view" ) ;
 
  torsoPosX = 0;
  torsoPosY = 0;

  // Initialize values needed for part 4
  float footLengthShort = footLength - deltaF;            
  hipToToeSegment = (float) Math.sqrt( Math.pow(thighLength + calfLength, 2) +
                                       Math.pow(footLengthShort, 2) );
  hipToToeAngle = (float) Math.atan((thighLength + calfLength) / footLengthShort);

// For hand movements
   rotate_bicep = ( SFRotation ) getEventOut( "rotate_bicep" ) ;
   rotate_arm   = ( SFRotation ) getEventOut( "rotate_arm" ) ;

// Initializing arm positions
   if( t < 1 ) {
      rotate_bicep.setValue( new SFRotation( 0 , 0 , 1 , bicep_back_angle ) ) ;
      rotate_arm.setValue( new SFRotation( 0 , 0 , 1 , arm_back_angle ) ) ;
     }

   if( t >= 1 ) {
      rotate_bicep.setValue( new SFRotation( 0 , 0 , 1 , bicep_fwd_angle ) ) ;
      rotate_arm.setValue( new SFRotation( 0 , 0 , 1 , arm_fwd_angle ) ) ;
     }

   offset = ( float ) ( footLength * Math.sin( curr_thigh_angle ) ) ;
   updateView( ) ;

  }

 public void processEvent( Event e )
 {
   String name = e.getName();

   if (name.equals("set_fraction")) {
     Update( );
   }
   if (name.equals("set_torso_translation")){
     UpdateTorsoPosition((ConstSFVec3f) e.getValue());
   }
 }                                                                          

 private void UpdateTorsoPosition(ConstSFVec3f translation)
 {
   torsoPosX = translation.getX();
   //torsoPosY = translation.getY();
   torsoPosZ = translation.getZ();
 }


 private void set_thigh_rotation( SFRotation rot ) 
 {
      rotate_thigh.setValue( rot ) ;
      current_thigh_angle.setValue( rot ) ;
 } 

 private void set_calf_rotation( SFRotation rot ) 
 {
      rotate_calf.setValue( rot ) ;
      current_calf_angle.setValue( rot ) ;
  }

 private void set_torso_height( float value ) 
 {
      adjust_torso.setValue( new SFVec3f( torsoPosX , value , 0 ) ) ;
 }

 private void set_thigh_height( float value ) 
 {
      adjust_thigh.setValue( new SFVec3f( 0, value , 0 ) ) ;
 }



 private void part1( float frac )
 {

   if (state != 1) {
     state = 1;
   }

   curr_thigh_angle =  ( - omega * frac + theta ) ;

   // Finding calf angle
   double ll = Math.sqrt( calfLength * calfLength + footLength * footLength ) ;
   double nn = -toeDistance + thighLength * Math.sin( Math.abs(curr_thigh_angle) )  ;
   double beta = Math.atan( 1/( footLength / calfLength ) ) ;

   curr_calf_angle = (float) (  Math.acos( nn/ll ) - beta ) - curr_thigh_angle ;
   max_calf_angle  = curr_calf_angle ;

//  Adjusting heights
   double aa = calfLength *  calfLength + footLength * footLength ;
   double bb = Math.pow( ( toeDistance - thighLength * Math.sin( curr_thigh_angle )) , 2.0 ) ;
   double cc = Math.sqrt( aa - bb ) + thighLength * Math.cos( curr_thigh_angle ) ;

   curr_torso_height = (float) cc ;
   float dy = curr_torso_height - torsoHeight ;

   set_thigh_height( dy  - thighLength/2  ) ;

   thighPosY = dy ;

   set_thigh_rotation( new SFRotation( 0 , 0 , 1 , curr_thigh_angle ) ) ;
   set_calf_rotation(  new SFRotation( 0 , 0 , 1 , curr_calf_angle )) ;

 } 
 

 private void part2( float frac )
 {
  if (state != 2) {
    state = 2;
  }

  curr_thigh_angle = -( omega * ( frac - (float) 0.5 )  );
  float calf_rate   = max_calf_angle * 2 ;
  curr_calf_angle  = -( calf_rate * ( frac - (float)0.5 ) ) + max_calf_angle ;
  
//  Adjusting heights
   float fac = 1 ;
   float delta = 0 ;

   if( ( -curr_thigh_angle  ) >= curr_calf_angle )
        fac = (float)-0.00 ;

      
   float  bz = curr_calf_angle + curr_thigh_angle         ;
   double l1 = thighLength * Math.cos( curr_thigh_angle ) ;
   double l2 = calfLength  * Math.cos(  bz   )            ;
   double l3 = fac * footLength  * Math.sin(  bz  )       ;

   curr_torso_height = (float) ( l1 + l2 + l3 ) ;
     
   float dy = curr_torso_height - torsoHeight + delta ;

   set_thigh_height( dy  - thighLength/2 ) ;

   thighPosY = dy ;

   set_thigh_rotation( new SFRotation( 0 , 0 , 1 , curr_thigh_angle ) ) ;
   set_calf_rotation ( new SFRotation( 0 , 0 , 1 , curr_calf_angle  ) ) ;
 
 }


 private void part3( float frac )
 {
   System.out.println( "Part3-torsoPosY: " + torsoPosY ) ;

   if (state != 3) {
     state = 3;
     
     // Kludge for finishing up adjust_height2
     start_frac = 0;

     // Record start angle and amount of rotation
     angle3start = curr_thigh_angle;
     angle3delta = -curr_thigh_angle;

     System.out.println("part3: angle3start = " + angle3start);
     System.out.println("part3: angle3delta = " + angle3delta);

     // Record the starting dimensions needed for translation calculations
     heelDistance = -(thighLength + calfLength) * (float) Math.sin(curr_thigh_angle); 
     torsoDistance = (thighLength + calfLength) * (float) Math.cos(curr_thigh_angle);
   }

   // Interpolate the new amount of rotation
   curr_thigh_angle = frac * angle3delta + angle3start;

   // Calculate the amount to translate the body in the X direction
   float newHeelDistance = -(thighLength + calfLength) * (float) Math.sin(curr_thigh_angle);
   torsoPosX = torsoPosX + (newHeelDistance - heelDistance);  
   heelDistance = newHeelDistance;

   // Calculate the amount to translate the body in the Y direction
   float newTorsoDistance = (thighLength + calfLength) * (float) Math.cos(curr_thigh_angle);
   //torsoPosY = torsoPosY + (newTorsoDistance - torsoDistance);
   torsoPosY = -(torsoHeight - newTorsoDistance);
   torsoDistance = newTorsoDistance;
   
   // Send out the rotation and translation events
   rotate_thigh.setValue(0, 0, 1, curr_thigh_angle);
   translate_body.setValue(torsoPosX, 0, torsoPosZ);
   adjust_thigh.setValue(0, torsoPosY - (thighLength / 2), 0);

 }


 private void part4( float frac )
 {
   if (state != 4) {
     state = 4;
     
     angle4start = curr_thigh_angle;
     angle4delta = theta - curr_thigh_angle;

     System.out.println("part4: angle4start = " + angle4start);
     System.out.println("part4: angle4delta = " + angle4delta);

     // Record starting dimensions needed for translation calculation
     hipDistance = (float)(hipToToeSegment *  Math.cos(hipToToeAngle + curr_thigh_angle));
     hipHeight = (float)(hipToToeSegment * (float) Math.sin(hipToToeAngle + curr_thigh_angle));
   }

   // Interpolate the new amount of rotation
   curr_thigh_angle = frac * angle4delta + angle4start;

   // Calculate amount to translate body in X direction
   float newHipDistance = hipToToeSegment * (float) Math.cos(hipToToeAngle + curr_thigh_angle);   
   torsoPosX = torsoPosX + (newHipDistance - hipDistance);
   hipDistance = newHipDistance;

   // Calculate amount to translate body in Y direction
   float newHipHeight = hipToToeSegment * (float) Math.sin(hipToToeAngle + curr_thigh_angle);
   //torsoPosY = torsoPosY + (newHipHeight - hipHeight);
   torsoPosY = -(torsoHeight - newHipHeight);
   hipHeight = newHipHeight;

   // Send out rotation and translation events
   rotate_thigh.setValue(0, 0, 1, curr_thigh_angle);
   translate_body.setValue(torsoPosX, 0, torsoPosZ);
   adjust_thigh.setValue(0, torsoPosY - thighLength/2, 0);

 }

 private void Update( )
 {

   //System.out.println( "Update: " + t  + "\t dt = " + dt ) ;

   if( t >= 0 && t <= 0.5 )
   {  
      state = 1;
      part1( t ) ;
   }

   if( t > 0.5 && t <= 1.0 )
   {
      state = 2;
      part2( t ) ;
   }

   if( t > 1.0 && t <= 1.5 )
   {
      part3( (t - 1) * 2 );
   }

   if( t > 1.5 && t <= 2.0 )
   {
      part4( (t - (float) 1.5) * 2);
   }

   swingHand( t ) ;
   updateView( ) ;

   t += dt ;
   if( t > (float) 2.0 ) t = 0 ;
 }


private void swingHand( float time )
{
  
 if ( time >= 0 && time <= 1 )
 {
    curr_bicep_angle = bicep_back_angle - bicep_omega * time ;
    curr_arm_angle   = arm_back_angle - arm_omega * time   ;

    rotate_bicep.setValue( new SFRotation( 0 , 0 , 1 , curr_bicep_angle ) ) ;
    rotate_arm.setValue( new SFRotation( 0 , 0 , 1 , curr_arm_angle ) ) ;    

 }

 if( time > 1 && time <= 2 )
 {
    curr_bicep_angle =  bicep_fwd_angle + bicep_omega * ( time - 1  ) ;
    curr_arm_angle   =  arm_fwd_angle + arm_omega * ( time - 1 )  ;

    rotate_bicep.setValue( new SFRotation( 0 , 0 , 1 , curr_bicep_angle ) ) ;
    rotate_arm.setValue( new SFRotation( 0 , 0 , 1 , curr_arm_angle ) ) ;    
 }
}

 
private void updateView( ) 
{
 	translate_touch_sensor.setValue( new SFVec3f( torsoPosX + sensor_offX , sensor_offY , sensor_offZ ) ) ;
	translate_view.setValue( new SFVec3f( torsoPosX + view_offX , view_offY , view_offZ ) ) ;
}

// End of Class
}