package AIT_2022.module.complex;

import adf.core.agent.communication.standard.bundle.StandardMessage;
import adf.core.agent.communication.standard.bundle.StandardMessagePriority;
import adf.core.component.module.complex.RoadDetector;
import adf.core.component.module.algorithm.*;
import adf.core.component.communication.CommunicationMessage;
import adf.core.agent.info.*;
import adf.core.agent.module.ModuleManager;
import adf.core.agent.develop.DevelopData;
import adf.core.agent.action.Action;
import adf.core.agent.communication.MessageManager;
import adf.core.agent.communication.standard.bundle.centralized.*;
import adf.core.agent.communication.standard.bundle.information.*;
import rescuecore2.worldmodel.EntityID;
import rescuecore2.standard.entities.*;
import static rescuecore2.standard.entities.StandardEntityURN.*;
import rescuecore2.misc.geometry.*;
import org.locationtech.jts.geom.*;

import AIT_2022.module.algorithm.GraphEdgeValidator;
import AIT_2022.module.complex.pf.CivMessages;

import java.util.*;
import java.util.stream.*;
import static java.util.stream.Collectors.*;
import static java.util.Comparator.*;

// @ DEBUG {{{
// import com.mrl.debugger.remote.VDClient;
import AIT_2022.module.util.DebugUtil;
// }}}

public class AITPfDetector extends RoadDetector
{
    private EntityID result = null;

    private Clustering clusterer;
    private StaticClustering highways;
    private Clustering stuckedHumans;
    private PathPlanning pathPlanner;
    private StaticClustering overcrowdingArea;
    private PathPlanning astar;
    private CivMessages civMessages;

    private Map<EntityID, Integer> tasks = new HashMap<>();
    private Set<EntityID> cluster = new HashSet<>();
    private Set<EntityID> completed = new HashSet<>();
    //key = agent's EntityID, value = Cluster
    private Map<EntityID, Collection<EntityID>> buriedPoliceCluster = new HashMap<>();

    // @ DEBUG {{{
    // private VDClient vdclient = VDClient.getInstance();
    DebugUtil debug;
    // }}}

    public AITPfDetector(
        AgentInfo ai, WorldInfo wi, ScenarioInfo si,
        ModuleManager mm, DevelopData dd)
    {
        super(ai, wi, si, mm, dd);

        this.clusterer = mm.getModule(
            "SampleRoadDetector.Clustering",
            "adf.sample.module.algorithm.SampleKMeans");
        this.registerModule(this.clusterer);

        this.highways = mm.getModule("AIT_2022.module.algorithm.Highways");
        this.registerModule(this.highways);

        this.stuckedHumans = mm.getModule(
            "AIT_2022.module.algorithm.StuckedHumans");
        this.registerModule(this.stuckedHumans);

        this.pathPlanner = mm.getModule(
            "RoadDetector.PathPlanning",
            "adf.sample.module.algorithm.SamplePathPlanning");
        this.registerModule(this.pathPlanner);

        this.overcrowdingArea = mm.getModule("AIT_2022.module.algorithm.OvercrowdingArea");
        this.registerModule(this.overcrowdingArea);

        this.astar = mm.getModule("AIT_2022.module.algorithm.AstarPathPlanning");
        this.registerModule(this.astar);

        this.civMessages = mm.getModule("AIT.PF.CivMessages");
        this.registerModule(this.civMessages);

        // @ DEBUG {{{
        // this.vdclient.init();
        this.debug = new DebugUtil(this.agentInfo, this.worldInfo, this.scenarioInfo);
        // }}}
    }

