import com.cyberbotics.webots.Controller;

public abstract class Judoka1 extends Controller {
  public static class Gene {
    Gene() {
      joint = 0;
      amplitude = 1.4;
      omega = 1;
      fi = 0.0;
      constant = 0.0;
    }
    void print() {
      System.out.println("joint     = "+joint);
      System.out.println("amplitude = "+amplitude);
      System.out.println("omega     = "+omega);
      System.out.println("fi        = "+fi);
      System.out.println("constant  = "+constant);
    }
    int joint;
    double amplitude;
    int omega;
    double fi;
    double constant;
  }
  static int [] joint = new int[21];
  static int camera;
  static int distance;
  public static void reset() {
    joint[0]=robot_get_device("back_1");
    joint[1]=robot_get_device("back_2");
    joint[2]=robot_get_device("left_hip_1");
    joint[3]=robot_get_device("left_hip_2");
    joint[4]=robot_get_device("left_hip_3");
    joint[5]=robot_get_device("left_knee");
    joint[6]=robot_get_device("left_ankle_1");
    joint[7]=robot_get_device("left_ankle_2");
    joint[8]=robot_get_device("left_shoulder_1");
    joint[9]=robot_get_device("left_shoulder_2");
    joint[10]=robot_get_device("left_elbow");
    joint[11]=robot_get_device("neck");
    joint[12]=robot_get_device("right_hip_1");
    joint[13]=robot_get_device("right_hip_2");
    joint[14]=robot_get_device("right_hip_3");
    joint[15]=robot_get_device("right_knee");
    joint[16]=robot_get_device("right_ankle_1");
    joint[17]=robot_get_device("right_ankle_2");
    joint[18]=robot_get_device("right_shoulder_1");
    joint[19]=robot_get_device("right_shoulder_2");
    joint[20]=robot_get_device("right_elbow");
    for(int i=0;i<21;i++) if (joint[i]==0)
     System.out.println("Joint "+i+" is missing");
    camera=robot_get_device("camera");
    distance=robot_get_device("distance");
  }
  public static void main() {
    Gene [] gene = new Gene[21];
    float [] futurPosition = new float[21];
    float t=0.0f;
    int i,targetJoint;
    int[] image;
    int l;

    for(i=0;i<21;i++) {
      gene[i]=new Gene();
      if (i==0 || i==1 || i==4 || i==14 || i==7 || i==17) gene[i].joint=-1;
      else gene[i].joint=i;
      if (i==5 || i==15) gene[i].amplitude *= 2;
    }
    camera_enable(camera,64);
    distance_sensor_enable(distance,64);
    for(i=0;i<21;i++) {
      servo_set_position(joint[i],0);
      futurPosition[i]=0.0f;
    }
    for(;;) { // The robot never dies!
      for(i=0;i<21;i++) {
        targetJoint = gene[i].joint;
        if (targetJoint == -1) continue;
        futurPosition[targetJoint] += gene[i].amplitude*
                                      Math.sin(gene[i].omega*t+
	 			      gene[i].fi)+gene[i].constant;
        if (futurPosition[targetJoint]<0) futurPosition[targetJoint]=0;
      }
      for(i=0;i<21;i++) {
        servo_set_position(joint[i],futurPosition[i]);
        futurPosition[i]=0.0f;
      }
      t += 0.1;
      robot_step(64); // run one step
      image = camera_get_image(camera);
      l = distance_sensor_get_value(distance);
    }
  }
}