package cz.cuni.amis.pogamut.usar2004.samples;
   
   import cz.cuni.amis.pogamut.base.utils.guice.AgentScoped;
   import cz.cuni.amis.pogamut.base3d.worldview.object.*;
   import cz.cuni.amis.pogamut.usar2004.agent.*;
   import cz.cuni.amis.pogamut.usar2004.agent.module.configuration.*;
   import cz.cuni.amis.pogamut.usar2004.agent.module.datatypes.*;
   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.agent.module.master.SensorMasterModule;
  import cz.cuni.amis.pogamut.usar2004.agent.module.master.ConfigMasterModule;
  import cz.cuni.amis.pogamut.usar2004.agent.module.master.GeometryMasterModule;
  import cz.cuni.amis.pogamut.usar2004.agent.module.master.ResponseModule;
  import cz.cuni.amis.pogamut.usar2004.agent.module.master.SensorSpecificModule;
  import cz.cuni.amis.pogamut.usar2004.agent.module.master.StateMasterModule;
  import cz.cuni.amis.pogamut.usar2004.agent.module.state.SuperState;
  import cz.cuni.amis.pogamut.usar2004.communication.messages.usarcommands.*;
  import cz.cuni.amis.pogamut.usar2004.communication.messages.usarinfomessages.NfoMessage;
  import cz.cuni.amis.pogamut.usar2004.utils.*;
  import cz.cuni.amis.utils.exception.PogamutException;
  import java.util.logging.Level;
  
  /**
   * This robot is designed for the map ONS-PointLookOut-250
   *
   * @author vejmanm
   */
  @AgentScoped
  public class Submarine extends USAR2004BotLogicController<USAR2004Bot>
  {
      /**
       * 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)
      {
          super.robotInitialized(nfom);
          logicInitialize(logicModule);
          //getAct().act(new GetStartPoses());
          getAct().act(new Initialize("USARBot.Submarine", "P2DX sample robot", new Location(229.31, -651.48, 12.42), null));
          System.out.println(logicModule.isRunning());
  
      }
  
      @Override
      public void initializeController(USAR2004Bot bot)
      {
          super.initializeController(bot);
          //startPoses = new NfoStartPoses(bot);
          //begin = new NfoBeginMapInfo(bot);
      }
      private final double descendingLatch = 0.05f;//how fast should it drop the level while descending
      private final double descendingRate = 0.5f;//how much severly should it correct the level by steering the sternPlane
      private final double steerRate = 0.4f;//how much should it steer
      private final int submarineLevel = 4;//how far from the bottom of the sea should it try to keep the sub.
      //properties needed for logic run:
      private boolean parametersObtained = false;
      private double maxRudderAngle = 0;
      private double maxSternPlaneAngle = 0;
      private double maxPropellerSpeed = 0;
      private double cameraMaxFov = 0;
      private double cameraMinFov = 0;
      private double cameraActFov = 0;
      private double cameraInc = 0.01f;
      private String cameraName = "";
      private String cameraType = "";
      private double laserRange = 0;
      private double laserTime = 0;
      private double previousLaserRange = 0;
      private double previousLaserTime = 0;
      private double cameraRotationSpeed = 0;
      private double cameraTilt = 0;
      private double cameraTiltInc = cameraInc;
      private double cameraMaxTilt = 0;
      private double cameraMinTilt = 0;
      private String mispkgName = "";
      private double time = 0;
      //works till the battery is dead
  
      /**
       * this method is triggered by receipt of STA message. This is where all behaviour is controlled
       * @throws PogamutException 
       */
      @Override
      public void logic() throws PogamutException
      {
          super.logic();
  
          obtainLaserRange();
  
          double steer = 0;
          if(time % 200 < 100)
          {
              steer = steerRate;
          }
          else
          {
             steer = -steerRate;
         }
 
         if(laserRange > submarineLevel)
         {
             double desc = 0;
             if(previousLaserRange - laserRange < descendingLatch)
             {//either descend/rise or steer! - either correct the level or steer
                 desc = descendingRate;
                 steer = 0;
             }
             bot.getAct().act(new DriveNautic(maxPropellerSpeed, maxRudderAngle * steer, maxSternPlaneAngle * desc, false, false));
         }
         else
         {
             //if sub is dropping it should go up, else she should steer a littlebit
             double desc = 0;
             if(previousLaserRange - laserRange > 0)
             {
                 desc = descendingRate;
                 steer = 0;
             }
             if(laserRange < 0.5)
             {
                 bot.getAct().act(new DriveNautic(-maxPropellerSpeed, maxRudderAngle * -Math.signum(steer), -maxSternPlaneAngle, false, false));
             }
             else
             {
                 bot.getAct().act(new DriveNautic(maxPropellerSpeed, maxRudderAngle * steer, -maxSternPlaneAngle * desc, false, false));
             }
         }
         System.out.println(laserRange);
         System.out.println(previousLaserRange);
         System.out.println(time);
 
         this.time = staModule.getStatesByClass(SuperState.class).get(0).getTime();
 
         if(!parametersObtained)
         {
             getConfig();
         }
         else
         {
             zoomCamera();
             rotateCamera();
         }
     }
 
     /**
      * Sets constant speed of camera roatation and repeatedly tilts the camera
      * from min angle to max.
      */
     private void rotateCamera()
     {
         if(cameraTilt >= cameraMaxTilt || cameraTilt <= cameraMinTilt)
         {
             cameraTiltInc *= -1;
         }
         cameraTilt += cameraTiltInc;
         //{Order int} ‘int’ is optional (0 by default) and indicates the control mode. ‘0’ means angle control, ‘1’ means speed control, and ‘2’ means torque control. 
         int[] links = new int[]
         {
             1, 2
         };
         double[] values = new double[]
         {
             cameraRotationSpeed, cameraTilt
         };
         int[] orders = new int[]
         {
             1, 0
         };
         bot.getAct().act(new MissionPackage(mispkgName, links, values, orders));
     }
 
     /**
      * Repeatedly sets the FOV of the camera from one extreme to another.
      */
     private void zoomCamera()
     {
         if(cameraActFov >= cameraMaxFov || cameraActFov <= cameraMinFov)
         {
             cameraInc *= -1;
         }
         cameraActFov += cameraInc;
         bot.getAct().act(new SetCamera(cameraType, cameraName, cameraActFov));
     }
 
     /**
      * Checks for new messages from sensor module and updates current and previous readings.
      */
     private void obtainLaserRange()
     {
         if(senModule.isReady())
         {
             if(senModule.getSensorsBySensorType(SensorType.LASER_SENSOR).size() > 0)
             {
                 SensorLaser range = (SensorLaser) senModule.getSensorsBySensorType(SensorType.LASER_SENSOR).get(0);
                 //sampling is slower than calling this method, so range has potentially old news. to prevent spoiling the lastRange we make sure, that we read the laserRange just as fresh
                 previousLaserTime = laserTime;
                 laserTime = range.getTime();
                 if(previousLaserTime != laserTime)
                 {
                     previousLaserRange = laserRange;
                 }
                 laserRange = range.getRangeAt(0);
             }
         }
     }
 
     /**
      * Obtaining of configuration information in several steps. It is unable to obtain all data in one logic cycle.
      */
     private void getConfig()
     {
         if(!confModule.isReady())
         {
             //get properties from server about the robot and the camera
             confModule.queryConfigurationByType("Robot");
         }
         else if(confModule.getNonEmptyDescription().size() < 2)
         {
             //if I write those two queries just one after another it will not be quick enough to record the second query. So i could go ahead and write sth. like Thread.Sleep(10); or something...
             confModule.queryConfigurationByType("Camera");
         }
         else if(confModule.getNonEmptyDescription().size() < 3)
         {
             confModule.queryConfigurationByType("MisPkg");
         }
         else//once we've got them lets save values and say that we are good to go.
         {
             ConfigNautic robotCfg = (ConfigNautic) confModule.getConfigurationsByConfigType(ConfigType.NAUTIC_VEHICLE).get(0);
             for(String feature : robotCfg.getFeatureNames())
             {
                 System.out.println(feature + ": " + robotCfg.getFeatureValueBy(feature));
             }
             ConfigSensor cameraCfg = (ConfigSensor) confModule.getConfigurationsByType("Camera").get(0);
             for(String feature : cameraCfg.getFeatureNames())
             {
                 System.out.println(feature + ": " + cameraCfg.getFeatureValueBy(feature));
             }
             ConfigMissionPackage mispkgConfig = (ConfigMissionPackage) confModule.getConfigurationsByConfigType(ConfigType.MISSION_PACKAGE).get(0);
 
 
             //getParameters
             maxRudderAngle = Float.parseFloat(robotCfg.getFeatureValueBy("MaxRudderAngle"));
             maxSternPlaneAngle = Float.parseFloat(robotCfg.getFeatureValueBy("MaxSternPlaneAngle"));
             maxPropellerSpeed = Float.parseFloat(robotCfg.getFeatureValueBy("MaxPropellerSpinSpeed"));
 
             cameraMaxFov = Float.parseFloat(cameraCfg.getFeatureValueBy("CameraMaxFov"));
             cameraMinFov = Float.parseFloat(cameraCfg.getFeatureValueBy("CameraMinFov"));
             cameraActFov = Float.parseFloat(cameraCfg.getFeatureValueBy("CameraDefFov"));
 
             cameraName = cameraCfg.getName();
             cameraType = cameraCfg.getType();
 
 
             mispkgName = mispkgConfig.getName(); //name of the missionpackage which the camera is based on.
             cameraRotationSpeed = mispkgConfig.getMaxSpeedAt(0); //max pan speed
             cameraMaxTilt = mispkgConfig.getMaxRangeAt(1);//max tilt
             cameraMinTilt = mispkgConfig.getMinRangeAt(1);//min tilt
 
             //setFlag
             parametersObtained = true;
             //data from the first run - got from SystemConsole thanks to those for cycles above these properties:
             //MaxRudderAngle: 0.4363
             //MaxSternPlaneAngle: 0.4363
             //MaxPropellerSpinSpeed: 6.2831
             //PropellerPitch: 0.3048
             //CameraMaxFov: 2.0943
             //CameraMinFov: 0.3491
             //CameraDefFov: 0.7853
         }
     }
 
     /**
      * 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);
 
         senModule = SensorMasterModule.getModuleInstance(bot);
         geoModule = GeometryMasterModule.getModuleInstance(bot);
         confModule = ConfigMasterModule.getModuleInstance(bot);
         resModule = ResponseModule.getModuleInstance(bot);
         staModule = StateMasterModule.getModuleInstance(bot);
 
         //laserOnly=new SensorSpecificModule<SensorLaser>(bot, SensorLaser.class);
         //initModule = new InitializeOnceOnStartPoses(bot, "USARBot.Submarine", "P2DX sample robot");
         //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 InitializeOnceOnStartPoses initModule;
     private SensorMasterModule senModule;
     private GeometryMasterModule geoModule;
     private ConfigMasterModule confModule;
     private ResponseModule resModule;
     private StateMasterModule staModule;
     private SensorSpecificModule<SensorLaser> laserOnly;
 
     public static void main(String[] args)
     {
         new USAR2004BotRunner(Submarine.class, "PogamutBotBot", "127.0.0.1", 3000).setMain(true).setLogLevel(Level.WARNING).startAgent();
     }
 }