/*
 * Copyright 2012, 2013 Hannes Janetzek
 * Copyright 2018-2019 Gustl22
 *
 * This file is part of the OpenScienceMap project (http://www.opensciencemap.org).
 *
 * This program is free software: you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 * PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License along with
 * this program. If not, see <http://www.gnu.org/licenses/>.
 */
package org.oscim.utils.geom;

import java.util.ArrayList;
import java.util.List;

// TODO Utils can be improved e.g. by avoiding object creations
public final class GeometryUtils {

    private GeometryUtils() {
    }

    /**
     * Test if point x/y is in polygon defined by vertices[offset ...
     * offset+length]
     * <p/>
     * -- from www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
     * <p/>
     * If there is only one connected component, then it is optional to repeat
     * the first vertex at the end. It's also optional to surround the component
     * with zero vertices.
     * <p/>
     * The polygon may contain multiple separate components, and/or holes,
     * provided that you separate the components and holes with a (0,0) vertex,
     * as follows.
     * <li>First, include a (0,0) vertex.
     * <li>Then include the first component' vertices, repeating its first
     * vertex after the last vertex.
     * <li>Include another (0,0) vertex.
     * <li>Include another component or hole, repeating its first vertex after
     * the last vertex.
     * <li>Repeat the above two steps for each component and hole.
     * <li>Include a final (0,0) vertex.
     * <p/>
     * Each component or hole's vertices may be listed either clockwise or
     * counter-clockwise.
     */
    public static boolean pointInPoly(float x, float y, float[] vertices,
                                      int length, int offset) {

        int end = (offset + length);

        boolean inside = false;
        for (int i = offset, j = (end - 2); i < end; j = i, i += 2) {
            if (((vertices[i + 1] > y) != (vertices[j + 1] > y)) &&
                    (x < (vertices[j] - vertices[i]) * (y - vertices[i + 1])
                            / (vertices[j + 1] - vertices[i + 1]) + vertices[i]))
                inside = !inside;
        }
        return inside;
    }

    /**
     * Returns the unsigned area of polygon.
     *
     * @param size the number of point coordinates
     * @return unsigned polygon area
     */
    public static float area(float[] points, int size) {
        float area = isClockwise(points, size);
        return area < 0 ? -area : area;
    }

    public static float area(float ax, float ay, float bx, float by, float cx, float cy) {

        float area = ((ax - cx) * (by - cy)
                - (bx - cx) * (ay - cy));

        return (area < 0 ? -area : area) * 0.5f;
    }

    public static float area(float[] a, int p1, int p2, int p3) {

        float area = ((a[p1] - a[p3]) * (a[p2 + 1] - a[p3 + 1])
                - (a[p2] - a[p3]) * (a[p1 + 1] - a[p3 + 1]));

        return (area < 0 ? -area : area) * 0.5f;
    }

    /**
     * @param v1 the first normalized vector
     * @param v2 the second normalized vector
     * @return the bisection of the to vectors
     */
    public static float[] bisectionNorm2D(float[] v1, float[] v2) {
        // Normalize vectors
        /*float absBA = (float) Math.sqrt(v1[0] * v1[0] + v1[1] * v1[1]);
        float absBC = (float) Math.sqrt(v2[0] * v2[0] + v2[1] * v2[1]);
        v1[0] /= absBA;
        v1[1] /= absBA;
        v2[0] /= absBC;
        v2[1] /= absBC;*/

        float[] bisection = new float[2];
        bisection[0] = v1[0] + v2[0];
        bisection[1] = v1[1] + v2[1];
        if (bisection[0] == 0 && bisection[1] == 0) {
            // 90 degree to v1
            bisection[0] = v1[1];
            bisection[1] = -v1[0];
        }
        return bisection;
    }

    /**
     * Calculates the center of a set of points.
     *
     * @param points   the points array
     * @param pointPos the start position of points
     * @param n        the number of points
     * @param out      the center output
     * @return the center of points
     */
    public static float[] center(float[] points, int pointPos, int n, float[] out) {
        if (out == null)
            out = new float[2];

        for (int i = 0; i < n; i += 2, pointPos += 2) {
            out[0] += points[pointPos];
            out[1] += points[pointPos + 1];
        }
        out[0] = out[0] * 2 / n;
        out[1] = out[1] * 2 / n;

        return out;
    }