    @Override
    public RoadDetector calc()
    {
        this.civMessages.calc();
        this.result = null;
        if (this.needIdle()) return this;
        
        // int time = this.agentInfo.getTime();
        // EntityID myId = this.agentInfo.getID();
        // String str = this.needIdle()+" ," + myId + " ,1";
        // if (this.result != null)
        //     this.debug.showResult(this.worldInfo.getEntity(this.result), "PfDetector", time, str);

        final Stream<EntityID> candidates =
            this.tasks.keySet()
                .stream()
                .filter(t -> !this.completed.contains(t))
                .sorted(this.comparator1()).limit(5);

        this.result =
            candidates.min(this.comparator2()).orElse(null);
        // str = this.needIdle()+" ," + myId + " ,2";
        // if (this.result != null)
        //     this.debug.showResult(this.worldInfo.getEntity(this.result), "PfDetector", time, str);

        if (this.result == null)
        {
            final int size = this.cluster.size();
            this.expandCluster();
            this.renewTasksOnCluster();
            this.renewTasksWithOvercrowdingArea(1);
            if (size != this.cluster.size()) return this.calc();
        }
        // str = this.needIdle()+" ," + myId + " ,3";
        // if (this.result != null)
        //     this.debug.showResult(this.worldInfo.getEntity(this.result), "PfDetector", time, str);

        // if (this.scenarioInfo.isDebugMode()) this.showResult();
        //if(this.result != null)
        //{
        //    List<String> resultList = new ArrayList<>();
        //    StandardEntity resultEntity = this.worldInfo.getEntity(this.result);
        //    resultList.add(this.agentInfo.getID().toString());
        //    resultList.add(Integer.valueOf(this.agentInfo.getTime()).toString());
        //    resultList.add(resultEntity.toString());
        //    resultList.add(this.tasks.get(this.result).toString());
        //    resultList.add(this.worldInfo.getLocation(this.result).first().toString());
        //    resultList.add(this.worldInfo.getLocation(this.result).second().toString());
        //    this.debug.csvFormatStdout("RESULT", resultList);
        //}
        return this;
    }

    private Comparator<EntityID> comparator1()
    {
        final EntityID me = this.agentInfo.getID();

        final Comparator<EntityID> comparator1 = comparing(this.tasks::get);
        final Comparator<EntityID> comparator2 = comparing(
            i -> this.worldInfo.getDistance(me, i));
        return comparator1.thenComparing(comparator2);
    }

    private Comparator<EntityID> comparator2()
    {
        final EntityID position = this.agentInfo.getPosition();

        final Comparator<EntityID> comparator1 = comparing(this.tasks::get);
        final Comparator<EntityID> comparator2 = comparing(
            i -> this.computeDistance(position, i));
        return comparator1.thenComparing(comparator2);
    }

    private double computeDistance(EntityID from, EntityID destination)
    {
        this.pathPlanner.setFrom(from);
        this.pathPlanner.setDestination(destination);
        this.pathPlanner.calc();
        final List<EntityID> path = this.pathPlanner.getResult();
        final int n = path.size();

        double ret = 0.0;
        for (int i=1; i<n; ++i)
            ret += this.worldInfo.getDistance(path.get(i-1), path.get(i));
        return ret;
    }

    private void putTask(EntityID task, int priority)
    {
        final int current = this.tasks.getOrDefault(task, Integer.MAX_VALUE);
        this.tasks.put(task, Math.min(current, priority));
    }

    @Override
    public EntityID getTarget()
    {
        return this.result;
    }

    @Override
    public RoadDetector updateInfo(MessageManager mm)
    {
        super.updateInfo(mm);
        if (this.getCountUpdateInfo() > 1) return this;

        final StandardEntity entity
                = this.worldInfo.getPosition(this.agentInfo.getID());
        if (entity instanceof Road) {
            final Area area = (Area) entity;
            if (area.getBlockades() == null || area.getBlockades().isEmpty()) {
                this.tasks.remove(entity.getID());
            }
        }

        // adding candidate with cluster
        if (this.cluster.isEmpty())
        {
            this.initCluster();
            this.renewTasksOnCluster();
            this.renewTasksWithOvercrowdingArea(1);
        }

        //if the agent find buried agent, it add buried-agent-cluster in own cluster.
        this.updateBuriedPoliceWithPerception();
        this.updateCompletedTasksWithPercept();
        this.putRequestsWithPercept();
        this.putNeighborBlockadesWithPerception();

        // receiving & sending message
        this.receiveMessage(mm);
        this.sendMessage(mm);
        return this;
    }

