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.agent.module.nfo.NfoStartPoses;
   import cz.cuni.amis.pogamut.usar2004.agent.module.sensor.SensorLaser;
   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.communication.messages.usarinfomessages.StateMessage;
  import cz.cuni.amis.pogamut.usar2004.utils.*;
  import java.util.ArrayList;
  import java.util.logging.Level;
  
  /**
   *
   * P2DX robot not using logic controller.
   *
   * @author vejmanm
   */
  @AgentScoped
  public class P2DX extends USAR2004BotController
  {
      /**
       * 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 sample robot", nfom.getStartPoses().get(0).getName()));
      }
  
      @Override
      public void initializeController(USAR2004Bot bot)
      {
          super.initializeController(bot);
  
          //startPoses = new NfoStartPoses(bot);
          //begin = new NfoBeginMapInfo(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);
  
          getWorldView().addEventListener(StateMessage.class, statList);
  
          //laserSensor = new SensorLaser(bot);
          log = new USAR2004BotLogicController();
          getWorldView().addEventListener(SensorMessage.class, sensorList);
          getWorldView().addEventListener(NfoMessage.class, nfoList);
  
          //Map<Class, Map<WorldObjectId, IViewable>> allVisible = getWorldView().getAllVisible();
      }
      //private NfoBeginMapInfo begin;
      private NfoStartPoses startPoses;
      private SensorLaser laserSensor;
      private USAR2004BotLogicController log;
  
      private void logic()
      {
  //        if(begin.isReady())
  //            System.out.println(begin.getLevel());
      }
      IWorldEventListener<StateMessage> statList = new IWorldEventListener<StateMessage>()
      {
          @Override
          public void notify(StateMessage t)
          {
              logic();
              getAct().act(new DriveSkid(left, right, false, false, false));
          }
      };
      IWorldEventListener<NfoMessage> nfoList = new IWorldEventListener<NfoMessage>()
      {
          @Override
          public void notify(NfoMessage t)
          {
              System.out.println("INFO INFO FOR STARTERS" + t.toString());
              if(!first)
              {
                  //INIT {ClassName USARBot.P2DX} {Location 4.5,1.9,1.8}
                  //getAct().act(new Initialize("USARBot.P2DX", "Blimp", new Location(4.5,1.9,1.8), null));
  
                  //getAct().act(new Initialize("USARBot.AirRobot", "Item" ,new Location(0.6903,4.7984, 7.8320),null));
                  first = true;
             }//new Location(-4.2861,40.1280,4.6800),null));//0.76,2.3,1.8),null));
         }
     };
     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);
             }
         }
     };
     boolean first = false;
     int left = 0, right = 0;
 
     /**
      * Sets higher speed for right motor and lower speed for left motor.
      */
     private void driveLeft()
     {
         System.out.println("left");
         right = 15;
         left = 5;
     }
 
     /**
      * Sets higher speed for left motor and lower speed for right motor.
      */
     private void driveRight()
     {
         System.out.println("right");
         right = 4;
         left = 15;
     }
 
     /**
      * Sets equal speeds for both side motors.
      */
     private void driveStraight()
     {
         System.out.println("straight");
         right = 15;
         left = 15;
     }
 
     /**
      * For ten cycles it reverses slowly backwards and then for ten cycles it
      * turns around.
      */
     private void driveBackwards()
     {
         System.out.println("back");
         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;
 
     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(P2DX.class, "PogamutBotBot", "127.0.0.1", 3000).setMain(true).setLogLevel(Level.WARNING).startAgent();
     }
 }