import java.text.*;
import java.util.*;

import nist.feather.*;

public class SoarRobot implements Runnable {

	private SoarSession m_ss;
	private ArRobot robot;
	private ArDPPTU ptu;
	private ArTcpConnection conn;
	private ArSerialConnection serialConn;
	private ArTime starttime;
	private int numberOfSonars;
	static private final String PROPERTIES = "SoarRobot.ini";
	static private final String ROBOT_AGENT = "robotagent.soar";
	static private final int bit1 = 128;
	static private final int bit2 = 64;
	static private final int bit3 = 32;
	static private final int bit4 = 16;
	static private final int bit5 = 8;
	private String runMore;

// -------------------------------------------------------------------

 static {
    try {
        System.loadLibrary("AriaJava");
    } catch (UnsatisfiedLinkError e) {
      System.err.println("Native code library failed to load. See the chapter on Dynamic Linking Problems in the SWIG Java documentation for help.\n" + e);
      System.exit(1);
    }
  }

// -------------------------------------------------------------------

	public SoarRobot() {

		starttime = new ArTime();
		runMore = new String("yes");

		//connect to robot
		Aria.init(0, true);

		robot = new ArRobot("robot1", true, true, true);
		conn = new ArTcpConnection();

		conn.setPort("172.20.38.11", 8101);
		robot.setDeviceConnection(conn);
		if (!robot.blockingConnect())
		{
		   System.err.println("Could not connect to robot, exiting.\n");
		   System.exit(1);
		}

		numberOfSonars = robot.getNumSonar();
		robot.enableMotors();
		robot.runAsync(false);

		//connect DPPTU pan-tilt-zoom unit
		ptu = new ArDPPTU(robot);
		ptu.init();
		System.out.println("the camera zooms ==> " + ptu.canZoom());

		//test DPPTU
		ptu.tilt(45);
		ptu.pan(-45);

		//start agent
		final String INCREASE_MAX_CHUNKS = "max-chunks 10000";
		final String INCREASE_MAX_ELABS = "max-elaborations 100000";

		writeHeading();
		try {
			m_ss = new SoarSession(PROPERTIES);
			//m_ss.setPrintCommandBeforeEval(true);
			m_ss.eval(INCREASE_MAX_CHUNKS);
			m_ss.eval(INCREASE_MAX_ELABS);
			m_ss.loadAgent(ROBOT_AGENT);
		}
		catch (Exception e) {
			System.out.println(e);
		}
	}

// -------------------------------------------------------------------

	public void startThread() {
	    Thread t = new Thread(this);
	    t.start(); // Calls the run method in "this."
  }

// -------------------------------------------------------------------

        public void run() {

		String s, c; int i;

		starttime.setToNow(); //set up time

		//main loop
		while (runMore.equals("yes")) { 
                   //check if new command on agent's output link
                   s = soarSessionEval("set g_newmoves");
		   //if new commands exist, get them from agent and pass to robot
		   if (! s.equals(""))
		     for(i=Integer.parseInt(s); i>0; i--) {
			c = soarSessionEval("getCommandName " + i);
			passCommandToRobot(c);
			soarSessionEval("setCommandComplete");
		     }
		   //get the basic robot data and pass it to the agent
		   passDataToAgent();
		   //get the bumper data and pass it to the agent
		   passBumperDataToAgent();
		   //get the sonar data and pass it to the agent
		   passSonarDataToAgent();
		   //get the vision data and pass it to the agent
		   passVisionDataToAgent();
		   //tell agent to run until new commands on output-link
		   try {
		      m_ss.runTillOutput();
		   }
		   catch (TclEvalException tee) {
		     System.out.println("Soar cannot run");
		   }
		   //wait
		   ArUtil.sleep(100);
		}

		robot.stop();
		robot.disconnect();
		soarSessionEval("exit");
        }

// -------------------------------------------------------------------

	public void quit() {

		runMore = new String("no");

	}

// -------------------------------------------------------------------