    private void initCluster()
    {
        this.clusterer.calc();

        final EntityID me = this.agentInfo.getID();
        final int index = this.clusterer.getClusterIndex(me);
        final Collection<EntityID> ids =
            this.clusterer.getClusterEntityIDs(index);
        this.cluster.addAll(ids);
    }

    private void expandCluster()
    {
        final EntityID me = this.agentInfo.getID();

        final int n = this.clusterer.getClusterNumber();
        final int index = this.clusterer.getClusterIndex(me);

        final int size = this.cluster.size();
        for (int i=1; i<n && size==this.cluster.size(); ++i)
        {
            final Collection<EntityID> ids =
                this.clusterer.getClusterEntityIDs(index+i*n);
            this.cluster.addAll(ids);
        }
    }

    private void renewTasksOnCluster()
    {
        this.renewTasksWithCluster(8);
        this.renewTasksWithBuildings(7);
        this.renewTasksWithHighways(6);
        this.renewTasksWithBuildingsAgentAt(5);
        this.renewTasksWithRefuges(0);
    }

    // @ RENEW TASKS {{{
    private void renewTasksWithCluster(int priority)
    {
        this.cluster.forEach(i -> this.putTask(i, priority));
    }

    private void renewTasksWithBuildings(int priority)
    {
        final Stream<EntityID> buildings =
            this.cluster
                .stream()
                .map(this.worldInfo::getEntity)
                .filter(Building.class::isInstance)
                .map(StandardEntity::getID)
                .filter(this.cluster::contains);

        buildings.forEach(i -> this.putTask(i, priority));
    }

    private void renewTasksWithBuildingsAgentAt(int priority)
    {
        final EntityID me = this.agentInfo.getID();
        final Stream<StandardEntity> agentAt =
            this.worldInfo
                .getEntityIDsOfType(FIRE_BRIGADE, POLICE_FORCE, AMBULANCE_TEAM)
                .stream().filter(i -> !i.equals(me))
                .map(i -> this.worldInfo.getPosition(1, i));

        final Stream<EntityID> buildings =
            agentAt
                .filter(Building.class::isInstance)
                .map(StandardEntity::getID)
                .filter(this.cluster::contains);

        buildings.forEach(i -> this.putTask(i, priority));
    }

    private void renewTasksWithRefuges(int priority)
    {
        final Stream<EntityID> refuges =
            this.cluster
                .stream()
                .map(this.worldInfo::getEntity)
                .filter(Refuge.class::isInstance)
                .map(StandardEntity::getID);

        refuges.forEach(i -> this.putTask(i, priority));
    }

    private void renewTasksWithHighways(int priority)
    {
        final Stream<EntityID> highways =
            this.highways.getClusterEntityIDs(0)
                .stream()
                .filter(this.cluster::contains)
                .map(this.worldInfo::getEntity)
                .filter(Road.class::isInstance)
                .map(StandardEntity::getID);

        highways.forEach(i -> this.putTask(i, priority));
    }

    private void renewTasksWithOvercrowdingArea(int priority)
    {
        EntityID myId = this.agentInfo.getID();
        EntityID myPosition = this.agentInfo.getPosition();
        int ind = this.overcrowdingArea.getClusterIndex(myId);
        Collection<EntityID> areas = this.overcrowdingArea.getClusterEntityIDs(ind);
        for (EntityID area : areas)
        {
            if (this.isSuperintendOvercrowdingArea(area))
            {
                Collection<EntityID> path = this.astar.getResult(myPosition, area);
                path.removeAll(this.completed);
                path.forEach(id -> this.putTask(id, priority));
            }
        }
    }
    // }}}

