package adaptive.agents.drivers;

import adaptive.ports.PioneerSonarData;
import adaptive.core.*;
import comms.core.*;
import adaptive.core.*;
import java.net.*;
import java.io.*;

/*****************************************************************************
 * <!
 * Copyright 1999, Institute for Complex Engineered Sytems,
 *                 Carnegie Mellon University
 *
 * DESCRIPTION:
 * >
 * I took the original PBASonarDriver and made it output an object containing
 * the 8 sonar values as well as their respective valid bits.  The output is an 
 * object of type SonarData as defined in adaptive.ports.SonarData
 *
 * The determination of the valid bit has been moved into the SonarData object.
 * The valid bit is set or cleared when the corresponding reading element is
 * written to.
 *
 * This should save some wiring in the GUI. though it is probably more 
 * overhead, I still think it's better. 
 * 
 * 
 * @author Rich Malak <A HREF="mailto:rmalak@cs.cmu.edu">rmalak@cs.cmu.edu</A>
 *         <br> </br>
 *
 *
 * REVISION HISTORY:
 *
 * Revision 1.0 99/6/29 rmalak
 * Guts mostly taken from PBASonarDriver
 * $Log: PBAPioneerSonarDriver.java,v $
 * Revision 1.1.2.4  2000/02/28 06:41:15  jrj
 * added state field to change the RAVEname in ModuleManager.  Note:  if
 * the state is null or that field is equal to "" it defaults to the old
 * way of setting the RAVEname.  I also intentionally dump a state of
 * null, so moving won't be interfered with by this change.
 *
 * 
 ****************************************************************************/
public class PBAPioneerSonarDriver extends PBSAgent implements Driver
{

  public final static int     DATA_PORT = 0;
  
  protected final static String SONAR_TOKEN = "SONAR";

  //this guy is just used for the message passing
  //we could do without it, but it's quite simple this way
  protected String ma_sonarID[];  

  //we don't strictly have to use a member for this, 
  //but, again, this way is simple to do...
  protected PioneerSonarData m_SData;
  protected String raveName;

  /************************************************************************/
  /* Here's a sketch of what this agent looks like:                       */
  /*                --------------
		    |            |
		    | CyberRAVE  |---> PioneerSonarData Object 
                    | sonar vals |   (see adaptive.ports.SonarData)
                    |            |   
		    |            |
		    --------------
  */
  /************************************************************************/

  /**********************************************************************
							 
   Set number of inputs and outputs.									 
   @param 

   @return 

   @exception 

  **********************************************************************/
    protected void initialize()
	{

	    setNumInputsOutputs( 0, 1 );

	    /* I don't need my own thread since I'm a silly bitch! */
	    setNoThread();
	    setOutputPortType( adaptive.ports.PioneerSonarData.class, DATA_PORT );
	    //            setOutputPortType( java.lang.Boolean.class, VALID_PORT );
	} /* initialize() */

  /**********************************************************************

   Allocate any internal storage we need.

   @param 
   
   @return 
   
   @exception 

  **********************************************************************/
    protected void allocateInternals() 
	{
	  //this is the actual data
	  m_SData = new PioneerSonarData();

	  //this is to help with messaging
	  ma_sonarID = new String[m_SData.length()];
	  for (int i=0; i<m_SData.length(); i++) {
	    ma_sonarID[i] = "s"+i;
	  }
	} /* allocateInternals() */


  /**********************************************************************

   Restore our internal state.

   @param 
   
   @return 
      @exception 

  **********************************************************************/
  protected boolean processInternalState(Object state){
    boolean retval=true;
    Object[] statearr;
    if (state!=null){
      try{
	statearr=(Object[])state;
	this.raveName=(String)statearr[0];
	if (this.raveName.equals("")){
	  defaultInternals();
	}
      }catch(Exception e){ 
        e.printStackTrace();
	retval=false; 
	defaultInternals(); 
      }
    }
    else{
      defaultInternals();
    }
    return (retval);
  } /* processInternalState() */

