import vrml.*;
import vrml.node.*;
import vrml.field.*;

public class joystick extends Script {
  // eventIn
  private SFFloat time;
  private SFVec3f set_translation;
  private SFBool is_active;

  // eventOut
  private SFRotation rotation_changed;
  private SFRotation wheel_angle;
  private SFRotation gas_pedal;

  // fields
  private SFVec3f current_translation;
  
  // internal variables
  private boolean isOn;
  private float dampening;
  private float max_gas;
  private float max_wheel;
  private float max_angle;
  private float[] rot;
  private float[][] result;
  private float[] current;
  private float[] temp;
  //  private Rotation x;
  //  private Rotation z;
  
  public void initialize() {
    try {
      isOn = false;
      dampening = (float).9;
      max_gas = 2;
      max_wheel = 2;
      max_angle = (float)(Math.PI/4);

      rot = new float[4];
      result = new float[3][3];
      current = new float[3];
      temp = new float[3];
      //x = new Rotation();
      //z = new Rotation();
      
      set_translation = (SFVec3f)getField("set_translation");
      is_active = (SFBool)getField("is_active");
      rotation_changed = (SFRotation)(getEventOut("rotation_changed"));
      wheel_angle = (SFRotation)(getEventOut("wheel_angle"));
      gas_pedal = (SFRotation)(getEventOut("gas_pedal"));
      current_translation = (SFVec3f)getField("current_translation");
    }
    catch (InvalidFieldException e) {
      System.exit(1);
    }
  }

  public void time_signal() {
    current_translation.getValue(current);
    if ((current[0] != 0) || (current[1] != 0) || (current[2] != 0)) {
      current[0] = current[0]*dampening;
      current[1] = current[1]*dampening;
      current[2] = current[2]*dampening;
      resetTrans(current);
    }
  }

  public void resetTrans(float[] current) {
    current_translation.setValue(current);

    rot[0] = 1;
    rot[1] = 0;
    rot[2] = 0;
    rot[3] = (current[1]/max_gas)*(float)(Math.PI/8);
    gas_pedal.setValue(rot);
    //x.setAA(rot);
    
    rot[0] = 0;
    rot[1] = 1;
    rot[2] = 0;
    rot[3] = -(current[0]/max_wheel)*(float)(Math.PI/6);
    wheel_angle.setValue(rot);
    /*z.setAA(rot);
    
    System.out.println("step 3");
    Mat.Mult(x.getR(), z.getR(), result);
    x.setR(result);
    x.getAA(rot);
    System.out.println("rot is " + rot[0] + " " + rot[1] + " " + rot[2] + " " + rot[3]);
    rotation_changed.setValue(rot); */
  }
  
  public void processEvent(Event e) {
    String event_name = e.getName();
    
    if (event_name.equals("time") && !isOn) {
      time_signal();
    } else if (event_name.equals("set_translation")) {
      ((ConstSFVec3f)e.getValue()).getValue(temp);
      resetTrans(temp);
    } else if (event_name.equals("is_active")) {
      isOn = ((ConstSFBool)e.getValue()).getValue();
    }
  }
};