    /**
     * Calculate the closest point on a line.
     * See: https://en.wikipedia.org/wiki/Vector_projection#Vector_projection_2
     *
     * @param pP point
     * @param pL point of line
     * @param vL vector of line
     * @return the closest point on line
     */
    public static float[] closestPointOnLine2D(float[] pP, float[] pL, float[] vL) {
        float[] vPL = diffVec(pL, pP);
        float[] vPS = diffVec(vPL, scale(vL, dotProduct(vPL, vL) / dotProduct(vL, vL)));
        return sumVec(pP, vPS);
    }

    /**
     * @param a first vector
     * @param b second vector
     * @return a - b
     */
    public static float[] diffVec(float[] a, float[] b) {
        float[] diff = new float[Math.min(a.length, b.length)];
        for (int i = 0; i < diff.length; i++) {
            diff[i] = a[i] - b[i];
        }
        return diff;
    }

    /**
     * @param a first vector
     * @param b second vector
     * @return a + b
     */
    public static float[] sumVec(float[] a, float[] b) {
        float[] add = new float[Math.min(a.length, b.length)];
        for (int i = 0; i < add.length; i++) {
            add[i] = b[i] + a[i];
        }
        return add;
    }

    public static float squaredDistance(float[] p, int a, int b) {
        return (p[a] - p[b]) * (p[a] - p[b]) + (p[a + 1] - p[b + 1]) * (p[a + 1] - p[b + 1]);
    }

    /**
     * square distance from a point a to a segment b,c
     */
    // modified from https://github.com/ekeneijeoma/simplify-java
    public static float squareSegmentDistance(float[] p, int a, int b, int c) {
        float x = p[b];
        float y = p[b + 1];

        float dx = p[c] - x;
        float dy = p[c + 1] - y;

        if (dx != 0 || dy != 0) {
            float t = ((p[a] - x) * dx + (p[a + 1] - y) * dy) / (dx * dx + dy * dy);

            if (t > 1) {
                x = p[c];
                y = p[c + 1];
            } else if (t > 0) {
                x += dx * t;
                y += dy * t;
            }
        }

        dx = p[a] - x;
        dy = p[a + 1] - y;

        return dx * dx + dy * dy;
    }

    public static double distance(float[] p, int a, int b) {
        float dx = p[a] - p[b];
        float dy = p[a + 1] - p[b + 1];
        return Math.sqrt(dx * dx + dy * dy);
    }

    /**
     * @param a point a[x,y]
     * @param b point b[x,y]
     * @return the distance between a and b.
     */
    public static double distance2D(float[] a, float[] b) {
        float dx = a[0] - b[0];
        float dy = a[1] - b[1];
        return Math.sqrt(dx * dx + dy * dy);
    }

    /**
     * Calculate the distance from a point to a line.
     * See: https://en.wikipedia.org/wiki/Vector_projection#Vector_projection_2
     *
     * @param pP point
     * @param pL point of line
     * @param vL vector of line
     * @return the minimum distance between line and point
     */
    public static float distancePointLine2D(float[] pP, float[] pL, float[] vL) {
        float[] vPL = diffVec(pL, pP);
        float[] vPS = diffVec(vPL, scale(vL, dotProduct(vPL, vL) / dotProduct(vL, vL)));
        return (float) Math.sqrt(dotProduct(vPS, vPS));
    }

    public static double dotProduct(float[] p, int a, int b, int c) {

        double ux = (p[b] - p[a]);
        double uy = (p[b + 1] - p[a + 1]);
        double ab = Math.sqrt(ux * ux + uy * uy);
        double vx = (p[b] - p[c]);
        double vy = (p[b + 1] - p[c + 1]);
        double bc = Math.sqrt(vx * vx + vy * vy);

        double d = ab * bc;

        if (d <= 0)
            return 0;

        double dotp = (ux * -vx + uy * -vy) / d;

        if (dotp > 1)
            dotp = 1;
        else if (dotp < -1)
            dotp = -1;

        return dotp;
    }

    /**
     * @param a first vector
     * @param b second vector
     * @return the dot product
     */
    public static float dotProduct(float[] a, float[] b) {
        float dp = 0;
        for (int i = 0; i < a.length; i++) {
            dp += a[i] * b[i];
        }
        return dp;
    }