    // @ CHECK COMPLETED {{{
    private void updateCompletedTasksWithPercept()
    {
        final Set<EntityID> changes =
            this.worldInfo.getChanged().getChangedEntities();
        final EntityID position = this.agentInfo.getPosition();

        Map<EntityID, EntityID> focused = new HashMap<>();
        for (EntityID id : changes)
        {
            final StandardEntity entity =
                this.worldInfo.getEntity(id);
            if (!Area.class.isInstance(entity)) continue;

            final EntityID value =
                !id.equals(position) && Building.class.isInstance(entity) ?
                    this.seekEntranceOnBuilding(id) : id;

            if (this.isRoadCompleted(value)) focused.put(id, value);
        }

        Set<EntityID> scope = new HashSet<>();
        Queue<EntityID> queue = new LinkedList<>();
        queue.add(position);

        while (!queue.isEmpty())
        {
            final EntityID id = queue.remove();
            if (!focused.containsKey(id)) continue;
            if (scope.contains(id)) continue;

            scope.add(id);
            final Area area = (Area)this.worldInfo.getEntity(id);
            area.getNeighbours().forEach(queue::add);
        }

        final Stream<EntityID> updates = focused.keySet()
            .stream().filter(k -> scope.contains(focused.get(k)));
        updates.forEach(this.completed::add);
    }

    private EntityID seekEntranceOnBuilding(EntityID building)
    {
        final EntityID position = this.agentInfo.getPosition();

        this.pathPlanner.setFrom(position);
        this.pathPlanner.setDestination(building);
        this.pathPlanner.calc();
        final List<EntityID> path =
            this.normalize(this.pathPlanner.getResult());

        final int n = path.size();
        for (int i=n-1; i>=0; --i)
        {
            final EntityID id = path.get(i);
            final StandardEntity entity = this.worldInfo.getEntity(id);
            if (entity instanceof Road) return id;
        }

        return null;
    }

    private List<EntityID> normalize(List<EntityID> path)
    {
        List<EntityID> ret = new ArrayList<>(path);

        final EntityID position = this.agentInfo.getPosition();
        if (ret.isEmpty() || !ret.get(0).equals(position)) ret.add(0, position);
        return ret;
    }

    private static final double AGENT_RADIUS = 500.0;
    private boolean isRoadCompleted(EntityID id)
    {
        if (id == null) return false;

        final Area area = (Area)this.worldInfo.getEntity(id);
        if (!area.isBlockadesDefined()) return true;

        final Collection<Blockade> blockades =
            area.getBlockades()
                .stream()
                .map(this.worldInfo::getEntity)
                .map(Blockade.class::cast)
                .collect(toList());

        final Geometry passable =
            GraphEdgeValidator.computePassable(area, blockades);

        final List<Line2D> edges =
            area.getEdges()
                .stream()
                .filter(Edge::isPassable)
                .map(Edge::getLine)
                .collect(toList());

        final int n = edges.size();
        for (int i=0; i<n; ++i) for (int j=i+1; j<n; ++j)
        {
            final Line2D l1 = edges.get(i);
            final Line2D l2 = edges.get(j);
            if (l1.getDirection().getLength() < AGENT_RADIUS*2) continue;
            if (l2.getDirection().getLength() < AGENT_RADIUS*2) continue;
            if (!GraphEdgeValidator.validate(passable, l1, l2)) return false;
        }

        return true;
    }
    // }}}

