package com.codegame.codeseries.notreal2d.collision;

import com.codeforces.commons.geometry.*;
import com.codeforces.commons.holder.Mutable;
import com.codeforces.commons.holder.SimpleMutable;
import com.codeforces.commons.pair.Pair;
import com.codegame.codeseries.notreal2d.Body;
import com.codegame.codeseries.notreal2d.form.*;
import com.codegame.codeseries.notreal2d.util.GeometryUtil;
import org.apache.commons.lang3.mutable.MutableDouble;

import javax.annotation.*;
import java.util.ArrayList;
import java.util.List;

import static com.codeforces.commons.math.Math.*;

/**
 * @author Maxim Shipko ([email protected])
 * Date: 26.06.2015
 * TODO check epsilon comparison
 */
public class RectangleAndArcCollider extends ColliderBase {
    public RectangleAndArcCollider(@Nonnegative double epsilon) {
        super(epsilon);
    }

    @Override
    protected boolean matchesOneWay(@Nonnull Body bodyA, @Nonnull Body bodyB) {
        return bodyA.getForm().getShape() == Shape.RECTANGLE && bodyB.getForm().getShape() == Shape.ARC;
    }

    @SuppressWarnings({"OverlyComplexMethod", "OverlyLongMethod", "ConstantConditions"})
    @Nullable
    @Override
    protected CollisionInfo collideOneWay(@Nonnull Body bodyA, @Nonnull Body bodyB) {
        RectangularForm rectangularFormA = (RectangularForm) bodyA.getForm();
        ArcForm arcFormB = (ArcForm) bodyB.getForm();

        double radiusA = rectangularFormA.getCircumcircleRadius();
        double radiusB = arcFormB.getRadius();
        double distance = bodyA.getPosition().getDistanceTo(bodyB.getPosition());

        if (distance > radiusA + radiusB) {
            return null;
        }

        if (distance < abs(radiusA - radiusB)) {
            return null;
        }

        Point2D[] pointsA = rectangularFormA.getPoints(bodyA.getPosition(), bodyA.getAngle(), epsilon);
        int pointACount = pointsA.length;

        double squaredRadiusB = radiusB * radiusB;

        double startAngleB = bodyB.getAngle() + arcFormB.getAngle();
        double finishAngleB = startAngleB + arcFormB.getSector();

        Point2D point1B = bodyB.getPosition().copy().add(new Vector2D(radiusB, 0.0D).setAngle(startAngleB));
        Point2D point2B = bodyB.getPosition().copy().add(new Vector2D(radiusB, 0.0D).setAngle(finishAngleB));

        List<IntersectionInfo> intersectionInfos = new ArrayList<>();

        for (int pointAIndex = 0; pointAIndex < pointACount; ++pointAIndex) {
            Point2D point1A = pointsA[pointAIndex];
            Point2D point2A = pointsA[pointAIndex == pointACount - 1 ? 0 : pointAIndex + 1];

            Line2D lineA = Line2D.getLineByTwoPoints(point1A, point2A);

            if (lineA.getSignedDistanceFrom(bodyA.getPosition()) > -epsilon) {
                throw new IllegalStateException(String.format("%s of %s is too small, " +
                                "does not represent a convex polygon, or its points are going in wrong order.",
                        Form.toString(bodyA.getForm()), bodyA
                ));
            }

            double distanceFromB = lineA.getSignedDistanceFrom(bodyB.getPosition());

            if (distanceFromB > radiusB) {
                continue;
            }

            double leftA = min(point1A.getX(), point2A.getX());
            double topA = min(point1A.getY(), point2A.getY());
            double rightA = max(point1A.getX(), point2A.getX());
            double bottomA = max(point1A.getY(), point2A.getY());

            Point2D projectionOfB = lineA.getProjectionOf(bodyB.getPosition());

            double offset = sqrt(squaredRadiusB - distanceFromB * distanceFromB);
            Vector2D offsetVector = new Vector2D(point1A, point2A).copy().setLength(offset);

            Point2D intersectionPoint1 = projectionOfB.copy().add(offsetVector);

            if (doesPointBelongToAAndB(
                    intersectionPoint1, leftA, topA, rightA, bottomA, bodyB, startAngleB, finishAngleB
            )) {
                addIntersectionInfo(intersectionPoint1, point1A, point2A, lineA, intersectionInfos);
            }

            Point2D intersectionPoint2 = projectionOfB.copy().add(offsetVector.copy().negate());

            if (doesPointBelongToAAndB(
                    intersectionPoint2, leftA, topA, rightA, bottomA, bodyB, startAngleB, finishAngleB
            )) {
                addIntersectionInfo(intersectionPoint2, point1A, point2A, lineA, intersectionInfos);
            }
        }

        int intersectionCount = intersectionInfos.size();

        if (intersectionCount == 0) {
            // TODO check arc inside rectangle
            return null;
        } else if (intersectionCount == 1 && arcFormB.isEndpointCollisionEnabled() && (
                !GeometryUtil.isPointOutsideConvexPolygon(point1B, pointsA, epsilon)
                        || !GeometryUtil.isPointOutsideConvexPolygon(point2B, pointsA, epsilon)
        )) {
            IntersectionInfo intersectionInfo = intersectionInfos.get(0);
            int intersectionLineCount = intersectionInfo.intersectionLines.size();

            if (intersectionLineCount == 1 || intersectionLineCount == 2) { // TODO separate 1 and 2 ??
                Line2D intersectionLine = intersectionInfo.intersectionLines.get(0);

                double distanceFromPoint1B = intersectionLine.getSignedDistanceFrom(point1B);
                double distanceFromPoint2B = intersectionLine.getSignedDistanceFrom(point2B);

                for (int pointAIndex = 0; pointAIndex < pointACount; ++pointAIndex) {
                    Point2D point1A = pointsA[pointAIndex];
                    Point2D point2A = pointsA[pointAIndex == pointACount - 1 ? 0 : pointAIndex + 1];

                    Line2D lineA = Line2D.getLineByTwoPoints(point1A, point2A);

                    if (lineA.getSignedDistanceFrom(point1B) >= epsilon) {
                        return new CollisionInfo(
                                bodyA, bodyB, point2B, intersectionLine.getUnitNormal().negate(),
                                -distanceFromPoint2B, epsilon
                        );
                    }

                    if (lineA.getSignedDistanceFrom(point2B) >= epsilon) {
                        return new CollisionInfo(
                                bodyA, bodyB, point1B, intersectionLine.getUnitNormal().negate(),
                                -distanceFromPoint1B, epsilon
                        );
                    }
                }

                if (distanceFromPoint1B < distanceFromPoint2B) {
                    return new CollisionInfo(
                            bodyA, bodyB, point1B, intersectionLine.getUnitNormal().negate(),
                            -distanceFromPoint1B, epsilon
                    );
                } else {
                    return new CollisionInfo(
                            bodyA, bodyB, point2B, intersectionLine.getUnitNormal().negate(),
                            -distanceFromPoint2B, epsilon
                    );
                }
            } else {
                throw new IllegalStateException(String.format("%s of %s is too small, " +
                                "does not represent a convex polygon, or its points are going in wrong order.",
                        Form.toString(bodyA.getForm()), bodyA
                ));
            }
        } else {
            Vector2D vectorCB = new Vector2D(intersectionInfos.get(0).intersectionPoint, bodyB.getPosition());
            Vector2D vectorCA = new Vector2D(intersectionInfos.get(0).intersectionPoint, bodyA.getPosition());

            if (distance > radiusB - epsilon && vectorCB.dotProduct(vectorCA) < 0.0D) {
                Mutable<Point2D> nearestPoint = new SimpleMutable<>();
                MutableDouble distanceToNearestPoint = new MutableDouble();

                for (IntersectionInfo intersectionInfo : intersectionInfos) {
                    updateNearestPoint(bodyB, intersectionInfo.intersectionPoint, nearestPoint, distanceToNearestPoint);

                    for (Pair<Point2D, Point2D> pointAndPoint : intersectionInfo.intersectionLinePointPairs) {
                        updateNearestPoint(bodyB, pointAndPoint.getFirst(), nearestPoint, distanceToNearestPoint);
                        updateNearestPoint(bodyB, pointAndPoint.getSecond(), nearestPoint, distanceToNearestPoint);
                    }
                }

                return nearestPoint.get() == null ? null : new CollisionInfo(
                        bodyA, bodyB, nearestPoint.get(),
                        new Vector2D(bodyB.getPosition(), nearestPoint.get()).normalize(),
                        radiusB - distanceToNearestPoint.doubleValue(), epsilon
                );
            } else {
                Mutable<Point2D> farthestPoint = new SimpleMutable<>();
                MutableDouble distanceToFarthestPoint = new MutableDouble();

                for (IntersectionInfo intersectionInfo : intersectionInfos) {
                    updateFarthestPoint(
                            bodyB, intersectionInfo.intersectionPoint, farthestPoint, distanceToFarthestPoint,
                            startAngleB, finishAngleB
                    );

                    for (Pair<Point2D, Point2D> pointAndPoint : intersectionInfo.intersectionLinePointPairs) {
                        updateFarthestPoint(
                                bodyB, pointAndPoint.getFirst(), farthestPoint, distanceToFarthestPoint,
                                startAngleB, finishAngleB
                        );
                        updateFarthestPoint(
                                bodyB, pointAndPoint.getSecond(), farthestPoint, distanceToFarthestPoint,
                                startAngleB, finishAngleB
                        );
                    }
                }

                return farthestPoint.get() == null ? null : new CollisionInfo(
                        bodyA, bodyB, farthestPoint.get(),
                        new Vector2D(farthestPoint.get(), bodyB.getPosition()).normalize(),
                        distanceToFarthestPoint.doubleValue() - radiusB, epsilon
                );
            }
        }
    }