    /**
     * @param pA position vector of A
     * @param vA direction vector of A
     * @param pB position vector of B
     * @param vB direction vector of B
     * @return the intersection point
     */
    public static float[] intersectionLines2D(float[] pA, float[] vA, float[] pB, float[] vB) {
        // pA + ldA * vA == pB + ldB * vB;

        float det = vB[0] * vA[1] - vB[1] * vA[0];
        if (det == 0) {
            // log.debug(vA.toString() + "and" + vB.toString() + "do not intersect");
            return null;
        }
        float lambA = ((pB[1] - pA[1]) * vB[0] - (pB[0] - pA[0]) * vB[1]) / det;

        float[] intersection = new float[2];
        intersection[0] = pA[0] + lambA * vA[0];
        intersection[1] = pA[1] + lambA * vA[1];

        return intersection;
    }

    /**
     * Calculate intersection of a plane with a line
     *
     * @param pL position vector of line
     * @param vL direction vector of line
     * @param pP position vector of plane
     * @param vP normal vector of plane
     * @return the intersection point
     */
    public static float[] intersectionLinePlane(float[] pL, float[] vL, float[] pP, float[] vP) {
        float det = dotProduct(vL, vP);
        if (det == 0) return null;
        float phi = dotProduct(diffVec(pP, pL), vP) / det;
        return sumVec(scale(vL, phi), pL);
    }

    /**
     * Is polygon clockwise.
     * Note that the coordinate system is left hand side.
     *
     * @param points the points array
     * @param size   the number of point coordinates
     * @return the signed area of the polygon
     * positive: clockwise
     * negative: counter-clockwise
     * 0: collinear
     */
    public static float isClockwise(float[] points, int size) {
        float area = 0f;
        for (int i = 0; i < size - 2; i += 2) {
            area += (points[i] * points[i + 3]) - (points[i + 1] * points[i + 2]);
        }
        area += (points[size - 2] * points[1]) - (points[size - 1] * points[0]);

        return 0.5f * area;
    }

    /**
     * Indicates the turn of tris pA-pB-pC.
     * Note that the coordinate system is left hand side.
     *
     * @return positive: clockwise
     * negative: counter-clockwise
     * 0: collinear
     */
    public static float isTrisClockwise(float[] pA, float[] pB, float[] pC) {
        return (pB[0] - pA[0]) * (pC[1] - pA[1]) - (pB[1] - pA[1]) * (pC[0] - pA[0]);
    }

    /**
     * @return the length of the vector
     */
    public static double length(float[] vec) {
        float length = 0f;
        for (float coord : vec) {
            length += coord * coord;
        }
        return Math.sqrt(length);
    }

    /**
     * @return the normalized vector (with length 1)
     */
    public static float[] normalize(float[] vec) {
        return scale(vec, 1f / (float) length(vec));
    }

    /**
     * Calculate the normalized direction vectors of point list (polygon)
     *
     * @param points     the list of 2D points
     * @param outLengths the optional list to store lengths of vectors
     * @return the normalized direction vectors
     */
    public static List<float[]> normalizedVectors2D(List<float[]> points, List<Float> outLengths) {
        List<float[]> normVectors = new ArrayList<>();

        for (int i = 0; i < points.size(); i++) {
            float[] pA = points.get(i);
            float[] pB = points.get((i + 1) % points.size());

            float[] vBA = diffVec(pB, pA);

            // Get length of AB
            float length = (float) Math.sqrt(vBA[0] * vBA[0] + vBA[1] * vBA[1]);
            if (outLengths != null)
                outLengths.add(length);

            vBA[0] /= length; // Normalize vector
            vBA[1] /= length;

            normVectors.add(vBA);
        }
        return normVectors;
    }

    /**
     * @param pA first point of plane
     * @param pB second point of plane
     * @param pC third point of plane
     * @return the normal of plane
     */
    public static float[] normalOfPlane(float[] pA, float[] pB, float[] pC) {
        // Calculate normal for color gradient
        float[] BA = diffVec(pB, pA);
        float[] BC = diffVec(pC, pA);

        // Vector product (c is at right angle to a and b)
        float[] normal = new float[3];
        normal[0] = BA[1] * BC[2] - BA[2] * BC[1];
        normal[1] = BA[2] * BC[0] - BA[0] * BC[2];
        normal[2] = BA[0] * BC[1] - BA[1] * BC[0];
        return normal;
    }

    /**
     * @param v     the vector
     * @param scale the scale
     * @return the scaled vector
     */
    public static float[] scale(float[] v, float scale) {
        float[] scaled = new float[v.length];
        for (int i = 0; i < v.length; i++) {
            scaled[i] = v[i] * scale;
        }
        return scaled;
    }
}