    private void putRequestsWithPercept()
    {
        final Set<EntityID> changes =
            this.worldInfo.getChanged().getChangedEntities();

        this.stuckedHumans.calc();
        final Collection<EntityID> stucks =
            this.stuckedHumans.getClusterEntityIDs(0);
        for (EntityID id : changes)
        {
            if (!stucks.contains(id)) continue;

            final StandardEntity entity = this.worldInfo.getEntity(id);
            if (!(entity instanceof Human)) continue;
            if (entity.getStandardURN() == POLICE_FORCE) continue;
            if (entity.getStandardURN() == CIVILIAN) continue;

            final Human human = (Human)entity;
            final EntityID position = human.getPosition();

            this.putTask(position, 2);
            this.completed.remove(position);
        }
    }

    private void putNeighborBlockadesWithPerception()
    {
        Collection<Human> humans = this.worldInfo.getChanged().getChangedEntities()
                .stream()
                .map(this.worldInfo::getEntity)
                .filter(Human.class::isInstance)
                .map(Human.class::cast)
                .filter(human -> !(this.worldInfo.getPosition(human) instanceof AmbulanceTeam))
                .filter(human -> !(human instanceof PoliceForce))
                .collect(toSet());

        for (Human human : humans)
        {
            Area area = (Area) this.worldInfo.getPosition(human);
            area.getNeighbours()
                    .stream()
                    .map(this.worldInfo::getEntity)
                    .filter(Area.class::isInstance)
                    .map(Area.class::cast)
                    .filter(a -> !(this.completed.contains(a)))
                    .filter(Area::isBlockadesDefined)
                    .map(Area::getID)
                    .forEach(neighbor -> this.putTask(neighbor, 2));
        }
    }

    private boolean needIdle()
    {
        final int time = this.agentInfo.getTime();
        final int ignored = this.scenarioInfo.getKernelAgentsIgnoreuntil();
        return time < ignored;
    }

    private void receiveMessage(MessageManager mm)
    {
        final List<CommunicationMessage> cmdpols =
                mm.getReceivedMessageList(CommandPolice.class);
        for (CommunicationMessage tmp : cmdpols)
        {
            final CommandPolice message = (CommandPolice)tmp;
            this.handleMessage(message);
        }
    }

    private void handleMessage(CommandPolice command)
    {
        if (command.getAction() != CommandPolice.ACTION_CLEAR) return;
        if (!command.isTargetIDDefined()) return;

        final EntityID sender = command.getSenderID();
        final StandardEntity entity = this.worldInfo.getEntity(sender);
        if (entity.getStandardURN().equals(POLICE_FORCE)) return;

        final EntityID position = command.getTargetID();
        if (!this.cluster.contains(position)) return;

        this.putTask(position, 4);
        this.completed.remove(position);

        final Set<EntityID> change = this.worldInfo.getChanged().getChangedEntities();
        if (change.contains(sender) || change.contains(position) 
                || this.worldInfo.getDistance(position, this.agentInfo.getPosition()) 
                        <= this.scenarioInfo.getRawConfig().getIntValue("comms.channels.0.range"))
            this.putTask(command.getTargetID(), 4);

    }

