package cz.cuni.amis.pogamut.usar2004.samples;
   
   import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
   import cz.cuni.amis.pogamut.base.utils.guice.AgentScoped;
   import cz.cuni.amis.pogamut.usar2004.agent.*;
   import cz.cuni.amis.pogamut.usar2004.agent.module.logic.USAR2004BotLogicController;
   import cz.cuni.amis.pogamut.usar2004.communication.messages.usarcommands.DriveSkid;
   import cz.cuni.amis.pogamut.usar2004.communication.messages.usarcommands.Initialize;
   import cz.cuni.amis.pogamut.usar2004.communication.messages.usarinfomessages.NfoMessage;
  import cz.cuni.amis.pogamut.usar2004.communication.messages.usarinfomessages.SensorMessage;
  import cz.cuni.amis.pogamut.usar2004.utils.*;
  import java.util.ArrayList;
  import java.util.logging.Level;
  
  /**
   *
   * P2DX robot utilizing logic controller. It drives to the most open direction
   * and reverses if an obstacle is too close.
   *
   * @author vejmanm
   */
  @AgentScoped
  public class P2DXLogicController extends USAR2004BotLogicController
  {
      /**
       * Method triggered after Game is initialized and STARTPOSES obtained. Tt
       * will spawn the robot into the environment.
       *
       * @param nfom NfoMessge containing STARTPOSES
       */
      @Override
      public void robotInitialized(NfoMessage nfom)
      {
          getAct().act(new Initialize("USARBot.P2DX", "P2DX - logic controller", nfom.getStartPoses().get(0).getName()));
      }
  
      @Override
      public void initializeController(USAR2004Bot bot)
      {
          super.initializeController(bot);
      }
  
      /**
       * Initialization of modules used within this robot.
       *
       * @param bot Necessary parameter for hooking listeners and for sending
       * commands
       */
      @Override
      public void prepareBot(USAR2004Bot bot)
      {
          super.prepareBot(bot);
  
          //laserSensor = new SensorLaser(bot);
          log = new USAR2004BotLogicController();
          getWorldView().addEventListener(SensorMessage.class, sensorList);
  
          //Map<Class, Map<WorldObjectId, IViewable>> allVisible = getWorldView().getAllVisible();
      }
      private USAR2004BotLogicController log;
  
      /**
       * this method is triggered by receipt of STA message. This is where all
       * behaviour is controlled
       */
      @Override
      public void logic()
      {
          System.out.println("P2DXLogicController.logic()");
          getAct().act(new DriveSkid(left, right, false, false, false));
      }
      IWorldEventListener<SensorMessage> sensorList = new IWorldEventListener<SensorMessage>()
      {
          @Override
          public void notify(SensorMessage t)
          {
              if(t.getLaserRanges().size() > 1)
              {
                  ArrayList<Double> f = new ArrayList<Double>();
                  f.addAll(t.getLaserRanges());
                  RangesReady(f);
              }
          }
      };
      int left = 0, right = 0;
  
      /**
       * Sets higher speed for right motor and lower speed for left motor.
       */
      private void driveLeft()
      {
          right = 15;
          left = 5;
      }
  
      /**
       * Sets higher speed for left motor and lower speed for right motor.
       */
      private void driveRight()
     {
         right = 4;
         left = 15;
     }
 
     /**
      * Sets equal speeds for both side motors.
      */
     private void driveStraight()
     {
         right = 15;
         left = 15;
     }
 
     /**
      * For ten cycles it reverses slowly backwards and then for ten cycles it
      * turns around.
      */
     private void driveBackwards()
     {
         if(step > 10)
         {
             right = -3;
             left = -3;
         }
         else
         {
             right = -3;
             left = 3;
         }
         step--;
         if(step == 0)
         {
             step = 20;
             reversActive = false;
         }
 
     }
     private int step = 20;
     boolean reversActive = false;
 
     /**
      * Devides ranges into thirds and decides which way to set motor speeds.
      *
      * @param Ranges list of ranges from Range scanner.
      */
     public void RangesReady(ArrayList<Double> Ranges)
     {
 
         int third = Ranges.size() / 3;
         double mleft = 0, mright = 0, mstraight = 0;
         for(int i = 0; i < Ranges.size(); i++)
         {
             if(i < third)
             {
                 mright += Ranges.get(i);
             }
             else if(i < 2 * third)
             {
                 mstraight += Ranges.get(i);
             }
             else
             {
                 mleft += Ranges.get(i);
             }
         }
         if(reversActive || mleft + mstraight < 200 || mright + mstraight < 200)
         {
             reversActive = true;
             driveBackwards();
         }
         else
         {
             if(Math.max(mleft, mright) == mleft)
             {
                 if(Math.max(mleft, mstraight) == mleft)
                 {
                     driveLeft();
                 }
                 else
                 {
                     driveStraight();
                 }
             }
             else
             {
 
                 if(Math.max(mright, mstraight) == mright)
                 {
                     driveRight();
                 }
                 else
                 {
                     driveStraight();
                 }
             }
         }
     }
 
     public static void main(String[] args)
     {
         new USAR2004BotRunner(P2DXLogicController.class, "PogamutBotBot", "127.0.0.1", 3000).setMain(true).setLogLevel(Level.WARNING).startAgent();
     }
 }