	private void passCommandToRobot(String com) {

		if (com.equals("enableMotors")) {
			robot.enableMotors();
		}
		else if (com.equals("disableMotors")) {
			robot.disableMotors();
		}
		else if (com.equals("stop")) {
			robot.stop();
		}
		else if (com.equals("move")) {
			String p1 = soarSessionEval("getParam1");
			robot.move(Double.parseDouble(p1));
		}
		else if (com.equals("setDeltaHeading")) {
			String p1 = soarSessionEval("getParam1");
			robot.setDeltaHeading(Double.parseDouble(p1));
		}
		else if (com.equals("setHeading")) {
			String p1 = soarSessionEval("getParam1");
			robot.setHeading(Double.parseDouble(p1));
		}
		else if (com.equals("setMaxRotVel")) {
			String p1 = soarSessionEval("getParam1");
			robot.setMaxRotVel(Double.parseDouble(p1));
		}
		else if (com.equals("setRotVel")) {
			String p1 = soarSessionEval("getParam1");
			robot.setRotVel(Double.parseDouble(p1));
		}
		else if (com.equals("setVel")) {
			String p1 = soarSessionEval("getParam1");
			robot.setVel(Double.parseDouble(p1));
		}
		else if (com.equals("setVel2")) {
			String p1 = soarSessionEval("getParam1");
			String p2 = soarSessionEval("getParam2");
			robot.setVel2(Double.parseDouble(p1),Double.parseDouble(p2));
		}
		else if (com.equals("setMaxTransVel")) {
			String p1 = soarSessionEval("getParam1");
			robot.setMaxTransVel(Double.parseDouble(p1));
		}
		else if (com.equals("pan")) {
			String p1 = soarSessionEval("getParam1");
			ptu.pan(Integer.parseInt(p1));
		}
		else if (com.equals("tilt")) {
			String p1 = soarSessionEval("getParam1");
			ptu.tilt(Integer.parseInt(p1));
		}

		//need to look at:
		//moveTo(ArPose,ArPose)
		//setDeadReconPose(ArPose)

	}

// -------------------------------------------------------------------

	public String soarSessionEval(String p) {

		String res = "";

		try {
			res = m_ss.eval(p);
		}
		catch (TclEvalException tee) {
		  System.out.println("SoarRobot cannot eval " + p);
		}

		return res;

	}

// -------------------------------------------------------------------

	//get the values from the robot & pass them to agent

	private void passDataToAgent() {
		setAgentInfo("timestamp", starttime.mSecSince());
		robot.lock();
		setAgentInfo("X", robot.getX());
		setAgentInfo("Y", robot.getY());
		setAgentInfo("Th", robot.getTh());
		setAgentInfo("Vel", robot.getVel());
		setAgentInfo("RotVel", robot.getRotVel());
		setAgentInfo("RobotRadius", robot.getRobotRadius());
		setAgentInfo("RobotDiagonal", robot.getRobotDiagonal());
		setAgentInfo("BatteryVoltage", robot.getBatteryVoltage());
		setAgentInfo("LeftVel", robot.getLeftVel());
		setAgentInfo("RightVel", robot.getRightVel());
		setAgentInfo("StallValue", robot.getStallValue());
		setAgentInfo("isLeftMotorStalled", robot.isLeftMotorStalled());
		setAgentInfo("isRightMotorStalled", robot.isRightMotorStalled());
		setAgentInfo("Control", robot.getControl());
		setAgentInfo("Flags", robot.getFlags());
		setAgentInfo("areMotorsEnabled", robot.areMotorsEnabled());
		setAgentInfo("areSonarsEnabled", robot.areSonarsEnabled());
		setAgentInfo("MaxTransVel", robot.getMaxTransVel());
		setAgentInfo("MaxRotVel", robot.getMaxRotVel());
		setAgentInfo("MoveDoneDist", robot.getMoveDoneDist());
		setAgentInfo("DirMotPrecTime", robot.getDirectMotionPrecedenceTime());
		//these two need a parameter
		setAgentInfo("isHeadingDone", robot.isHeadingDone(0.0));
		setAgentInfo("isMoveDone", robot.isMoveDone(0.0));

		setAgentInfo("newdata", "yes"); //set agent flag to create new wmes
		robot.unlock();

	}

// -------------------------------------------------------------------

