package ch.jp.robwar;


/**
 *
 * @author JP Martin
 * @date 8.28.1998
 */
public class Sprinkler implements RobotBrain {
  
  /**
   * The point we really, absolutely want to reach
   */
  private Position targetPoint = new Position(2.0,2.0);
  
  /**
   * This internal variable is used to remember where the robot
   * wants to go.
   */
  private double targetAngle = 270.0;
  
  private int state= 0;
  private int subState=0;
  
  /**
   * This internal variable, when !=0, forces the robot to back up.
   */
  private int recule = 0;
  
  /**
   * This allows the robot brain to remember its past actions.
   */
  private Instructions previousInstr;
  
  /**
   * This function is called by the simulation for each time step.
   * It returns the orders that the robot brain gives to the robot body.
   */
  public Instructions nextMove(Feedback senses) {    
  
  
    // if we hit an obstacle, avoid it
    if (state!=0 && senses.stimuli.hitObstacle) {
      subState=0;
      state=0;       
    }  
  
    if (state==0) {  // getting past an obstacle
    
      // if I hit a wall, turn
      if (senses.stimuli.hitObstacle) {
         subState=0; 
         return new Instructions(5,0.01,false);
      } else {
         if (subState<5) { // go straight for a while
           subState++;
           return new Instructions(0.0,0.1,false);
         } else {            
           // otherwise, turn towards the target point
           double dx;
           double dy;
           double angleRad; 
           
           if (senses.selfInfo.position.x==targetPoint.x) {
             if (senses.selfInfo.position.y<=targetPoint.y) angleRad=3*java.lang.Math.PI/2;
             else angleRad=java.lang.Math.PI/2; // straight up
           } else {
             angleRad=java.lang.Math.atan(
              (targetPoint.y-senses.selfInfo.position.y)/(senses.selfInfo.position.x-targetPoint.x)
             );
             if (targetPoint.x<senses.selfInfo.position.x) {
               // 180 degree turn if we want to go west
               angleRad=java.lang.Math.PI+angleRad;
             }
           }
           
           targetAngle=angleRad*180/java.lang.Math.PI;
           if (targetAngle<0) targetAngle+=360;
           if (targetAngle>=360) targetAngle-=360;
           
           state=1; // turning towards targetAngle
         }
      }             
    }
    
    if (state==1) { // turn towards the point, and forward
      double rotation;
      rotation=targetAngle-senses.selfInfo.orientation;
      if (rotation>180) rotation-=360;
      if (rotation>180) rotation-=360;
      if (rotation<-180) rotation+=360;
      if (rotation<-5) rotation=-5;
      if (rotation>5) rotation=5;
      
      // if necessary, turn
      if (rotation!=0) return new Instructions(rotation,0.0,false);      
      
      // otherwise, go forward
      double spd;
      spd= (senses.selfInfo.position.x-targetPoint.x)*(senses.selfInfo.position.x-targetPoint.x)
          +(senses.selfInfo.position.y-targetPoint.y)*(senses.selfInfo.position.y-targetPoint.y);
      if (spd>0.01) spd=0.01;
      spd = java.lang.Math.sqrt(spd);
      
      if (spd>0.001) return new Instructions(0.0,spd,false);
      else state=2;
    }
       
    if (state==2) { // shover everyone with shots
      return new Instructions(5.0,0.0,true);
    }
    
    // error case
    state=0;
    // don't try this at home
    // System.out.println("Sprinkler error");
    return new Instructions();
        
  }
  
}