    private void updateNearestPoint(
            @Nonnull Body body, @Nonnull Point2D point, @Nonnull Mutable<Point2D> nearestPoint,
            @Nonnull MutableDouble distanceToNearestPoint) {
        double distanceToPoint = body.getDistanceTo(point);

        if (distanceToPoint >= epsilon
                && (nearestPoint.get() == null || distanceToPoint < distanceToNearestPoint.doubleValue())) {
            nearestPoint.set(point);
            distanceToNearestPoint.setValue(distanceToPoint);
        }
    }

    private static void updateFarthestPoint(
            @Nonnull Body body, @Nonnull Point2D point, @Nonnull Mutable<Point2D> farthestPoint,
            @Nonnull MutableDouble distanceToFarthestPoint, double startAngle, double finishAngle) {
        double distanceToPoint = body.getDistanceTo(point);

        if (GeometryUtil.isAngleBetween(new Vector2D(body.getPosition(), point).getAngle(), startAngle, finishAngle)
                && (farthestPoint.get() == null || distanceToPoint > distanceToFarthestPoint.doubleValue())) {
            farthestPoint.set(point);
            distanceToFarthestPoint.setValue(distanceToPoint);
        }
    }

    private boolean doesPointBelongToAAndB(
            @Nonnull Point2D point, double leftA, double topA, double rightA, double bottomA,
            @Nonnull Body bodyB, double startAngleB, double finishAngleB) {
        boolean belongsToA = (point.getX() > leftA - epsilon)
                && (point.getX() < rightA + epsilon)
                && (point.getY() > topA - epsilon)
                && (point.getY() < bottomA + epsilon);

        double pointAngleB = new Vector2D(bodyB.getPosition(), point).getAngle();
        if (pointAngleB < startAngleB) {
            pointAngleB += DOUBLE_PI;
        }

        boolean belongsToB = pointAngleB >= startAngleB && pointAngleB <= finishAngleB;

        return belongsToA && belongsToB;
    }