    /**
     * This method decide type of message to send.
     * The priority of message is as follows:
     * 1. MessagePoliceForce if tha agent is buried.
     * 2. MessageFireBrigade or MessagePoliceForce or MessageAmbulanceTeam if the agent find the buried.
     *    if step%3 = 0 then MessageFireBrigades, if step%3 = 1 then MessagePoliceForce,
     *    if step%3 = 2 then MessageAmbulanceTeam
     * 3. MessageCivilian if the agent find the buried civilian.
     * @param mm MessageManager
     */
    private void sendMessage(MessageManager mm)
    {
        EntityID  me = this.agentInfo.getID();
        PoliceForce myEntity = (PoliceForce) this.worldInfo.getEntity(me);
        int now = this.agentInfo.getTime();
        boolean voice;
        boolean radio;
        int noAction = -1;
        EntityID noTarget = new EntityID(-1);

        List<StandardMessage> sendMessages;

        //1.
        if (this.isMyselfBuried()) {
            PoliceForce policeForce = (PoliceForce) this.agentInfo.me();
            mm.addMessage(new MessagePoliceForce(
                    true, policeForce, -1, this.agentInfo.getID()));
            mm.addMessage(new MessagePoliceForce(
                    false, policeForce, -1, this.agentInfo.getID()));
            return;
        }

        Set<Human> buriedHumans = this.worldInfo.getChanged().getChangedEntities()
                .stream()
                .filter(this::needToRescue)
                .map(this.worldInfo::getEntity)
                .filter(Human.class::isInstance)
                .map(Human.class::cast)
                .collect(toSet());

        //2
        if (now % 3 == 0)
        {
            FireBrigade buriedFb = buriedHumans
                    .stream()
                    .filter(FireBrigade.class::isInstance)
                    .map(FireBrigade.class::cast)
                    .min(Comparator.comparing(e -> this.worldInfo.getDistance(me, e.getID())))
                    .orElse(null);
            if (buriedFb != null)
            {
                voice = true;
                radio = true;
                List<StandardMessage> messages = makeMessageFb(voice, radio, buriedFb, noAction, noTarget);
                for (StandardMessage message : messages) this.sendMessage(mm, message);
                return;
            }
        }

        if (now % 3 == 1)
        {
            PoliceForce buriedPf = buriedHumans
                    .stream()
                    .filter(PoliceForce.class::isInstance)
                    .map(PoliceForce.class::cast)
                    .min(Comparator.comparing(e -> this.worldInfo.getDistance(me, e.getID())))
                    .orElse(null);
            if (buriedPf != null)
            {
                voice = true;
                radio = true;
                List<StandardMessage> messages = this.makeMessagePf(voice, radio, buriedPf, noAction, noTarget);
                for (StandardMessage message : messages) this.sendMessage(mm, message);
                return;
            }
        }

        if (now %3 ==2)
        {
            AmbulanceTeam buriedAt = buriedHumans
                    .stream()
                    .filter(AmbulanceTeam.class::isInstance)
                    .map(AmbulanceTeam.class::cast)
                    .min(Comparator.comparing(e -> this.worldInfo.getDistance(me, e.getID())))
                    .orElse(null);
            if (buriedAt != null)
            {
                voice = true;
                radio = true;
                List<StandardMessage> messages = this.makeMessageAt(voice, radio, buriedAt, noAction, noTarget);
                for (StandardMessage message : messages) this.sendMessage(mm, message);
                return;
            }
        }

        // 4.
        Civilian buriedCivilian = buriedHumans
                .stream()
                .filter(Civilian.class::isInstance)
                .map(Civilian.class::cast)
                .min(Comparator.comparing(e -> this.worldInfo.getDistance(me, e.getID())))
                .orElse(null);
        if (buriedCivilian != null)
        {
            voice = true;
            radio = false;
            List<StandardMessage> messages = this.makeMessageCivilian(voice, radio, buriedCivilian);
            for (StandardMessage message : messages) this.sendMessage(mm, message);
        }
    }

    private boolean isMyselfBuried()
    {
        EntityID myID = this.agentInfo.getID();
        Human me = (Human) this.worldInfo.getEntity(myID);
        if (!(this.agentInfo.getPositionArea() instanceof Building)) return false;
        if (!me.isBuriednessDefined()) return false;
        return me.getBuriedness() != 0;
    }

    private void sendMessage(MessageManager mm, StandardMessage message)
    {
        mm.addMessage(message);
    }

    private List<StandardMessage> makeMessagePf(boolean voice, boolean radio, PoliceForce policeForce, int action, EntityID target)
    {
        List<StandardMessage> list = new ArrayList<>();

        if (voice)
        {
            MessagePoliceForce message = new MessagePoliceForce(false, policeForce, action, target);
            list.add(message);
        }

        if (radio)
        {
            MessagePoliceForce message = new MessagePoliceForce(true, policeForce, action, target);
            list.add(message);
        }
        return list;
    }

