import java.net.*;
import java.io.*;
import java.util.StringTokenizer;


public class ConnectionHandler{
public Socket Supervisor = null;
     public DataInputStream input = null;
     public DataOutputStream output = null;
	public ConnectionHandler()
	{
		try {

		this.Supervisor = new Socket("localhost",54000);
		this.input = new DataInputStream(this.Supervisor.getInputStream());
		this.output = new DataOutputStream(this.Supervisor.getOutputStream());

		}catch(UnknownHostException e){
			System.err.println("Trying to connect to unknown host: " + e);
		}
		catch(IOException e) 
		{
			System.err.println("IOException: " + e);
		}
	}


	public void writeToUrbi(String command){
		try{
			output.writeBytes(command);	
			output.flush();
		}catch(Exception ex){
			ex.printStackTrace();
			}
	}

	public String readFromUrbi(){
		try{
			byte b[] = new byte[1000];
			int x = input.read(b);
			System.out.println("Bytes read from Urbi: "+x);
                        //byte b1[] = new byte[x]; 
			/*for(int i=0; i<x ; i++){
				b1[1] = b[i];	
			}*/
			String response = new String(b);
			return response;
		}catch(Exception e){
			e.printStackTrace();
		}
		return null;
		
	}
	
	public void close(){
		try{
			Supervisor.close();
			System.out.println("Connection Closed");
		}catch(Exception ex){
			ex.printStackTrace();
		}
	}
	public static void main(String args[]){
		ConnectionHandler handle = new ConnectionHandler();
		handle.writeToUrbi("mytag+end: robot.walk(10s);\n");
		String x = "";
		String response="";
		
		while(response != null){
		response = handle.readFromUrbi();
	
		StringTokenizer tokens = new StringTokenizer(response);

		while (tokens.hasMoreTokens())
		{
			String word = tokens.nextToken();
			if(word.equals("end"))
			{
			   response = null;
			   break;
			}
		}
		}
		handle.close();
	}

}
