package adaptive.agents;

import adaptive.core.*;
import adaptive.support.*;
import adaptive.ports.*;


/****************************************************************************
 *
 * Copyright 1998, Institute for Complex Engineered Systems,
 *                 Carnegie Mellon University
 *
 * Author: Jonathan Jackson <jackson4@andrew.cmu.edu>
 *
 * Description:  make robot wander randomly, but avoid obstacles
 *
 * $Log: PBARandomWander.java &,v
 *
 **************************************************************/
public class PBARandomWander extends PBAgent
{
  static protected boolean DEBUG=false;
  final static float FREQ=2;//in Hz;
  final static int SONAR_INPUT=0;
  final static int MOTOR_COM=0;
     final static double[] MIN_DIST={0.1,0.35,0.7,0.75,0.75,0.7,0.35,0.1,0.1,0.2,0.55,0.5,0.5,0.55,0.2,0.1,};
    //final static double[] MIN_DIST={0.1,0.05,0.37,0.375,0.375,0.37,0.05,0.1,0.1,0.2,0.55,0.5,0.5,0.55,0.2,0.1,};
  final static float SPEED=0.25f;
  final static float W_SPEED=0.20f;
  final static int MAXFORWARD=600;

  int count=0;
  boolean straightTooLong=false;
  boolean turning=false;
  //put internal structure definitions here
  float desiredDirection=0;  //direction in degrees with 0 straight ahead
                      //counter-clockwise increase
  float actualDirection=0;

  float[] lastReading=new float[16];

/**********************************************************************
Description: Required default constructor required by Agent interface.
sets the number, type, and names for input and output ports
@param none
@return nada
@exception none
**********************************************************************/
  public void initialize(){
    setNumInputsOutputs(1,1);
    setInputPortType(RobotSonarData.class,SONAR_INPUT);
    setInputPortName("Sonar Data:",SONAR_INPUT);
    setOutputPortType(PioneerCommand.class,MOTOR_COM);
    setOutputPortName("Motor Commands",MOTOR_COM);
    this.setSleepTime(500);
  } /* initialize() */

/**********************************************************************
<! Description: > initializes output ports' values.
@param none
@return boolean (true=success),(false=failure)
@exception none
**********************************************************************/
  protected boolean initOutputVals(){
    setOutput(new PioneerCommand(0,0),MOTOR_COM);
    return( true );

  } /* initOutputVals() */
    

/**********************************************************************
Description: allocates data internals.
@param none
@return nada
@exception none
**********************************************************************/
  protected void allocateInternals(){

  } /* allocateInternals() */
	    
/**********************************************************************
Description: default data internals.
Used in case there is no state being set
@param none
@return nada
@exception none
**********************************************************************/
  private void defaultInternals(){
    
  } /* defaultInternals() */


/**********************************************************************
<! Description: > serializes my state into an object.
@param none
@return Object holding the state
@exception none
**********************************************************************/
  public Object dumpState(){
	    
    return (null);  
  } /* dumpState() */


/**********************************************************************
<! Description: > takes a dumped object and loads it into my state.
@param none
@return true == OK, false == bad.
@exception none
**********************************************************************/
  public boolean processInternalState( Object statearr ){
    boolean retval=true;
    /*    Object[] state;
    if (statearr!=null){ 
            try{
      }
    } 
    else{ 
      defaultInternals(); 
    }*/
    return (retval);
  } /* processInternalState() */


/**********************************************************************
Description: Required main loop of the agent.
     Called with each cycle of operation for the agent
@param none
@return nada
@exception none
**********************************************************************/
  public void runLoop(){
    PioneerCommand tc;
    RobotSonarData sonars;
    float v,w;


      //read sonar
      sonars=(RobotSonarData)getInput(SONAR_INPUT);

      //select action
      if (count>MAXFORWARD*FREQ){
	straightTooLong=true;
      }
      if (count <0){
	straightTooLong=false;
      }
      if (obstacleAt(sonars,10) ||
	  obstacleAt(sonars,-10) ||
	  obstacleAt(sonars,30) ||
	  obstacleAt(sonars,-30)||
	  obstacleAt(sonars, 50)||
	  obstacleAt(sonars, -50)//||
	  //  obstacleAt(sonars, -90)||
	  //obstacleAt(sonars,90)||
	  /*straightTooLong*/
	   ){
	if (!turning){
	  //turn away from obstacle in front of robot
	  desiredDirection=fitAngle(maxOpen(sonars));
	  if (Math.abs(desiredDirection)<0.5){
	    desiredDirection-=180*Math.random();
	    desiredDirection=fitAngle(desiredDirection);
	  }
	  if (!straightTooLong) count=0;
	  turning=true;
	}
	else if (straightTooLong){
	  count-=4;
	}
      }
      else{
	turning=false;
	count++;
      }

      //output action
      v=0;w=0;
      if (!turning){//go forward in arc w/ random direction
	v=SPEED;
	w=(Math.random()>0.5)?0.05f:-0.05f;
      }
      else if (desiredDirection>0){
	w=W_SPEED;
	//count=0;
      }
      else{
	w=-W_SPEED;
	//count=0;
      }
      tc=new PioneerCommand(v,w);
      setOutput(tc,MOTOR_COM);


    
  } /* runLoop() */
 
  private boolean obstacleAt(RobotSonarData sonars,float angle){
    int index=sonars.findClosestSonar(angle);

    PositionData temp=sonars.getPosition(index);

    float last=lastReading[index];
    lastReading[index]=sonars.getReading(index);
    return (sonars.isValid(index) && sonars.getReading(index)<this.MIN_DIST[index] && last<MIN_DIST[index]*1.1);
  }
  private float fitAngle(float angle){
    //fits angle within range of -180 to 180
    while (angle <-180) angle+=360;
    while (angle >180)  angle-=360;
    return angle;
  }
  /* private float maxOpen(RobotSonarData sonars){
    //take sum of angles of sonars, weighted by object distance
    float direction; 
    float sum=0; 
    float value; 
    PositionData temp; 
    float maxDist=0; 
    int maxSonar=0; 
    for (int i=0;i<sonars.length()/2;i++){ 
      temp=sonars.getPosition(i);  
      value=sonars.getReading(i); 
      if (!sonars.isValid(i)) value=2;
      sum+=value*temp.getT();  
      //    System.out.println("Sonar "+i+" angle="+temp.getT()+">"+sonars.getReading(i));
    }  
    direction=sum/sonars.length(); 
    //    for (int i=0;
    System.out.println("Max open direction is "+direction); 
    return (direction); 
  }*/
  private float maxOpen(RobotSonarData sonars){
    float angle;
    int i;
    int maxreading=0;
    if (Math.random()>0.5){
      maxreading=7;
    }
    float max=0.0f;
    float reading;
    for (angle=-90;angle<91;angle+=20){
      i=sonars.findClosestSonar(angle);
      reading=sonars.getReading(i);
      if (reading >2.5f) reading =-1;
      if (DEBUG) System.out.println(angle+" "+i+"::"+reading);
      if (reading>max){
	maxreading=i;
	max=reading;
      }
    }
    angle=sonars.getPosition(maxreading).getT();
    if (DEBUG) System.out.println("Max open direction is"+angle);
    return angle;
  }
  
}