    private List<StandardMessage> makeMessageAt(boolean voice, boolean radio, AmbulanceTeam ambulanceTeam, int action, EntityID target)
    {
        List<StandardMessage> list = new ArrayList<>();
        if (voice)
        {
            MessageAmbulanceTeam message = new MessageAmbulanceTeam(false, ambulanceTeam, action, target);
            list.add(message);
        }

        if (radio)
        {
            MessageAmbulanceTeam message = new MessageAmbulanceTeam(true, ambulanceTeam, action, target);
            list.add(message);
        }

        return list;
    }

    private List<StandardMessage> makeMessageFb(boolean voice, boolean radio, FireBrigade fireBrigade, int action, EntityID target)
    {
        List<StandardMessage> list = new ArrayList<>();
        if (voice)
        {
            MessageFireBrigade message = new MessageFireBrigade(false, fireBrigade, action, target);
            list.add(message);
        }

        if (radio)
        {
            MessageFireBrigade message = new MessageFireBrigade(true, fireBrigade, action, target);
            list.add(message);
        }

        return list;
    }

    private List<StandardMessage> makeMessageCivilian(boolean voice, boolean radio, Civilian civilian)
    {
        List<StandardMessage> list = new ArrayList<>();
        if (voice)
        {
            MessageCivilian message = new MessageCivilian(false, civilian);
            list.add(message);
        }

        if (radio)
        {
            MessageCivilian message = new MessageCivilian(true, civilian);
            list.add(message);
        }

        return list;
    }


    private boolean needToRescue(EntityID id)
    {
        if (!this.isHuman(id)) return false;
        if (this.worldInfo.getPosition(id).getStandardURN() != BUILDING)
            return false;
        final Human h = (Human) this.worldInfo.getEntity(id);
        final int hp = h.getHP();
        final int damage = h.getDamage();
        final int buried = h.getBuriedness();
        if (hp*damage*buried == 0) return false;
        return true;
    }

    private boolean isHuman(EntityID id)
    {
        if (this.worldInfo.getEntity(id) instanceof Human) return true;
        return false;
    }

    private boolean isSendHumanInfo(Human h)
    {
        if (this.worldInfo.getPosition(h) instanceof Refuge) return false;
        if (h.getHP() == 0) return false;
        return h.getDamage() != 0;
    }

    private void showResult()
    {
        final EntityID me = this.agentInfo.getID();
        String out= "";
        out += String.format("🚜 [%10d] ", me.getValue());
        out += String.format("ROAD-DETECTOR -> %10d", this.result.getValue());
        System.out.println(out);
    }


    private final Double MAX_DISTANCE =
            Math.max(this.worldInfo.getBounds().getMaxX(), this.worldInfo.getBounds().getMaxY());
    private final Double ONE_SIDE =
            this.scenarioInfo.getScenarioAgentsPf() != 0 ? Math.floor(Math.sqrt(this.scenarioInfo.getScenarioAgentsPf())) : 1;
    private final Double INCLUDE_DISTANCE = MAX_DISTANCE / ONE_SIDE;

    private boolean judgeAddClusterWithBuriedPolice(PoliceForce policeForce)
    {

        Integer ownClusterIndex = this.clusterer.getClusterIndex(this.agentInfo.getID());
        EntityID ownArea =
                this.clusterer.getClusterEntityIDs(ownClusterIndex)
                        .stream()
                        .min(Comparator.comparing(EntityID::getValue))
                        .orElse(new EntityID(-1));

        if (ownArea.equals(new EntityID(-1))) return false;

        Integer someoneClusterIndex = this.clusterer.getClusterIndex(policeForce.getID());
        EntityID someoneArea =
                this.clusterer.getClusterEntityIDs(someoneClusterIndex)
                        .stream()
                        .min(Comparator.comparing(EntityID::getValue))
                        .orElse(new EntityID(-1));

        if (someoneArea.equals(new EntityID(-1))) return false;

        return this.worldInfo.getDistance(ownArea, someoneArea) < INCLUDE_DISTANCE;
    }

