package cie.mobile;

import name.lxm.robot.arch.*;

public class RunAwayModule extends AbstractModule
{
	String[] port_names = {"force", "command"};
	SimpleInPort force_port;
	SimpleOutPort cmd_port;
	Object lock = new Object();
	double threashold_neglect = 0;
	double threashold_attention = 0;
	public void run()
	{
		while(bRun)
		{
			/*
			synchronized(lock)
			{
				try{
					lock.wait();
				}catch(InterruptedException e)
				{
					System.out.println("Thread is interrupted!");
					break;
				}
			}
			*/
			try{
				Thread.sleep(500);
			}catch(InterruptedException e)
			{}
			
			//get the force
			Object o = force_port.getValue();
			if(o != null)
			{
				double[] f = (double[]) o;
				process(f);
			}
		}
	}

	public Port getPort(String portname)
	{
		if(port_names[0].equals(portname))
			return force_port;
		if(port_names[1].equals(portname))
			return cmd_port;
		return null;
	}

	public RunAwayModule()
	{
	}

	public void init(ModuleDoc conf) throws Exception
	{
		super.init(conf);
		force_port = new SimpleInPort(this, port_names[0]);
		force_port.registerListener(this);
		cmd_port = new SimpleOutPort(this, port_names[1]);
		String s = doc.getParamValue("threashold_neglect");
		if(s == null)
			throw new IllegalXMLFormatException("Module " + module_name + " needs the parameter threashold_neglect.");
		threashold_neglect = Double.parseDouble(s);
		s = doc.getParamValue("threashold_attention");
		if(s == null)
			throw new IllegalXMLFormatException("Module " + module_name + " needs the parameter threashold_attension.");
		threashold_attention = Double.parseDouble(s);
	}

	/**
	 * calculate the moving command according to the
	 * given barrier force
	 * @param force - double[]  force vector
	 */
	private void process(double[] force)
	{
		double f = Math.sqrt(force[0]*force[0] + force[1]*force[1]);
		double degree;
		
		if(f < threashold_neglect)
		{
			//this force is neglectable
			cmd_port.setValue(this, "FORWARD 500", 1000);
			return;
		}else/* if(f < threashold_attention)*/
		{
			/*degree = Math.acos(force[0] / f);*/
			degree = Math.abs(force[0])/force[0]*Math.PI/18.0;
			cmd_port.setValue(this, "ONLYTURN " + Double.toString(degree), 1000);
			
			System.out.println("*ONLYTURN " + Double.toString(degree) + " " + force[0] + " " + force[1]);
			sleep(2000);
			return;
		}/*else
		{
			//this force should be paied highly attention
			degree =Math.acos(force[0]/f);
			double dd = degree/Math.abs(degree)*Math.atan((f-threashold_attention)/threashold_attention);
			cmd_port.setValue(this, "TURN " + Double.toString(degree+dd), 1000);
			System.out.println("-TURN " + Double.toString(degree+dd));
			sleep(1000);
		}*/
	}

	public void valueUpdated()
	{
		/*
		synchronized(lock)
		{
			lock.notifyAll();
		}
		*/
	}
}