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();

    private void driveLeft(){
        System.out.println("left");
            right=15;
            left=5;
    }
    private void driveRight(){
        System.out.println("right");
            right=4;
            left=15;
    }
    
    
    private void driveStraight(){
        System.out.println("straight");
        right=15;
        left=15;
    }
    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);                
            }
        }

    }

    /**
     * Adds every object that can be casted to initial class to the output list.
     * Note that if You feed this method with SuperClass it will return all
     * available submodules.
     *
     * @param c Class representing the type of which the return list should be
     * @return Returns a list of eligible objects, that can be casted to Class c
     */
    public List<SuperSensor> getSensorsByClass(Class c)
    {
        return sensorModules.getSensorsByClass(c);
    }

    /**
     * Gets sensor message representatives from local hashmap specified by type
     * and by name. Returns null if none matches or this hash map is empty or
     * either type or name is null.
     *
     * @param type String representing the type of sensor to return.
     * @param name String representing the name of sensor to return.
     * @return Returns List of specified type of Sensor module.
     */
    public SuperSensor getSensorByTypeName(String type, String name)
    {
        if(type == null || name == null)
        {
            return null;
        }
        return (SuperSensor) sensorModules.getSensorByTypeName(type.toLowerCase(), name.toLowerCase());
    }

    /**
     * For each type of sensor it adds all individuals to the returnee List as a
     * couple (Type, Name)
     *
     * @return returns Map of couples (Type/Name) of non empty sensor
     * representatives.
     */
    public List<MessageDescriptor> getNonEmptyDescripton()
    {
        return sensorModules.getNonEmptyDescription();
    }

    /**
     * Asks SensorType enum if it knows SensorType represented by string
     * <B>type</B>. If it does, it also contains Class reference. This reference
     * is then instantiated and returned. If it does not, it returns instance of
     * base class SuperSensor which is represented by SensorType.UNKNOWN_SENSOR.
     *
     * @param type String representing possible valid SensorType.
     * @return Returns Class instance relevant to input String.
     */
    protected SuperSensor createNewSensor(SensorMessage message)
    {
        return ModuleInstanceProvider.getSensorInstanceByType(message.getType());
    }

    /**
     * Returns a flag that indicates if sensorUpdate was successful.
     *
     * @param message new SensorMessage object.
     * @return Return false if this message type with this name does not exist
     * yet.
     */
    protected boolean updateSensorCollection(SensorMessage message)
    {
        if(!sensorModules.containsKey(message.getType().toLowerCase()))
        {
            return false;
        }
        if(sensorModules.get(message.getType().toLowerCase()).isEmpty())
        {
            return false;
        }
        if(!sensorModules.get(message.getType().toLowerCase()).containsKey(message.getName().toLowerCase()))
        {
            return false;
        }

        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();

                    "-0.4", "0.2", "-0.3", "0.1", "0.1", "0.5", "-0.1"
                };
                break;
            case 4:
            case 3:
                names = new String[]
                {
                    "LRA", "LRB", "LRC", "RFA", "RFB", "RFC", "LFA", "LFB", "LFC"
                };
                values = new String[]
                {
                    "-0.2", "0.3", "-0.6", "0.1", "0.1", "0.4", "0", "0", "0"
                };
                break;
            case 8:
            case 7:
                names = new String[]
                {
                    "LFA", "LFB", "LFC", "RRA", "RRB", "RRC", "RFA", "RFB", "RFC"
                };
                values = new String[]
                {
                    "0.1", "0.1", "0.4", "-0.2", "0.3", "-0.6", "0", "0", "0"
                };
                break;

            default:
                names = new String[]
                {
                    "LFA"
                };
                values = new String[]
                {
                    "0"
                };
                break;
        }
        bot.getAct().act(new Multidrive(names, values));
        if(walkCount > 7)

        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);                
            }
        }

	protected double[] getTriple( String txt ) {
		StringTokenizer st = new StringTokenizer(txt, DELIMITERS_EXTENDED);
		
		int num = 1;
		while (num-- > 0 && st.hasMoreTokens()) {
			st.nextToken();
		}
		
		// now we should have an token with integer
		try {
			double t1,t2,t3;
			t1 = new Double(st.nextToken()).doubleValue();
			t2 = new Double(st.nextToken()).doubleValue();
			t3 = new Double(st.nextToken()).doubleValue();
        	return new double[]{t1,t2,t3};
        } catch (NumberFormatException e) {
        	exceptionOccured(e, "Wrong double number format in '"+txt+".");
        	return null; 
        } catch (Exception e) {
        	exceptionOccured(e, "Can't get triple from text '"+txt+"'.");
        	return null;
        }
	}
	
	/**
	 * Return a Location instance starting from num token(tokens are separated by space) in txt
	 * 
	 * @param  txt  text of parameter
	 * @return Location from given token.
	 */
	protected Location locationDoubleValue(String txt) {

    public SuperSensor getSensorByTypeName(String type, String name)
    {
        if(this.isEmpty())
        {
            return null;
        }
        if(this.containsKey(type) && !this.get(type).isEmpty())
        {
            return this.get(type).get(name);
        }
        return null;
    }

    /**
     * For each type of sensor it adds all individuals to the returnee List as a
     * couple (Type, Name)
     *
     * @return returns Map of couples (Type/Name) of non empty sensor
     * representatives.
     */
    public List<MessageDescriptor> getNonEmptyDescription()
    {
        if(this.isEmpty())
        {
            return null;
        }
        List<MessageDescriptor> list = new ArrayList<MessageDescriptor>();
        for(String type : this.keySet())
        {
            for(String name : this.get(type).keySet())
            {
                list.add(new MessageDescriptor(type, name));
            }
        }
        return list;
    }
}

        }
    }

    /**
     * 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));
    }

    public static 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;
        }
    }

     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 
     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 
     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 
     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0, 
     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0

            Iterator it = PartsValues.entrySet().iterator();
            while(it.hasNext())
            {
                Map.Entry en = (Map.Entry) it.next();
                buf.append(" ").append(en.getKey()).append(" ").append(en.getValue()).append(",");
            }
            buf.append(" | ");
        }

        return buf.toString();

    }

    /**
     * Gets all properties and values to create a HTML formated string;
     *
     * @return Returns all properties in HTML format
     */
    public String toHtmlString()
    {
        StringBuilder buf = new StringBuilder();
        buf.append(super.toString()
                + "<b>Type</b> : "
                + String.valueOf(Type)
                + " <br/> "
                + "<b>Time</b> : "