	//get bumper data, extract individual bumpers and pass to agent

	private void passBumperDataToAgent () {

		long bumpValue = robot.getNumFrontBumpers();

		setAgentInfo("FrontBump0", bumpValue & bit1);
		setAgentInfo("FrontBump1", bumpValue & bit2);
		setAgentInfo("FrontBump2", bumpValue & bit3);
		setAgentInfo("FrontBump3", bumpValue & bit4);
		setAgentInfo("FrontBump4", bumpValue & bit5);

		bumpValue = robot.getNumRearBumpers();

		setAgentInfo("RearBump0", bumpValue & bit1);
		setAgentInfo("RearBump1", bumpValue & bit2);
		setAgentInfo("RearBump2", bumpValue & bit3);
		setAgentInfo("RearBump3", bumpValue & bit4);
		setAgentInfo("RearBump4", bumpValue & bit5);

	}

// -------------------------------------------------------------------

	// get sonar data from robot and pass to agent

	private void passSonarDataToAgent() {

		int i, reading; boolean isNew; 
		String param;

		setAgentInfo("g_sonardata", "{}");

		for(i=0; i < numberOfSonars; i++) {

		  try {
		    reading = robot.getSonarRange(i);
		    isNew = robot.isSonarNew(i);
		    if (isNew == true) param = "true";
		       else param = "false";
		    m_ss.eval("newSonarData " + i + " " + reading + " " + isNew);
		  }
		  catch (TclEvalException tee) {
		    System.out.println("sonar info error for sonar " + i);
		  }
		}
	}

// -------------------------------------------------------------------

	// get vision data from robot and pass to agent

	private void passVisionDataToAgent() {

	}

// -------------------------------------------------------------------

	public void update(int action, int code) {

		// Return immediately
	}

// -------------------------------------------------------------------

	public void finalize() {

		System.out.println("SoarRobot finalize() running");
		try {
			m_ss.stop();
		}
		catch (TclEvalException tee) {
			// ignore it
		}
	}

// -------------------------------------------------------------------

	private void setAgentInfo(String key, int i) {

		String value = Integer.toString(i);
		setAgentInfo(key, value);
	}

// -------------------------------------------------------------------

	private void setAgentInfo(String key, double d) {

		String value = Double.toString(d);
		setAgentInfo(key, value);
	}

// -------------------------------------------------------------------

	private void setAgentInfo(String key, boolean b) {

		String value;
		if (b == true) 
			value = "true";
		else
			value = "false";
		setAgentInfo(key, value);
	}

// -------------------------------------------------------------------

	private void setAgentInfo(String key, String value) {

		String cmd = "set " + key + " " + value;
		try {
			m_ss.eval(cmd);
		}
		catch (TclEvalException tee) {
			System.out.println("setAgentInfo error for key = " + key);
		}
	}

// -------------------------------------------------------------------

	// So that Tcl/Soar code can distinguish between simulation and
	// interactive testing.
	private void setSimulation() {

		final String SET_SIMULATION = "set g_simulation 1;";

		try {
			m_ss.eval(SET_SIMULATION);
		}
		catch (TclEvalException tee) {
			System.out.println("setSimulation() error: " + tee);
		}
	}

// -------------------------------------------------------------------

	private void writeHeading() {

		final String LINE =
"-------------------------------------------------------------------------------";

		System.out.println(LINE);
		System.out.println(LINE);
		System.out.println("SoarRobot starting at " + new Date());
		System.out.println(LINE);
		System.out.println(LINE);
	}

// -------------------------------------------------------------------

}