  public void defaultInternals(){
    /* get the DNS name of the localhost */
    Object[] arr=(Object[])getDefaultState();
    this.raveName=(String)arr[0];
  }
  public Object getStateDescription(){
    String[] descriptors=new String[1];
    descriptors[0]=new String("Robot's RAVE name");
    return descriptors;
    //    return new String("Robot's RAVE name");
  }
  public Object getDefaultState(){
    Object[] state=new Object[1];
    String raveDummy;
    try{
      String fullLocalHost = adaptive.core.net.LocalHost.getFullLocalHostName();
      raveDummy = fullLocalHost.substring(0,fullLocalHost.indexOf((int) '.')); 
    }catch( UnknownHostException e ){ 
      System.out.println( "Unknown localhost!!" );
      raveDummy="unknown";
    }
    state[0]=raveDummy;
    return state;
  }
  /**********************************************************************

   Save internal state. No real state to save so don't do anything.

   @param 
   
   @return 
   
   @exception 

  **********************************************************************/
  protected Object dumpState()
	{
	    //I'm intentionally not dumping the state so that having the raveName in the state won't prevent moving.
	    return null;
	}

  /**********************************************************************

   Set initial output values.

   @param 
   
   @return 
   
   @exception 

  **********************************************************************/
    protected boolean initOutputVals() 
	{
	  //note that the valid bits will be cleared if the 
	  //value passed to the constructor is an invalid one   
	  setOutput( new PioneerSonarData(-1.0f) , DATA_PORT );
	  return true;

	} /* initOutputVals() */


  /**********************************************************************

   initializes comms stuff.

   @param 
   
   @return 
   
   @exception 

  **********************************************************************/
  public void initMessages() {
    try {

      registerForMessage( raveName, SONAR_TOKEN );
      
      /* tell the dummy robot that I'm alive */
      sendMessage( raveName, new Message( "COMMANDER" ) );
      sendMessage (raveName, new Message( "SETSONAR 1"));
    }

    catch( IOException ioe) {
      System.out.println( "Communications send error.");
      System.out.println( ioe.getMessage());
    }
  } /* initMessage */
  /******************************************************************
   * <!Description:>
   *
   * This method is called inside stop().
   * We need to stop the tanks motors just in case.
   *
   *
   * @param 
   *
   * @return 
   *
   * @exception 
   ******************************************************************/
  protected void endMessages(){
    try { 

      System.out.println("Before send endMessage");

      sendMessage( raveName,
                   new Message("SETSONAR 0" ));//turn sonars off
      System.out.println("After send endMessage");
    }
    catch(IOException ioe) { 
    }
    catch(NoPhoneNumberException npne) { 
    }
  }



 /**********************************************************************

   Deallocate internal variables. Nothing to deallocate. Do nothing.

   @param 
   
   @return 
   
   @exception 

  **********************************************************************/
  protected void deAllocateInternals() {
    //no internals to deallocate
  }
  

  /**********************************************************************

   receives a message from the dummy robot.

   @param 
   
   @return 
   
   @exception 

  **********************************************************************/
    public synchronized void receiveMessage( Message m ) {
      Float reading;
      /* OK, I need to parse this message and grad the float after my ID */
      for (int i=0; i<m_SData.length(); i++) {
	String temp=m.getMessage();
	//	System.out.println(temp);
	String dummy = temp.substring(temp.indexOf( ma_sonarID[i]+" " ) + ma_sonarID[i].length() + 1 );
	//	System.out.println("Message:"+dummy.substring(0,dummy.indexOf(' ')));
	try{
	  reading = new Float( dummy.substring( 0, dummy.indexOf(' ') ) );
	  //System.out.println(dummy);
	  // System.out.println("s"+i+">"+reading);
	}catch(NumberFormatException nfe){
	  System.out.println("Caught number format exception in parsing sonar message for "+i+":"+dummy.substring( 0, dummy.indexOf(' ') ));
	  System.out.println(temp);
	  reading=new Float(-1);
	}
       	
	m_SData.update(reading.floatValue(), i);
      }

      setOutput( new PioneerSonarData(m_SData), DATA_PORT);
	   
    } /* receiveMessage() */

      


} /* class PBASonarPreprocessor */