package AIT_2022.module.complex.common;


import java.util.Set;
import java.util.stream.Collectors;

import adf.core.agent.info.AgentInfo;
import adf.core.agent.info.ScenarioInfo;
import adf.core.agent.info.WorldInfo;
import rescuecore2.misc.Pair;
import rescuecore2.misc.geometry.Line2D;
import rescuecore2.misc.geometry.Point2D;
import rescuecore2.standard.entities.*;
import rescuecore2.worldmodel.EntityID;

import static java.util.Comparator.*;

import java.util.HashSet;

public class EntityFilter {
    private WorldInfo wi;
    private AgentInfo ai;
    private ScenarioInfo si;
    private Set<EntityID> changed = new HashSet<>();

    public EntityFilter(WorldInfo wi, AgentInfo ai, ScenarioInfo si) {
        this.wi = wi;
        this.ai = ai;
        this.si = si;
        this.changed = this.wi.getChanged().getChangedEntities();
    }

    public EntityFilter(WorldInfo wi, AgentInfo ai) {
        this.wi = wi;
        this.ai = ai;
        this.changed = this.wi.getChanged().getChangedEntities();
    }

    public void update(WorldInfo wi, AgentInfo ai) {
        this.wi = wi;
        this.ai = ai;
        this.changed = this.wi.getChanged().getChangedEntities();
    }

    public EntityID getNearyAT() {
        return this.changed.stream()
                .map(this.wi::getEntity)
                .filter(e -> e instanceof AmbulanceTeam)
                .map(AmbulanceTeam.class::cast)
                .min(comparing(a -> this.distance(a.getID(), this.ai.getID())))
                .map(AmbulanceTeam::getID).orElse(null);
    }

    public EntityID getNearyFB() {
        return this.changed.stream()
                .map(this.wi::getEntity)
                .filter(e -> e instanceof FireBrigade)
                .map(FireBrigade.class::cast)
                .min(comparing(a -> this.distance(a.getID(), this.ai.getID())))
                .map(FireBrigade::getID).orElse(null);
    }

    public EntityID getNearyPF() {
        return this.changed.stream()
                .map(this.wi::getEntity)
                .filter(e -> e instanceof PoliceForce)
                .map(PoliceForce.class::cast)
                .min(comparing(a -> this.distance(a.getID(), this.ai.getID())))
                .map(PoliceForce::getID).orElse(null);
    }

    public EntityID getNearyATinVoiceRange() {
        return this.changed.stream()
                .map(this.wi::getEntity)
                .filter(e -> e instanceof AmbulanceTeam)
                .filter(e -> this.wi.getPosition(e.getID()) instanceof Road).map(AmbulanceTeam.class::cast)
                .min(comparing(a -> this.wi.getDistance(a.getPosition(), this.ai.getPosition())))
                .filter(a -> this.wi.getDistance(a.getPosition(), this.ai.getID()) < this.si
                        .getRawConfig().getIntValue("comms.channels.0.range"))
                .map(AmbulanceTeam::getID).orElse(null);
    }

    public EntityID getNearyFBinVoiceRange() {
        return this.changed.stream()
                .map(this.wi::getEntity)
                .filter(e -> e instanceof FireBrigade)
                .filter(e -> this.wi.getPosition(e.getID()) instanceof Road).map(FireBrigade.class::cast)
                .min(comparing(a -> this.wi.getDistance(a.getPosition(), this.ai.getPosition())))
                .filter(a -> this.wi.getDistance(a.getPosition(), this.ai.getID()) < this.si
                        .getRawConfig().getIntValue("comms.channels.0.range"))
                .map(FireBrigade::getID).orElse(null);
    }

    public EntityID getNearyPFinVoiceRange() {
        return this.changed.stream()
                .map(this.wi::getEntity)
                .filter(e -> e instanceof PoliceForce)
                .filter(e -> this.wi.getPosition(e.getID()) instanceof Road).map(PoliceForce.class::cast)
                .min(comparing(a -> this.wi.getDistance(a.getPosition(), this.ai.getPosition())))
                .filter(a -> this.wi.getDistance(a.getPosition(), this.ai.getID()) < this.si
                        .getRawConfig().getIntValue("comms.channels.0.range"))
                .map(PoliceForce::getID).orElse(null);
    }

    public Set<Blockade> getBlockades () {
        return this.changed.stream()
                .map(this.wi::getEntity)
                .filter(e -> e instanceof Blockade)
                .map(Blockade.class::cast)
                .collect(Collectors.toSet());
    }

    public Blockade getNearyBlockade() {
        return this.nearyBlockade(this.getBlockades(), this.ai.getX(), this.ai.getY());
    }

    public EntityID getNearyBlockadeID() {
        Blockade b = this.nearyBlockade(this.getBlockades(), this.ai.getX(), this.ai.getY()); 
        return b == null ? null : b.getID();
    }

    private double distance(EntityID a1, EntityID a2) {
        Pair<Integer, Integer> p1 = this.wi.getLocation(a1);
        Pair<Integer, Integer> p2 = this.wi.getLocation(a2);
        return Math.hypot(
            Math.abs(p1.first()-p2.first()), Math.abs(p1.second()-p2.second()));
    }

    private Blockade nearyBlockade(Set<Blockade> bs, double x, double y) {
        if (bs == null) return null;
        Blockade ret = null;
        double dis = Double.MAX_VALUE;
        final Stucked stucked = new Stucked(bs, x, y);
        for (Blockade b : bs) {
            if (stucked.isStucked())
                return b;
            final double d = this.getDistance(b, x, y);
            if (dis > d) {
                ret = b;
                dis = d;
            }
        }
        return ret;
    }

    private double getDistance(Blockade b, double x, double y) {
        int[] apexs = b.getApexes();
        double dis = Double.MAX_VALUE;
        for (int i = 0; i < apexs.length / 4; ++i) {
            final Line2D line = new Line2D(
                    new Point2D(apexs[i], apexs[i + 1]),
                    new Point2D(apexs[i + 2], apexs[i + 3]));
            final Point2D p = new Point2D(x, y);
            final double d = this.calcDistancePointToLine(p, line);
            if (dis > d)
                dis = d;
        }
        return dis;
    }

    private double calcDistancePointToLine(Point2D point, Line2D line) {
        final Point2D l1 = line.getOrigin();
        final Point2D l2 = line.getEndPoint();
        return Math
                .abs((l2.getX() - l1.getX()) * (l1.getY() - point.getY())
                        - (l1.getX() - point.getX()) * (l2.getY() - l1.getY()))
                / Math.sqrt(Math.pow(l2.getX() - l1.getX(), 2) + Math.pow(l2.getY() - l1.getY(), 2));
    }
}
