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.geometry.GeoSensorEffecter;
   import cz.cuni.amis.pogamut.usar2004.agent.module.logic.USAR2004BotLogicController;
  import cz.cuni.amis.pogamut.usar2004.agent.module.master.*;
  import cz.cuni.amis.pogamut.usar2004.agent.module.sensor.SensorLaser;
  import cz.cuni.amis.pogamut.usar2004.agent.module.response.ResponseSensorEffecter;
  import cz.cuni.amis.pogamut.usar2004.agent.module.sensor.*;
  import cz.cuni.amis.pogamut.usar2004.communication.messages.datatypes.SensorMount;
  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.samples.AirScanner.Record;
  import cz.cuni.amis.pogamut.usar2004.samples.AirScanner.RiskLevel;
  import cz.cuni.amis.pogamut.usar2004.samples.AirScanner.ScanPreview;
  import cz.cuni.amis.pogamut.usar2004.samples.AirScanner.State;
  import cz.cuni.amis.pogamut.usar2004.samples.AirScanner.ToolBox;
  import cz.cuni.amis.pogamut.usar2004.utils.*;
  import cz.cuni.amis.utils.exception.PogamutException;
  import java.awt.geom.Point2D;
  import java.util.HashMap;
  import java.util.LinkedList;
  import java.util.List;
  import java.util.Map;
  import java.util.logging.Level;
  import javax.swing.SwingUtilities;
  
  /**
   * This robot is designed for the map DM-TallTestWorld_250 and is based on logic
   * bot controller
   *
   * @author vejmanm
   */
  @AgentScoped
  public class AerialVehicle 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);
          //INIT {ClassName USARBot.AirRobot} {Name AirRObot sample robot} {Location 209.30999755859375,-651.47998046875,12.420000076293945}
          //bot.getAct().act(new Initialize("USARBot.AirRobot", "AirRobot sample robot", new Location(209.30999755859375,-651.47998046875,12.420000076293945), null));
          logicInitialize(logicModule);
          getAct().act(new Initialize("USARBot.AirRobot", "AirRobot - aierial sample robot", nfom.getStartPoses().get(0).getName()));
          //getAct().act(new Trace("ALL"));
          System.out.println(logicModule.isRunning());
      }
  
      @Override
      public void initializeController(USAR2004Bot bot)
      {
          super.initializeController(bot);
          //startPoses = new NfoStartPoses(bot);
          //begin = new NfoBeginMapInfo(bot);
      }
      
      
      
      //-------------------------------------------------
      /*
       * This is the place to change the scanning area
       */
      //-------------------------------------------------
      private final Location destCenter = new Location(-49,-225);//(11, -50);//
      private final double destWidth = 774;//174;//
      private final double destHeight = 774;//174;//
      private final double seaLevel = 3;//-5; must be less or equal t zero
      
      
      private final Map<String, Point2D> highRiskAction = new HashMap<String, Point2D>()
      {
          
          {//name, linear and lateral velocity coeficient
              put("L4", new math.geom2d.Point2D(-0.95, 0.31));
              put("R4", new math.geom2d.Point2D(-0.95, -0.31));
              put("L3", new math.geom2d.Point2D(-0.81, 0.59));
              put("R3", new math.geom2d.Point2D(-0.81, -0.59));
              put("L2", new math.geom2d.Point2D(-0.59, 0.81));
              put("R2", new math.geom2d.Point2D(-0.59, -0.81));
              put("L1", new math.geom2d.Point2D(-0.31, 0.95));
              put("R1", new math.geom2d.Point2D(-0.31, -0.95));
              put("M0", new math.geom2d.Point2D(-1, 0));
          }
      };
      private final Map<String, Double> highRisk = new HashMap<String, Double>()
      {
          
          {
              put("L4", 0.47d);
             put("R4", 0.47d);
             put("L3", 0.61d);
             put("R3", 0.61d);
             put("L2", 0.86d);
             put("R2", 0.86d);
             put("L1", 1.22d);
             put("R1", 1.22d);
             put("M0", 1.5d);
         }
     };
     private final Map<String, Double> lowRisk = new HashMap<String, Double>()
     {
         
         {
             put("L4", 1.57d);
             put("R4", 1.57d);
             put("L3", 1.91d);
             put("R3", 1.91d);
             put("L2", 2.72d);
             put("R2", 2.72d);
             put("L1", 3.5d);
             put("R1", 3.5d);
             put("M0", 4.2d);
         }
     };
     private final String[] sonarOrder = new String[]
     {
         "L4", "L3", "L2", "L1", "M0", "R1", "R2", "R3", "R4"
     };
     private final Location zeroPoint = new Location(0, 0, 0);//when robot is spawned his location is noted and than subtracted from every read from the senzor, so this is actually the spawnLocation
     private final int numOfRays = 10;//number of rays used to measure the avg. altitude of the robot from the range scanner
     private final double minDensity = 15;// how far will the next row be    
     private final double scanAltitude = 6;
     private final int dockWaitPenalty = 8;
     //private final double rotationSpeed=0.2d;//rychlost otáčení při dokončení štráfku
     private final double scanningSpeed = 0.2d;//0.18d;//0.3d;//
     private final double flyingSpeed = 0.35d;
     private final int jammedLimit = 35; //ment for flying speed
     private boolean singleFlight = true;//když je single flight tak to udělá jednu žížalu, když true, tak to udělá 4 žížaly
     private Location destination = destCenter; //(-100,40);//
     private double dWidth = destWidth;
     private double dHeight = destHeight;//80;
     private Location longStep; //bude se muset vypočítat je to max(width,height) a směr se určí podle destination-(nejbližší z rohů obdélníka) samozřejmě se odečítají souřadnice podle max(width,height)
     private Location shortStep; //je vypočítán z density a směr je podle nejbližšího z rohů obdélníka
     private double altitude = scanAltitude;//MaxRange/2;
     private double actAlt = 0;
     private Location actLoc;
     private Rotation actRot;
     private Location nextLoc;// = new Location();
     private State state = State.DEFAULT;
     private double speed = 0;
     private double rotSpeed = 0;
     private double minDev = 0.9d;//longitude/latitude minimal margin
 //    private final double latitudeDev = 0.007d;//grid spacing in degrees
 //    private final double longitudeDev = 0.007d;//grid spacing in degrees
     //properties needed for logic run:
     private boolean parametersObtained = false;
     private boolean scanning = false; //not scanning-no need for slow motion
     private double maxLateralVelocity = 0;
     private double maxLinearVelocity = 0;
     private double maxRotationalVelocity = 0;
     private double maxAltitudeVelocity = 0;
     private boolean risen = false;
     private int offset = 0;
     private int cycle = 0;
     private int cycleStop = 0;
     private Location startLoc;// = new Location();
     private String infoToShow = "";
     private String infoToWrite = "";
     //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
     {
         System.out.println("logika");
         try
         {
             super.logic();
             if(lf == null)//wait for the form to initialize!
             {
                 return;
             }
             time = staModule.getStatesByVehilceType(VehicleType.AERIAL_VEHICLE).getTime();
             issueInfos();
             while(resModule.size() > 0)
             {
                 System.out.println(((ResponseSensorEffecter) resModule.pull()).getStatus());
             }
 
             if(!parametersObtained)
             {
                 if(!acceptSensorMessage())
                 {
                     return;
                 }
                 getConfig();
                 return;//lets gather information befor rising;
             }
             else
             {
                 acceptSensorMessages();
                 checkBattery();
             }
             if(!risen)
             {
                 rise();
             }
             else
             {
                 move();
                 //getAct().act(new DriveAerial(0, 100, 50, 25, true));
             }
             lf.setInfo(infoToShow);
             lf.setPostInfo(infoToWrite);
         }
         catch(Exception e)
         {
             System.out.println("ERROR LOGIC. " + e.getMessage());
         }
 
     }
 
     /**
      * Gatheres data to send to the scan preview form to show at the left white
      * panel.
      */
     private void issueInfos()
     {
         infoToShow = "";
         infoToShow += "\nRobot State: " + state.toString();
         infoToShow += "\nTime Elapsed: " + ToolBox.getTime(System.currentTimeMillis() - startTime);
         infoToShow += "\n\nnoRiskCount: " + noriskCount;
         infoToShow += "\nriskCount: " + riskCount;
         infoToWrite = "";
         infoToWrite += "Elapsed: " + ToolBox.getTime(System.currentTimeMillis() - startTime);
         infoToWrite += "\nBattery Cycles: " + battFills;
     }
 
     /**
      * Carefully tries to obtain all sensor messages. It is sed at the
      * beginning.
      *
      * @return Returns false if it was not able to obtain values. True
      * otherwise.
      */
     private boolean acceptSensorMessage()
     {
 
         if(!senModule.isSensorReady(SensorType.LASER_SENSOR)
                 || !senModule.isSensorReady(SensorType.INS_SENSOR)
                 || !senModule.isSensorReady(SensorType.GROUND_TRUTH)
                 || !senModule.isSensorReady(SensorType.RANGE_SENSOR))
         {
             return false;
         }
         laser = (SensorLaser) senModule.getSensorsBySensorType(SensorType.LASER_SENSOR).get(0);
         ins = (SensorINS) senModule.getSensorsBySensorType(SensorType.INS_SENSOR).get(0);
         sonar = (SensorRange) senModule.getSensorsBySensorType(SensorType.RANGE_SENSOR).get(0);
         truth = (SensorGroundTruth) senModule.getSensorsBySensorType(SensorType.GROUND_TRUTH).get(0);
         return true;
     }
 
     /**
      * Gets all sensor messages queued from the last logic call.
      */
     private void acceptSensorMessages()
     {
         List<SuperSensor> lasers = senModule.getSensorsBySensorType(SensorType.LASER_SENSOR);
         List<SuperSensor> inses = senModule.getSensorsBySensorType(SensorType.INS_SENSOR);
         List<SuperSensor> sonars = senModule.getSensorsBySensorType(SensorType.RANGE_SENSOR);
         List<SuperSensor> truths = senModule.getSensorsBySensorType(SensorType.GROUND_TRUTH);
 
         int minIndex = ToolBox.getMin(lasers.size(), inses.size(), sonars.size(), truths.size());
 
         for(int i = 0; i < minIndex; i++)
         {
             laser = (SensorLaser) lasers.get(i);
             truth = (SensorGroundTruth) truths.get(i);
             ins = (SensorINS) inses.get(i);
             sonar = (SensorRange) sonars.get(i);
             refreshScanProcedure();
         }
     }
 
     /**
      * Sets actual rotation and orientation and sets important sensor data to
      * the preview JForm.
      */
     private void refreshScanProcedure()
     {
         actLoc = Location.sub(startLoc, truth.getLocation());//Location.sub(startLoc, ins.getLocation());//
         actRot = truth.getOrientation();//ins.getOrientation();//
 
         lf.setRecord(new Record(laser.getRanges(), null, laser.getFOV()));
         lf.setLocation(actLoc);
         lf.setOrientation(actRot);
         lf.refreshGraphics();
     }
     private SensorLaser laser;
     private SensorGroundTruth truth;
     private SensorINS ins;
     private SensorRange sonar;
     private double timeStamp = -1;
     private double time = 0;
     private long startTime = 0;
     private double trip = 0;
     private Location prevLoc = null;
     private int battFull = 0;
     private final int battLife = 500;
     private int battFills = 0; //initial charge
     private State stallState = State.DEFAULT;
     private Location stallActLoc = null;
     private Location stallNextLoc = null;
 
     /**
      * System to compute range, battery left and used takes place here. If the
      * robot is low on battery it returns to the base. If it has not been able
      * to recover from the last recharge it terminates the scanning procedure.
      */
     private void checkBattery()
     {
         trip += Location.getDistance2D(actLoc, prevLoc);
         prevLoc = actLoc;
         double battUsed = battFull - staModule.getStatesByVehilceType(VehicleType.AERIAL_VEHICLE).getBattery();
         double range = (battLife - battUsed) * (trip / battUsed);
         double distanceFromHome = Location.getDistance2D(actLoc, zeroPoint);
         infoToShow += "\n\nBatteryLife: " + battLife;
         infoToShow += "\nBatteryUsed: " + battUsed;
         infoToShow += "\n\nDistance From Home: " + ToolBox.getTwoDecimalPlaces(distanceFromHome);
         infoToShow += "\nDistance From Next: " + ToolBox.getTwoDecimalPlaces(Location.getDistance2D(actLoc, nextLoc));
         infoToShow += "\nRange: " + ToolBox.getTwoDecimalPlaces(range);
         if(state == State.CHARGE || state == State.CHARGING || state == State.TERMINATE || state == State.LAND || tempState == State.TERMINATE || tempState == State.CHARGE || trip < 5)//trip<5->nevěrohodný range
         {
             return;
         }
         if(distanceFromHome > range * 0.85)
         {
             if(state == State.CONTINUE)
             {
                 finishGoalToStart();
                 return;
             }
             if(state == State.AVOIDING && tempState == State.CHARGE)
             {
                 return; //vyhýbá se překážce, ale už měl namířeno na nabití
             }
             if(state == State.AVOIDING)
             {
                 stallState = tempState;//vyhýbá se překážce, ale ještě neví, že musí na nabití
                 stallNextLoc = tempNextLoc;
                 tempNextLoc = null;
                 stallActLoc = (Location.getDistance2D(actLoc, stallNextLoc) < 15)?stallNextLoc:actLoc;
             }
             else
             {
                 stallState = state; //musí na nabití
                 stallNextLoc = nextLoc;
                 //pokud už byl fakt blízko k další metě, tak je to jakoby tam byl - zbytečná ztráta času
                 stallActLoc = (Location.getDistance2D(actLoc, nextLoc) < 15)?nextLoc:actLoc;
             }
             state = State.CHARGE;
             nextLoc = zeroPoint;
             minDev = 1.5d;
             lf.setRechargeBreakPoint(actLoc);
         }
 
     }
 
     /**
      * Computes the closest corner based on input parameters of scanning area
      * and actual location of the robot.
      *
      * @param center Center point of the rectangle.
      * @param width Width of the rectangle.
      * @param height Height of the rectangle.
      * @param actual Actual location from which it gets the closest corner.
      * @return Returns closest corner of scanning area from the actual location.
      */
     private Location getClosestCorner(Location center, double width, double height, Location actual)
     {
         Location closest = new Location(center.x - width / 2, center.y + height / 2);//LeftTop
         Location rt = new Location(center.x + width / 2, center.y + height / 2);//RightTop
         Location lb = new Location(center.x - width / 2, center.y - height / 2);//LeftBottom
         Location rb = new Location(center.x + width / 2, center.y - height / 2);//RightBottom
 
         if(Location.getDistance2D(closest, actual) > Location.getDistance2D(rt, actual))
         {
             closest = rt;
         }
         if(Location.getDistance2D(closest, actual) > Location.getDistance2D(lb, actual))
         {
             closest = lb;
         }
         if(Location.getDistance2D(closest, actual) > Location.getDistance2D(rb, actual))
         {
             closest = rb;
         }
         return closest;
     }
 
     /**
      * It computes its path to cover the whole scanning area.
      *
      * @param center center of the scanning area
      * @param corner first corner to beggin with.
      */
     private void setupSteps(Location center, Location corner)
     {
         //set direction towards the center and pick the longer side every time!
         //the small step has the oposite coordinate and points to the center as well.
         //finally set the Stop to number of rows made.
 
         if(this.dHeight > this.dWidth)
         {
             double density = this.dWidth / Math.ceil(this.dWidth / this.minDensity);
             this.longStep = new Location(0, this.dHeight * Math.signum(center.y - corner.y));
             this.shortStep = new Location(density * Math.signum(center.x - corner.x), 0);
             this.cycleStop = (int) (this.dWidth / density);
         }
         else
         {
             double density = this.dHeight / Math.ceil(this.dHeight / this.minDensity);
             this.longStep = new Location(this.dWidth * Math.signum(center.x - corner.x), 0);
             this.shortStep = new Location(0, density * Math.signum(center.y - corner.y));
             this.cycleStop = (int) (this.dHeight / density);
         }
     }
 
     /**
      * This computes the avarage height from ten rays from Range scanner that
      * aim directly below the robot.
      */
     private void checkAltitudeFromLaser()
     {
         //SensorLaser laser = (SensorLaser) (senModule.getSensorsBySensorType(SensorType.LASER_SENSOR).get(0));
         //half of the FOV + actRotation to degrees + 90° gives us the angle of a ray that is aiming vertically
         //máme v paměti, že jeden paprsek = 1°
         //a taky že rotace je mezi 0 a 6.28, resp. že když se nakloní doprava tak se k nule přičítá a když doleva, tak se od 6.28 odečítá
         offset = (int) ((actRot.roll >= Math.PI)?(360 - (actRot.roll * 180 / Math.PI)):(-actRot.roll * 180 / Math.PI));
         actAlt = laser.getNMidAvg(numOfRays, offset);
         lf.setOffset(offset);
     }
 
     /**
      * This method elevates robots altitude till the scanning altitude is
      * reached.
      */
     private void rise()
     {
         if(altitude == 0)
         {
             return;
         }
         //TODO: actAlt se musí počítat ještě s náklonem dopředu, takže tam bude nějakej kosínus
         actAlt = laser.getNMidAvg(10);//nemusíme počítat s odchylkou, když soupá kolmo
         //musíme stoupat pomalu, aby se nezbláznila odchylka
         bot.getAct().act(new DriveAerial(maxAltitudeVelocity / 4, 0, 0, 0, false));
         if(altitude - actAlt < 0)
         {
             risen = true;
             bot.getAct().act(new DriveAerial(0, 0, 0, 0, false));
         }
     }
 
     /**
      * main control block that computes next direction based on altitude, risk
      * level and computes next goal if needed. Also unjams the robot if
      * necessary.
      */
     private void move()
     {
         checkAltitudeFromLaser();
         if(state != State.CHARGING && state != State.LAND)
         {
             if(checkSonars() == RiskLevel.HIGHRISK)
             {
                 return;
             }
         }
         approachNext();
         scanning = (state == State.AVOIDING)?isScanning(tempState):isScanning(state);
         if(state == State.CHARGING || state == State.LAND)
         {
             if(wait(dockWaitPenalty))
             {
                 if(state == State.CHARGING)
                 {
                     setResumeState();
                 }
                 else if(!singleFlight && wait(dockWaitPenalty))
                 {
                     setupMultiple();
                 }
             }
             return;//no point in doing the next thing
         }
         if(hasReachedTarget(actLoc, nextLoc))//hasReachedTarget(latitude, latitudeNext) && hasReachedTarget(longitude, longitudeNext))
         {
             state = State.getNextState(state);
             computeNext(state);
         }
         carryOnIfJammed();
     }
 
     /**
      * Method to establish if the robot is currently scanning important area or
      * not, so it knows wether to speed up or not.
      *
      * @param state Can be actual state or temporary if avoiding obstacle
      * @return Returns true if the robot is scanning important area based on its
      * state.
      */
     private boolean isScanning(State state)
     {
         return (state == State.LONGFORTH || state == State.SHORTFORTH || state == State.LONGBACK || state == State.SHORTBACK);
     }
 
     /**
      * We use minimal deviation constant to allow robot to be just near its
      * goal. The INS noise would cause the robot to spin around when the
      * acceptable distance was too close.
      *
      * @param actual Actual position of the robot
      * @param target Goal position.
      * @return Returns true if the robot is within acceptable distance from its
      * target.
      */
     private boolean hasReachedTarget(Location actual, Location target)
     {
         if(Location.getDistance2D(actual, target) > minDev)
         {
             return false;
         }
         return true;
     }
 
     /**
      * Helper method for terminating the scanning procedure and setting the last
      * goal to its start position.
      */
     private void finishGoalToStart()
     {
         this.state = State.TERMINATE;
         nextLoc = new Location(0, 0, 0);
     }
 
     /**
      * This comutes next goal or choses next action according to actual state of
      * the robot.
      *
      * @param state current state of the robot.
      */
     private void computeNext(State state)
     {
         switch(state)
         {
             case DEFAULT:
             case LONGFORTH:
                 nextLoc = nextLoc.add(longStep);
                 break;
             case SHORTFORTH:
             case SHORTBACK:
                 nextLoc = nextLoc.add(shortStep);
                 if(cycle >= cycleStop)
                 {
                     finishGoalToStart();
                 }
                 cycle++;
                 break;
             case LONGBACK:
                 nextLoc = nextLoc.sub(longStep);
                 break;
             case TERMINATE:
                 break;
             case LAND:
             case CHARGING:
                 altitude = 0;
                 break;
             case CONTINUED:
                 continueScanning();
                 break;
             case AVOIDED:
                 tryReleaseDiversion();
                 break;
             default:
                 System.out.println("Unexpected STATE");
         }
     }
     private int multipleRunCount = 0;
     private double multipleMargin = 10;
 
     /**
      * If scanning area is too large it has to devide it into quadrants, this
      * computes actual quadrant to scan. It narrows individual segments because
      * the neighboring sides are covered even if the robot flies aside at some
      * margin distance.
      */
     private void setupMultiple()
     {
         switch(multipleRunCount)
         {
             case 1:
                 destination = new Location(destCenter.x - dWidth / 2 - multipleMargin, destCenter.y + dHeight / 2 + multipleMargin);
                 break;
             case 2:
                 destination = new Location(destCenter.x - dWidth / 2 - multipleMargin, destCenter.y - dHeight / 2 - multipleMargin);
                 break;
             case 3:
                 destination = new Location(destCenter.x + dWidth / 2 + multipleMargin, destCenter.y - dHeight / 2 - multipleMargin);
                 break;
             case 4:
                 altitude = 0;
                 break;
         }
         if(multipleRunCount < 4)
         {
             resetINS();
             nextLoc = getClosestCorner(destination, dWidth, dHeight, new Location(0, 0, 0));
             setupSteps(destination, nextLoc);
             cycle = 0;
             state = State.DEFAULT;
             multipleRunCount++;
             prepareForTakeof();
         }
     }
 
     /**
      * Sets widths and heights of long and short streights if division into
      * quadrant is needed.
      */
     private void initMultipleRun()
     {
         this.dWidth = destWidth / 2 - multipleMargin;// Math.abs(destination.x * 2);
         this.dHeight = destHeight / 2 - multipleMargin; //Math.abs(destination.y * 2);
         this.destination = new Location(destCenter.x + dWidth / 2 + multipleMargin, destCenter.y + dHeight / 2 + multipleMargin);
     }
 
     /**
      * Once the method is called it uses timeStamp variable to determine when it
      * was called for the first time and every other call it checks if the time
      * passed from the timeStamp is greater than the input variable timeSpan
      *
      * @param timeSpan - time interval that should pass from the first calling
      * of this method to its last call
      * @return returns false if the time passed is less than timeSpan and true
      * if it is greater.
      */
     private boolean wait(int timeSpan)
     {
         if(timeStamp == -1)
         {
             timeStamp = time;
         }
         else if(time - timeStamp > timeSpan)
         {
             return true;
         }
         return false;
     }
 
     /**
      * If the robot is awoiding and is presumably jammed, this methods proceeds
      * to next goal.
      */
     private void tryReleaseDiversion()
     {
         if(state == State.AVOIDING || state == State.AVOIDED)
         {
             nextLoc = tempNextLoc;
             tempNextLoc = null;
             state = tempState;
         }
     }
 
     /**
      * This establishes previously interrupted scanning procedure for it had to
      * recharge.
      */
     private void continueScanning()
     {
         this.state = stallState;
         nextLoc = stallNextLoc;
         stallNextLoc = null;
         minDev = 0.9d;
     }
 
     /**
      * sets up necessary variables to resume scan procedure Resets INS position
      * and rotation according to groundTruth - simulation of recharging and
      * sensor reset.
      *
      */
     private void setResumeState()
     {
         resetINS();
         state = State.CONTINUE;
         nextLoc = stallActLoc;
         stallActLoc = null;
         prepareForTakeof();
     }
 
     /**
      * Setting important variables, reseting battery and preparing for another
      * take off.
      */
     private void prepareForTakeof()
     {
         risen = false;
         battFills++;
         double battUsed = battFull - staModule.getStatesByVehilceType(VehicleType.AERIAL_VEHICLE).getBattery();
         double range = (battLife - battUsed) * (trip / battUsed);
         System.out.println("range left: " + range + " trip odometer: " + trip + " battery used: " + battUsed);
         battFull = staModule.getStatesByVehilceType(VehicleType.AERIAL_VEHICLE).getBattery();//reset batteryMeter
         trip = 0; //reset odometer
         timeStamp = -1;//reset of the wait method
         altitude = scanAltitude;
     }
 
     /**
      * Gets sensor data from ground truth sensor and sets it into the INS sensor
      * which results in erasing the drift it gained.
      */
     private void resetINS()
     {
         Location truthLoc = truth.getLocation();
         Rotation truthRot = truth.getOrientation();
         lf.setStartLocation(Location.sub(startLoc, truthLoc.getLocation()));
         StringBuilder sb = new StringBuilder();
         sb.append(truthLoc.x).append(',').append(truthLoc.y).append(',').append(truthLoc.z).append(',');
         sb.append(truthRot.roll).append(',').append(truthRot.pitch).append(',').append(truthRot.yaw);
         bot.getAct().act(new SetSensorEffecter("INS", "INS", "POSE", sb.toString()));
     }
 
     /**
      * Computes an angle of the robot in degrees.
      *
      * @param sx sinus
      * @param cx cosinus
      * @return returns angle in degrees.
      */
     private double getAngle(double sx, double cx)
     {
         if(sx > 0 && cx > 0)
         {
             return Math.acos(Math.abs(cx)) * 180 / Math.PI;
         }
         else if(sx > 0 && cx <= 0)
         {
             return 180 - Math.acos(Math.abs(cx)) * 180 / Math.PI;
         }
         else if(sx <= 0 && cx <= 0)
         {
             return 180 + Math.acos(Math.abs(cx)) * 180 / Math.PI;
         }
         else
         {
             return 360 - Math.acos(Math.abs(cx)) * 180 / Math.PI;
         }
     }
 
     /**
      * System of setting lateral, linear, altitude and rotational speeds takes
      * place here. If the robot is close to the base, it has to slow down not to
      * descend out of desired location. Also when it is close to goal it stops
      * the forward movement for better dexterity.
      */
     private void approachNext()
     {
         speed = (scanning)?scanningSpeed:flyingSpeed;
         rotSpeed = (scanning)?scanningSpeed:flyingSpeed * 2 / 3;
 
         double rotVel;
         double altVel;
         double linVel = maxLinearVelocity * speed;
 
         double diff = getTargetAngle(false);
         //směr zatáčení musí bejt ne podle dot, ale podle toho, jestli je úhel s osou menší než úhel s osou co svírá ten druhej vektor!!!            
         rotVel = Math.signum(-diff) * Math.min(Math.abs((double) (maxRotationalVelocity * (-diff / 90))), rotSpeed);
 
         if(actLoc.z < seaLevel + 1 && actAlt > scanAltitude && (state != State.LAND && state != State.CHARGING))
         {
             altVel = (double) (maxAltitudeVelocity * (seaLevel - actLoc.z) / 6);
         }
         else
         {
             altVel = (double) (maxAltitudeVelocity * (altitude - actAlt) / 5);
         }
         //altVel = (double) ((actLoc.z < seaLevel)?(maxAltitudeVelocity * (seaLevel - actLoc.z) / 5):(maxAltitudeVelocity * (altitude - actAlt) / 5));//we don't want to go lower than some minimum value
         //altVel = Math.min(altVel, maxAltitudeVelocity / 4);//make sure that it wont take off too quickly
         if(state == State.AVOIDING || Location.getDistance2D(actLoc, nextLoc) < 2.7)
         {
             linVel = (Math.abs(rotVel) >= rotSpeed)?0:linVel;
         }
         else
         {
             linVel = (Math.abs(rotVel) >= rotSpeed)?linVel * 3 / 5:linVel;
         }
         shouldGoWithIt = true;
         if(Math.abs(rotVel) >= rotSpeed)
         {
             noriskCount = Math.max(0, noriskCount - 1);
             if(state == State.AVOIDING)
             {
                 shouldGoWithIt = false;
             }
         }
         //landing - we have to slow down before falling
         if(state == State.TERMINATE || state == State.CHARGE)
         {
             linVel *= Math.min((Location.getDistance2D(actLoc, nextLoc)) / 6, 1);
         }
         //landed - we don't need to correct the position while falling
         if(state == State.LAND || state == State.CHARGING)
         {
             bot.getAct().act(new DriveAerial(altVel, 0, 0, 0, false));
         }
         else
         {
             altVel = (Math.abs(rotVel) >= rotSpeed)?0:altVel;
             bot.getAct().act(new DriveAerial(altVel, linVel, 0, rotVel, false));
         }
     }
     Location tempNextLoc;
     State tempState;
     List<SensorMount> sonarGeo;
     Map.Entry<String, Double> greatestRiskSonar;//filled by getRiskLevel()
     private final double sonarThreashold = 4.91d;//value beyond this threashold is eqal to infinity
 
     /**
      * Triggered by robot being close to an obstacle. Computes the most
      * advantageous direction based on sonar ring.
      */
     private void computeDiversion(Map<String, Double> sonars)
     {
         //kounknout kde všude jsou překážky a vypočítat bod, kterej je vedle překážky a kam se robot ještě vejde aniž by se dostal do HighRisk
         //mám paprsek, kde je místa nejmíň, tak kouknu na ty od něj doleva a od něj doprava, sečtu všechny hodnoty a podělim je počtem senzorů a 
         //vyberu potom bod, kde je první volenj, ale asi taky se skreslením-udělám váženej průměr totiž!
         boolean leftSide = true;
         double leftWM = 0;//left weighted mean
         double rightWM = 0;//right weighted mean
         double leftFittest = 0;
         double rightFittest = 0;
         int leftFindex = 0;
         int rightFindex = 0;
 
         //int targetIndex=getSonarTargetIndex(); //computes the index of the sonar that is closest to target direction
         //targetIndex ensures that the robot wont be influenced by minor disturbances
 
         int greatestRiskIndex = -1;
         for(int i = 0; i < sonars.size(); i++)
         {
             Double entry = sonars.get(sonarOrder[i]);//order of entries does matter at this point
             if(sonarOrder[i].equals(greatestRiskSonar.getKey()))
             {
                 greatestRiskIndex = i;
                 leftSide = false;
                 continue;
             }
 
             //computes the odds left and right from the sonar with greatest risk
             if(leftSide)
             {
                 leftWM += entry * entry;//lets get the mean from the second power - it will ensure, that one inf.(>4.8) ray is valued better than six finite.(<4.8)
                 if(leftFittest < entry || entry > sonarThreashold)//we want the closest open direction to the occupied as possible
                 {
                     leftFittest = entry;
                     leftFindex = i;
                 }
             }
             else
             {
                 rightWM += entry * entry;
                 //we want the first open direction or the best possible
                 if(rightFittest < entry && rightFittest < sonarThreashold)
                 {
                     rightFittest = entry;
                     rightFindex = i;
                 }
             }
         }
         if(leftWM < rightWM)
         {
             if(rightFindex < 7 && sonars.get(sonarOrder[rightFindex - 1]) < 2.5d)
             {
                 RiskLevel righterRisk = getRiskLevel(sonarOrder[rightFindex + 2], sonars.get(sonarOrder[rightFindex + 2]));
                 if(righterRisk == RiskLevel.NORISK)
                 {
                     rightFindex += 2;
                 }
             }
             //když to neni na kraji, tak to bere druhej nejlepší paprsek, ještě by se dal dát
             else if(/*
                      * rightFindex - greatestRiskIndex == 1 &&
                      */rightFindex < 8)
             {//ale musíme se ujistit, že tím dalším směrem nás nečeká žádná překážka
                 RiskLevel righterRisk = getRiskLevel(sonarOrder[rightFindex + 1], sonars.get(sonarOrder[rightFindex + 1]));
                 if(righterRisk == RiskLevel.NORISK)
                 {
                     rightFindex++;
                 }
             }
             int targetIndex = getSonarTargetIndex();
             if(targetIndex > rightFindex && targetIndex < 8)
             {
                 RiskLevel targetRiskLevel = getRiskLevel(sonarOrder[targetIndex], sonars.get(sonarOrder[targetIndex]));
                 if(targetRiskLevel == RiskLevel.NORISK)
                 {
                     //vlastně nepotřebujem měnit směr
                     rightFindex = targetIndex;
                     tryReleaseDiversion();
                     return;
                 }
             }
             setDiversionPoint(rightFindex, rightFittest);
         }
         else
         {
             if(leftFindex > 1 && sonars.get(sonarOrder[leftFindex + 1]) < 1.5d)
             {
                 RiskLevel lefterRisk = getRiskLevel(sonarOrder[leftFindex - 2], sonars.get(sonarOrder[leftFindex - 2]));
                 if(lefterRisk == RiskLevel.NORISK)
                 {
                     leftFindex -= 2;
                 }
             }
             else if(/*
                      * greatestRiskIndex - leftFindex == 1 &&
                      */leftFindex > 0)
             {
                 RiskLevel lefterRisk = getRiskLevel(sonarOrder[leftFindex - 1], sonars.get(sonarOrder[leftFindex - 1]));
                 if(lefterRisk == RiskLevel.NORISK)
                 {
                     leftFindex--;
                 }
             }
             int targetIndex = getSonarTargetIndex();
             if(targetIndex < leftFindex && targetIndex > 0)
             {
                 RiskLevel targetRiskLevel = getRiskLevel(sonarOrder[targetIndex], sonars.get(sonarOrder[targetIndex]));
                 if(targetRiskLevel == RiskLevel.NORISK)
                 {
                     leftFindex = targetIndex;
                     tryReleaseDiversion();
                     return;
                 }
             }
             setDiversionPoint(leftFindex, leftFittest);
         }
     }
     private boolean shouldGoWithIt = true;
 
     /**
      * Sets a diversion point in a direction of the fittest sonar based on its
      * index.
      *
      * @param index index of the sonar sensor.
      * @param fittest fittest value obtained from sonar sensor.
      */
     private void setDiversionPoint(int index, double fittest)
     {
         Rotation sonarRot = sonarGeo.get(index).getOrientation();
         //if its left side sonar give it +0.3 rad to go further from an obstacle. if it is right lets give it -0.3 for the same reason
         double angle = Math.PI / 2 - (sonarRot.yaw + actRot.yaw) + ((greatestRiskSonar.getKey().charAt(0) == 'L')?-0.3:(greatestRiskSonar.getKey().charAt(0) == 'R')?0.3:0);
         Location actDiv = new Location(-Math.sin(angle) * fittest, -Math.cos(angle) * fittest);
         Location diversion = actLoc.add(actDiv);
         lf.setDivPoint(diversion);
         if(state != State.AVOIDING)
         {
             tempState = state;
             tempNextLoc = nextLoc;
         }
 
         state = State.AVOIDING;
         nextLoc = diversion;
     }
 
     /**
      * *
      * Computes an angle in degrees of the aircraft relative to next Location
      * that the robot is heading to.
      *
      * @param actual Tells the method whether it should be considering temporary
      * next Location (true) or not (false). Temporary next Location is greater
      * goal that the robot is not pursuing when avoiding an obstacle.
      * @return Returns angle in degrees relative to next Location or temp. next
      * Location based on <B>actual</B>
      */
     private double getTargetAngle(boolean actual)
     {
         //v1 is vector from actual location to next location
         double v1x;
         double v1y;
         if(actual && state == State.AVOIDING && tempNextLoc != null)
         {
             v1x = tempNextLoc.x - actLoc.x;
             v1y = tempNextLoc.y - actLoc.y;
         }
         else
         {
             v1x = nextLoc.x - actLoc.x;
             v1y = nextLoc.y - actLoc.y;
 
         }
         double v1l = Math.sqrt(v1x * v1x + v1y * v1y);
 
         double sinv1 = v1y / v1l;
         double cosv1 = v1x / v1l;
 
         double v1Theta = (getAngle(sinv1, cosv1) + 180) % 360;//requiered angle
         double v2Theta = actRot.yaw * 180 / Math.PI;//robot actual angle
         double diff = v2Theta - v1Theta;//(Math.abs(v2Theta - v1Theta) < Math.abs(v1Theta - v2Theta))?v2Theta - v1Theta:v1Theta - v2Theta;
         if(diff > 180)
         {
             diff = diff - 360;
         }
         else if(diff < -180)
         {
             diff = diff + 360;
         }
         return diff;
     }
 
     /**
      * If the robot faces the goal this gets the index of sonar that points
      * closest to this direction.
      *
      * @return returns a sonar index, that points to goal direction.
      */
     private int getSonarTargetIndex()
     {
         int sonarIndex = -1;
         int count = 0;
         double v1Theta = getTargetAngle(true) / 180 * Math.PI;
         double minDiv = 7;//2*PI
         for(SensorMount sensorMount : sonarGeo)
         {
             if(minDiv > Math.abs(v1Theta - sensorMount.getOrientation().yaw))
             {
                 minDiv = Math.abs(v1Theta - sensorMount.getOrientation().yaw);
                 sonarIndex = count;
             }
             count++;
         }
         return sonarIndex;
     }
 
     /**
      * Triggered by robot being too close to an obstacle. It dodges an obstacle
      * in a wince maner.
      */
     private void abortAim()
     {
         String key = greatestRiskSonar.getKey();
         Point2D velocity = highRiskAction.get(key);
         getAct().act(new DriveAerial(0, velocity.getX() * 40, velocity.getY() * 40, 0, true));//*40 na procenta
         //zastav-ozkoušet, kdy se to stane-> zastavit nebo dát DRIVE a opačný hodnoty než v předchozím a zastavit
         //otočit o 180 nebo 360 a vybrat nejlepší cestu ven, nebo zhasnout.
     }
     int noriskCount = 0;
     int riskCount = 0;
 
     /**
      * Checks the sonar ring for possible threat and also issues longterm stuff
      * such a jamm count represents.
      *
      * @return Returns LOW, HIGH or NO RISK based on Sonar sensor reading.
      */
     private RiskLevel checkSonars()
     {
         //Map<String, Double> sonars = ((SensorRange) senModule.getSensorsBySensorType(SensorType.RANGE_SENSOR).get(0)).getRanges();
         RiskLevel worstThread = getRiskLevel(sonar.getRanges());
 
         //když je od cíle blíž než je nejhorší risk level, tak to nevadí, že je robot v ohrožení
         if(worstThread != RiskLevel.NORISK && greatestRiskSonar.getValue() > Location.getDistance2D(actLoc, nextLoc))
         {
             return RiskLevel.NORISK;
         }
 
         switch(worstThread)
         {
             case NORISK:
                 noriskCount++;
                 if(noriskCount > 6)
                 {
                     tryReleaseDiversion();
                 }
                 if(noriskCount > 15)
                 {
                     riskCount = 0;
                 }
                 break;
             case LOWRISK:
                 //if(!delay)
                 if(shouldGoWithIt)
                 {
                     computeDiversion(sonar.getRanges());
                 }
                 noriskCount = 0;
                 riskCount++;
                 lf.setLowRiskPoint(actLoc);
                 break;
             case HIGHRISK:
                 abortAim();
                 noriskCount = 0;
                 lf.setHighRiskPoint(actLoc);
                 riskCount++;
                 break;
         }
         return worstThread;
     }
 
     /**
      * *
      * If the Robot is avoiding some obstacle for a long time(edge of the world,
      * target Location in the middle of a tree/house etc.), there is probably no
      * point trying any further. Next Location to go after the one currently
      * pursued is computed and given to the robot to chase. The robot simply
      * leaves out its original goal after some time trying to avoid an obstacle.
      */
     private void carryOnIfJammed()
     {
         if(riskCount > jammedLimit * (flyingSpeed / speed))
         {
             if(state == State.CHARGE || tempState == State.CHARGE || tempState == State.TERMINATE || state == State.TERMINATE)
             {
                 getAct().act(new DriveAerial(0, -maxLinearVelocity, 0, 0, false));
                 riskCount = 0;
                 return;
             }
             if(state == State.AVOIDING)
             {
                 tryReleaseDiversion();
             }
             else if(state == State.CONTINUE)
             {
                 continueScanning();
             }
             state = State.getNextState(state);
             computeNext(state);
             riskCount = 0;
         }
     }
 
     /**
      * Gets risk level from each sonar from the Map sonars and returns the
      * greatest risk. It also logs the location of the biggest thread.
      *
      * @param sonars Map acquired from the sensor module.
      * @return returns greates thread to robot.
      */
     private RiskLevel getRiskLevel(Map<String, Double> sonars)
     {
         List<Double> sonarValues = new LinkedList<Double>();
         for(int i = 0; i < sonars.size(); i++)
         {
             sonarValues.add(sonars.get(sonarOrder[i]));
         }
         lf.setSonars(sonarValues);
         RiskLevel globalRisk = RiskLevel.NORISK;
         double riskValue = sonarThreashold;
 
         //order of entrys does not matter at this point
         for(Map.Entry<String, Double> entry : sonars.entrySet())
         {
             RiskLevel risk = getRiskLevel(entry);
             //due to nonequal conditions for each sonar, we have to get the urgency first(greater urgency at one could be further than no urgency at other)
             //and than ask the distance. We want to determine the greatest risk and than the nearest sonar at this risk level
 
             if(risk.isGreaterRisk(globalRisk))
             {
                 globalRisk = risk;
                 riskValue = entry.getValue();
                 greatestRiskSonar = entry;
             }
             else if(risk == globalRisk)
             {
                 if(entry.getValue() < riskValue)
                 {
                     riskValue = entry.getValue();
                     greatestRiskSonar = entry;
                 }
             }
         }
 
         return globalRisk;
     }
 
     /**
      * Decides the risk individually based on the name of the sonar and static
      * arrays with Low Risk and High Risk Threasholds. (Side sonars have grater
      * tollerance than the front ones.)
      *
      * @param sonar Entry acquired from the sonar sensor Map.
      * @return Returns the risk based on the name of the sonar.
      */
     private RiskLevel getRiskLevel(Map.Entry<String, Double> sonar)
     {
         return (sonar.getValue() < lowRisk.get(sonar.getKey()))?(sonar.getValue() < highRisk.get(sonar.getKey()))?RiskLevel.HIGHRISK:RiskLevel.LOWRISK:RiskLevel.NORISK;
     }
 
     /**
      * Gets risk level from concrete sonar.
      *
      * @param sonar Name of the sonar
      * @param value value of the sonar sensor
      * @return Returns risk level of input sonar.
      */
     private RiskLevel getRiskLevel(String sonar, double value)
     {
         return (value < lowRisk.get(sonar))?(value < highRisk.get(sonar))?RiskLevel.HIGHRISK:RiskLevel.LOWRISK:RiskLevel.NORISK;
     }
 
     /**
      * If a scanning area has width or height greater than 150 meters it is
      * considered to be large space.
      *
      * @return Returns true if large ara is about to be scanned.
      */
     private boolean isLargeSpace()
     {
         return ((destHeight > 150) || (destWidth > 150));
     }
 
     
     //    /**
 //     * If the geometry or configuration modules are ready it issues a query
 //     * about the robots configuration and than a query about the sonar geometry.
 //     * Please note that this method works in cycles. First it issues a
 //     * configuration query, when recieved it issues a geometry query. When
 //     * obtained both it can proceed in filling and configurating variables.
 //     * complete config information. There is a reason why both queries are
 //     * issued at different calls. This method is considered to be called - once
 //     * at most - from the logic() which is in fact triggered every time the
 //     * state message from the server is recieved. Thus, each query in this
 //     * method is issued at different cycle. This fact gives the server necessary
 //     * time to process the queries. In other words: The experience with the
 //     * USARSim server so far was that it won't process more than one CONF/GEO
 //     * message at a time. Which is why we can't call more than one
 //     * GETCONF/GETGEO command message in one cycle. Note that there would be
 //     * another "else if" branch if we wanted to know any other conf./geo. data
 //     * about anything else. The if clause with anothre isReady() would not be
 //     * sufficient, we would have to ask for example about the number of messages
 //     * conf or geo module has in store.
 //     *
 //     */
     
     
     /**
      * Obtaining of configuration information in several steps. It is unable to
      * obtain all data in one logic cycle.
      */
     private void getConfig()
     {
         if(!confModule.isReady())
         {
             confModule.queryConfigurationByType("Robot");
         }
         else if(!geoModule.isReady())
         {
             geoModule.queryGeometryByType("Sonar");
         }
         else if(senModule.isReady())
         {
             ConfigAerial robotCfg = (ConfigAerial) confModule.getConfigurationsByConfigType(ConfigType.AERIAL_VEHICLE).get(0);
             //getParameters
             maxLateralVelocity = Double.parseDouble(robotCfg.getFeatureValueBy("MaxLateralVelocity"));
             maxLinearVelocity = Double.parseDouble(robotCfg.getFeatureValueBy("MaxLinearVelocity"));
             maxAltitudeVelocity = Double.parseDouble(robotCfg.getFeatureValueBy("MaxAltitudeVelocity"));
             maxRotationalVelocity = Double.parseDouble(robotCfg.getFeatureValueBy("MaxRotationalVelocity"));
             //set startLoc
             //SensorINS ins = (SensorINS) (senModule.getSensorsBySensorType(SensorType.INS_SENSOR).get(0));
             startLoc = ins.getLocation();//((SensorGroundTruth) senModule.getSensorsBySensorType(SensorType.GROUND_TRUTH).get(0)).getLocation();//
             //prepare variables for the flight.
             singleFlight = !isLargeSpace();
             //TODO: tohle se mi nějak nezdá, opravdu to dělá, co má? -> jestli v actLoc je správná hodnota? zeroPoint?
             actLoc = zeroPoint;
             prevLoc = zeroPoint;
             lf.setStartLocation(zeroPoint);
             if(singleFlight)
             {
                 nextLoc = getClosestCorner(destination, destWidth, destHeight, new Location(0, 0, 0));
                 setupSteps(destination, nextLoc);
             }
             else
             {
                 initMultipleRun();
                 setupMultiple();
             }
             battFull = staModule.getStatesByVehilceType(VehicleType.AERIAL_VEHICLE).getBattery();
             startTime = System.currentTimeMillis();//staModule.getStatesByVehilceType(VehicleType.AERIAL_VEHICLE).getTime();
             //saveGeometryOfSonars
             sonarGeo = ((GeoSensorEffecter) geoModule.getGeometriesByGeometryType(GeometryType.SENSOR_EFFECTER).get(0)).getSensorMountCollection();
 
             parametersObtained = true;         //setFlag
         }
     }
 
     /**
      * 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);
 
         senModule = SensorMasterModuleQueued.getModuleInstance(bot);
         geoModule = GeometryMasterModule.getModuleInstance(bot);
         confModule = ConfigMasterModule.getModuleInstance(bot);
         resModule = ResponseModule.getModuleInstance(bot);
         staModule = StateMasterModule.getModuleInstance(bot);
     }
     private SensorMasterModuleQueued senModule;
     private GeometryMasterModule geoModule;
     private ConfigMasterModule confModule;
     private ResponseModule resModule;
     private StateMasterModule staModule;
     static ScanPreview lf = null;
 
     public static void main(String[] args)
     {
         SwingUtilities.invokeLater(new Runnable()
         {
             @Override
             public void run()
             {
                 try
                 {
                     lf = new ScanPreview();
                     lf.runScanPreview(); // Calling Login Frame
                     lf.show();
 
                 }
                 catch(Exception e)
                 {
                     System.out.println("error");
                 }
             }
         });
 
         try
         {
             new USAR2004BotRunner(AerialVehicle.class, "PogamutBotBot", "127.0.0.1", 3000).setMain(true).setLogLevel(Level.OFF).startAgent();
 
         }
         catch(Exception e)
         {
             System.out.println(e.getCause());
         }
     }
 }