/** A robot controller for judo competition
 *  
 *  Author: Martin Dlouhy
 *  SourceURL: www.robotika.cz
 *  
 *  Revision History:
 *     03 Aug 12, MD - first implementation, load sequence from a file seq.txt
 */

import com.cyberbotics.webots.Controller;
import java.io.*;

public abstract class Judoka0 extends Controller 
{
  public static int readPosInt(DataInputStream in)
  {
    byte c;

    try
    {
      do
      {
        c=in.readByte();
      } while( (c == ' ') || (c == '\n') || (c == '\t') || (c == '\r') );

      int ret = 0;
      while( c >= '0' && c <= '9' )
      {
        ret = ret * 10 + (c - '0');
        c=in.readByte();
      }

      return ret;
    }
    catch(IOException e)
    {
      System.err.println("Caught readImage IOException: " +
                  e.getMessage());
      return -1;
    }
  }

  static int [] joint = new int[21];
  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");
  }

  public static void main() 
  {
    DataInputStream in = null;
    
    int i;
    int pos[] = new int[21];

    int repeatCmd = 0;
    
    for(i=0;i<21;i++) 
    {
      servo_set_position(joint[i],0);
      pos[i] = 0;
    }

    try
    {
      for(;;) 
      { // The robot never dies!
      
	      if(in == null)
	      {
	        in = new DataInputStream(new FileInputStream("seq.txt"));
	      }
	      
        if(repeatCmd <= 0)
        {
          repeatCmd = readPosInt(in);
          if(repeatCmd < 0)
          {
            in = new DataInputStream(new FileInputStream("seq.txt"));
            repeatCmd = readPosInt(in);          
	        }
	        for(i = 0; i < 21; i++)
	        {
	          pos[i] = readPosInt(in);
	          if(pos[i] < 0)
	          {
	            in = null;
	            break;
	          }
	        }
        }
        else
        {
          repeatCmd--;
        }
		
        if(in != null)
	      {
          for(i = 0; i < 21; i++)
          {
	          servo_set_position(joint[i], 3.1415f * pos[i]/180.0f);
          }
	      }
	      
	      robot_step(32); // run one step
	    }
	  }
    catch(IOException e)
    {
      System.err.println("Caught IOException: " + e.getMessage());
    }      
  }
}