    //if buried-Police is in agent's field of view, agent add buried-police's cluster.
    private void updateBuriedPoliceWithPerception()
    {
        EntityID myPosition = this.agentInfo.getPosition();
        //peek police force
        Collection<PoliceForce> policeForces =
                this.worldInfo.getChanged().getChangedEntities().stream()
                        .map(this.worldInfo::getEntity)
                        .filter(e -> e instanceof PoliceForce)
                        .map(PoliceForce.class::cast)
                        .filter(PoliceForce::isBuriednessDefined)
                        .collect(toSet());

        for (PoliceForce policeForce : policeForces)
        {
            // remove not-buried-police
            if (policeForce.getID().equals(this.agentInfo.getID())) continue;
            if (policeForce.getBuriedness() == 0)
            {
                if(this.buriedPoliceCluster.containsKey(policeForce.getID()))
                    this.cluster.removeAll(this.buriedPoliceCluster.get(policeForce.getID()));
                this.buriedPoliceCluster.remove(policeForce.getID());
            }
            // set buried-police
            else if (!this.buriedPoliceCluster.containsKey(policeForce.getID()))
            {
                if(this.judgeAddClusterWithBuriedPolice(policeForce))
                {
                    //add someone's cluster
                    this.addSomeoneClusterInMyCluster(policeForce.getID());
                    //add someone's superintend overcrowding area
                    Integer overcrowdingAreaInd = this.overcrowdingArea.getClusterIndex(policeForce.getID());
                    Collection<EntityID> areas = this.overcrowdingArea.getClusterEntityIDs(overcrowdingAreaInd);
                    areas.stream().sorted(Comparator.comparing(EntityID::getValue))
                            .forEach(area ->
                            {
                                if (this.isSuperintendOvercrowdingArea(area, policeForce.getID()))
                                {
                                    Collection<EntityID> path = this.astar.getResult(myPosition, area);
                                    path.removeAll(this.completed);
                                    path.forEach(id -> this.putTask(id, 1));
                                }
                            });
                }
                else//If agent are not in charge, set size 0
                    this.buriedPoliceCluster.put(policeForce.getID(), new HashSet<>(0));
            }
        }
    }

    private void addSomeoneClusterInMyCluster(EntityID someone)
    {
        int ind = this.clusterer.getClusterIndex(someone);
        Collection<EntityID> someoneCluster = this.clusterer.getClusterEntityIDs(ind);
        this.buriedPoliceCluster.put(someone, someoneCluster);
        this.cluster.addAll(someoneCluster);
    }

    private boolean isSuperintendOvercrowdingArea(EntityID area)
    {
        if (this.completed.contains(area)) return false;
        int ownClusterIndex = this.clusterer.getClusterIndex(this.agentInfo.getID());
        EntityID ownArea =
                this.clusterer.getClusterEntityIDs(ownClusterIndex)
                        .stream()
                        .min(Comparator.comparing(EntityID::getValue))
                        .orElse(null);
        if (ownArea == null) return false;

        return this.worldInfo.getDistance(ownArea, area) < INCLUDE_DISTANCE;
    }

    private boolean isSuperintendOvercrowdingArea(EntityID area, EntityID someone)
    {
        if (this.completed.contains(area)) return false;
        int someoneClusterIndex = this.clusterer.getClusterIndex(someone);
        EntityID ownArea =
                this.clusterer.getClusterEntityIDs(someoneClusterIndex)
                        .stream()
                        .min(Comparator.comparing(EntityID::getValue))
                        .orElse(null);
        if (ownArea == null) return false;

        return this.worldInfo.getDistance(ownArea, area) < INCLUDE_DISTANCE;
    }
}