    private void addIntersectionInfo(
            Point2D point, Point2D point1A, Point2D point2A, Line2D lineA, List<IntersectionInfo> intersectionInfos) {
        boolean alreadyAdded = false;

        for (IntersectionInfo intersectionInfo : intersectionInfos) {
            if (intersectionInfo.intersectionPoint.nearlyEquals(point, epsilon)) {
                intersectionInfo.intersectionLines.add(lineA);
                intersectionInfo.intersectionLinePointPairs.add(new Pair<>(point1A, point2A));
                alreadyAdded = true;
                break;
            }
        }

        if (!alreadyAdded) {
            IntersectionInfo intersectionInfo = new IntersectionInfo(point);
            intersectionInfo.intersectionLines.add(lineA);
            intersectionInfo.intersectionLinePointPairs.add(new Pair<>(point1A, point2A));
            intersectionInfos.add(intersectionInfo);
        }
    }

    @SuppressWarnings("PublicField")
    private static final class IntersectionInfo {
        public final Point2D intersectionPoint;
        public final List<Line2D> intersectionLines = new ArrayList<>();
        public final List<Pair<Point2D, Point2D>> intersectionLinePointPairs = new ArrayList<>();

        private IntersectionInfo(Point2D intersectionPoint) {
            this.intersectionPoint = intersectionPoint;
        }
    }
}