import * as Utils from '../utils/utils'
import * as Intersection from '../algorithms/intersection';
import * as geom from '../classes'
import { PlanarSet } from '../data_structures/PlanarSet';
import { IntervalTree } from '../data_structures/interval-tree';

/**
 * Calculate distance and shortest segment between points
 * @param pt1
 * @param pt2
 * @returns {Number | Segment} - distance and shortest segment
 */
export function point2point(pt1, pt2): [number, geom.Segment] {
    return pt1.distanceTo(pt2);
}

/**
 * Calculate distance and shortest segment between point and line
 */
export function point2line(pt: geom.Point, line: geom.Line): [number, geom.Segment] {
    let closest_point = pt.projectionOn(line);
    let vec = new geom.Vector(pt, closest_point);
    return [vec.length, new geom.Segment(pt, closest_point)];
}

/**
 * Calculate distance and shortest segment between point and circle
 */
export function point2circle(pt: geom.Point, circle: geom.Circle): [number, geom.Segment] {
    let [dist2center, shortest_dist] = pt.distanceTo(circle.center);
    if (Utils.EQ_0(dist2center)) {
        return [circle.r, new geom.Segment(pt, circle.toArc().start)];
    } else {
        let dist = Math.abs(dist2center - circle.r);
        let v = new geom.Vector(circle.pc, pt).normalize().multiply(circle.r);
        let closest_point = circle.pc.translate(v);
        return [dist, new geom.Segment(pt, closest_point)];
    }
}

/**
 * Calculate distance and shortest segment between point and segment
 */
export function point2segment(pt: geom.Point, segment: geom.Segment): [number, geom.Segment] {
    /* Degenerated case of zero-length segment */
    if (segment.start.equalTo(segment.end)) {
        return point2point(pt, segment.start);
    }

    let v_seg = new geom.Vector(segment.start, segment.end);
    let v_ps2pt = new geom.Vector(segment.start, pt);
    let v_pe2pt = new geom.Vector(segment.end, pt);
    let start_sp = v_seg.dot(v_ps2pt);
    /* dot product v_seg * v_ps2pt */
    let end_sp = -v_seg.dot(v_pe2pt);
    /* minus dot product v_seg * v_pe2pt */

    let dist;
    let closest_point;
    if (Utils.GE(start_sp, 0) && Utils.GE(end_sp, 0)) {    /* point inside segment scope */
        let v_unit = segment.tangentInStart(); // new geom.Vector(v_seg.x / this.length, v_seg.y / this.length);
        /* unit vector ||v_unit|| = 1 */
        dist = Math.abs(v_unit.cross(v_ps2pt));
        /* dist = abs(v_unit x v_ps2pt) */
        closest_point = segment.start.translate(v_unit.multiply(v_unit.dot(v_ps2pt)));
        return [dist, new geom.Segment(pt, closest_point)];
    } else if (start_sp < 0) {                             /* point is out of scope closer to ps */
        return pt.distanceTo(segment.start);
    } else {                                               /* point is out of scope closer to pe */
        return pt.distanceTo(segment.end);
    }
};

/**
 * Calculate distance and shortest segment between point and arc
 */
export function point2arc(pt: geom.Point, arc: geom.Arc): [number, geom.Segment] {
    let circle = new geom.Circle(arc.pc, arc.r);
    let dist_and_segment = [];
    let dist, shortest_segment;
    [dist, shortest_segment] = point2circle(pt, circle);
    if (shortest_segment.end.on(arc)) {
        dist_and_segment.push(point2circle(pt, circle));
    }
    dist_and_segment.push(point2point(pt, arc.start));
    dist_and_segment.push(point2point(pt, arc.end));

    sort(dist_and_segment);

    return dist_and_segment[0];
}

/**
 * Calculate distance and shortest segment between segment and line
 */
export function segment2line(seg: geom.Segment, line: geom.Line): [number, geom.Segment] {
    let ip = seg.intersect(line);
    if (ip.length > 0) {
        return [0, new geom.Segment(ip[0], ip[0])];   // distance = 0, closest point is the first point
    }
    let dist_and_segment = [];
    dist_and_segment.push(point2line(seg.start, line));
    dist_and_segment.push(point2line(seg.end, line));

    sort(dist_and_segment);
    return dist_and_segment[0];

}

/**
 * Calculate distance and shortest segment between two segments
 * @param seg1
 * @param seg2
 * @returns {Number | Segment} - distance and shortest segment
 */
export function segment2segment(seg1, seg2): [number, geom.Segment] {
    let ip = Intersection.intersectSegment2Segment(seg1, seg2);
    if (ip.length > 0) {
        return [0, new geom.Segment(ip[0], ip[0])];   // distance = 0, closest point is the first point
    }

    // Seg1 and seg2 not intersected
    let dist_and_segment = [];
    let dist_tmp, shortest_segment_tmp
    [dist_tmp, shortest_segment_tmp] = point2segment(seg2.start, seg1);
    dist_and_segment.push([dist_tmp, shortest_segment_tmp.reverse()]);
    [dist_tmp, shortest_segment_tmp] = point2segment(seg2.end, seg1);
    dist_and_segment.push([dist_tmp, shortest_segment_tmp.reverse()]);
    dist_and_segment.push(point2segment(seg1.start, seg2));
    dist_and_segment.push(point2segment(seg1.end, seg2));

    sort(dist_and_segment);
    return dist_and_segment[0];
}

/**
 * Calculate distance and shortest segment between segment and circle
 * @param seg
 * @param circle
 * @returns {Number | Segment} - distance and shortest segment
 */
export function segment2circle(seg, circle): [number, geom.Segment] {
    /* Case 1 Segment and circle intersected. Return the first point and zero distance */
    let ip = seg.intersect(circle);
    if (ip.length > 0) {
        return [0, new geom.Segment(ip[0], ip[0])];
    }

    // No intersection between segment and circle

    /* Case 2. Distance to projection of center point to line bigger than radius
     * And projection point belong to segment
      * Then measure again distance from projection to circle and return it */
    let line = new geom.Line(seg.ps, seg.pe);
    let [dist, shortest_segment] = point2line(circle.center, line);
    if (Utils.GE(dist, circle.r) && shortest_segment.end.on(seg)) {
        return point2circle(shortest_segment.end, circle);
    }
    /* Case 3. Otherwise closest point is one of the end points of the segment */
    else {
        let [dist_from_start, shortest_segment_from_start] = point2circle(seg.start, circle);
        let [dist_from_end, shortest_segment_from_end] = point2circle(seg.end, circle);
        return Utils.LT(dist_from_start, dist_from_end) ?
            [dist_from_start, shortest_segment_from_start] :
            [dist_from_end, shortest_segment_from_end];
    }
}

/**
 * Calculate distance and shortest segment between segment and arc
 * @param seg
 * @param arc
 * @returns {Number | Segment} - distance and shortest segment
 */
export function segment2arc(seg, arc): [number, geom.Segment] {
    /* Case 1 Segment and arc intersected. Return the first point and zero distance */
    let ip = seg.intersect(arc);
    if (ip.length > 0) {
        return [0, new geom.Segment(ip[0], ip[0])];
    }

    // No intersection between segment and arc
    let line = new geom.Line(seg.ps, seg.pe);
    let circle = new geom.Circle(arc.pc, arc.r);

    /* Case 2. Distance to projection of center point to line bigger than radius AND
     * projection point belongs to segment AND
       * distance from projection point to circle belongs to arc  =>
       * return this distance from projection to circle */
    let [dist_from_center, shortest_segment_from_center] = point2line(circle.center, line);
    if (Utils.GE(dist_from_center, circle.r) && shortest_segment_from_center.end.on(seg)) {
        let [dist_from_projection, shortest_segment_from_projection] =
            point2circle(shortest_segment_from_center.end, circle);
        if (shortest_segment_from_projection.end.on(arc)) {
            return [dist_from_projection, shortest_segment_from_projection];
        }
    }
    /* Case 3. Otherwise closest point is one of the end points of the segment */
    let dist_and_segment = [];
    dist_and_segment.push(point2arc(seg.start, arc));
    dist_and_segment.push(point2arc(seg.end, arc));

    let dist_tmp, segment_tmp;
    [dist_tmp, segment_tmp] = point2segment(arc.start, seg);
    dist_and_segment.push([dist_tmp, segment_tmp.reverse()]);

    [dist_tmp, segment_tmp] = point2segment(arc.end, seg);
    dist_and_segment.push([dist_tmp, segment_tmp.reverse()]);

    sort(dist_and_segment);
    return dist_and_segment[0];
}

/**
 * Calculate distance and shortest segment between two circles
 * @param circle1
 * @param circle2
 * @returns {Number | Segment} - distance and shortest segment
 */
export function circle2circle(circle1, circle2): [number, geom.Segment] {
    let ip = circle1.intersect(circle2);
    if (ip.length > 0) {
        return [0, new geom.Segment(ip[0], ip[0])];
    }

    // Case 1. Concentric circles. Convert to arcs and take distance between two arc starts
    if (circle1.center.equalTo(circle2.center)) {
        let arc1 = circle1.toArc();
        let arc2 = circle2.toArc();
        return point2point(arc1.start, arc2.start);
    } else {
        // Case 2. Not concentric circles
        let line = new geom.Line(circle1.center, circle2.center);
        let ip1 = line.intersect(circle1);
        let ip2 = line.intersect(circle2);

        let dist_and_segment = [];

        dist_and_segment.push(point2point(ip1[0], ip2[0]));
        dist_and_segment.push(point2point(ip1[0], ip2[1]));
        dist_and_segment.push(point2point(ip1[1], ip2[0]));
        dist_and_segment.push(point2point(ip1[1], ip2[1]));

        sort(dist_and_segment);
        return dist_and_segment[0];
    }
}

/**
 * Calculate distance and shortest segment between two circles
 * @param circle
 * @param line
 * @returns {Number | Segment} - distance and shortest segment
 */
export function circle2line(circle, line): [number, geom.Segment] {
    let ip = circle.intersect(line);
    if (ip.length > 0) {
        return [0, new geom.Segment(ip[0], ip[0])];
    }

    let [dist_from_center, shortest_segment_from_center] = point2line(circle.center, line);
    let [dist, shortest_segment] = point2circle(shortest_segment_from_center.end, circle);
    shortest_segment = shortest_segment.reverse();
    return [dist, shortest_segment];
}

/**
 * Calculate distance and shortest segment between arc and line
 * @param arc
 * @param line
 * @returns {Number | Segment} - distance and shortest segment
 */
export function arc2line(arc, line): [number, geom.Segment] {
    /* Case 1 Line and arc intersected. Return the first point and zero distance */
    let ip = line.intersect(arc);
    if (ip.length > 0) {
        return [0, new geom.Segment(ip[0], ip[0])];
    }

    let circle = new geom.Circle(arc.center, arc.r);

    /* Case 2. Distance to projection of center point to line bigger than radius AND
     * projection point belongs to segment AND
       * distance from projection point to circle belongs to arc  =>
       * return this distance from projection to circle */
    let [dist_from_center, shortest_segment_from_center] = point2line(circle.center, line);
    if (Utils.GE(dist_from_center, circle.r)) {
        let [dist_from_projection, shortest_segment_from_projection] =
            point2circle(shortest_segment_from_center.end, circle);
        if (shortest_segment_from_projection.end.on(arc)) {
            return [dist_from_projection, shortest_segment_from_projection];
        }
    } else {
        let dist_and_segment = [];
        dist_and_segment.push(point2line(arc.start, line));
        dist_and_segment.push(point2line(arc.end, line));

        sort(dist_and_segment);
        return dist_and_segment[0];
    }
}

/**
 * Calculate distance and shortest segment between arc and circle
 * @param arc
 * @param circle2
 * @returns {Number | Segment} - distance and shortest segment
 */
export function arc2circle(arc, circle2): [number, geom.Segment] {
    let ip = arc.intersect(circle2);
    if (ip.length > 0) {
        return [0, new geom.Segment(ip[0], ip[0])];
    }

    let circle1 = new geom.Circle(arc.center, arc.r);

    let [dist, shortest_segment] = circle2circle(circle1, circle2);
    if (shortest_segment.start.on(arc)) {
        return [dist, shortest_segment];
    } else {
        let dist_and_segment = [];

        dist_and_segment.push(point2circle(arc.start, circle2));
        dist_and_segment.push(point2circle(arc.end, circle2));

        sort(dist_and_segment);

        return dist_and_segment[0];
    }
}

/**
 * Calculate distance and shortest segment between two arcs
 * @param arc1
 * @param arc2
 * @returns {Number | Segment} - distance and shortest segment
 */
export function arc2arc(arc1, arc2): [number, geom.Segment] {
    let ip = arc1.intersect(arc2);
    if (ip.length > 0) {
        return [0, new geom.Segment(ip[0], ip[0])];
    }

    let circle1 = new geom.Circle(arc1.center, arc1.r);
    let circle2 = new geom.Circle(arc2.center, arc2.r);

    let [dist, shortest_segment] = circle2circle(circle1, circle2);
    if (shortest_segment.start.on(arc1) && shortest_segment.end.on(arc2)) {
        return [dist, shortest_segment];
    } else {
        let dist_and_segment = [];

        let dist_tmp, segment_tmp;

        [dist_tmp, segment_tmp] = point2arc(arc1.start, arc2);
        if (segment_tmp.end.on(arc2)) {
            dist_and_segment.push([dist_tmp, segment_tmp]);
        }

        [dist_tmp, segment_tmp] = point2arc(arc1.end, arc2);
        if (segment_tmp.end.on(arc2)) {
            dist_and_segment.push([dist_tmp, segment_tmp]);
        }

        [dist_tmp, segment_tmp] = point2arc(arc2.start, arc1);
        if (segment_tmp.end.on(arc1)) {
            dist_and_segment.push([dist_tmp, segment_tmp.reverse()]);
        }

        [dist_tmp, segment_tmp] = point2arc(arc2.end, arc1);
        if (segment_tmp.end.on(arc1)) {
            dist_and_segment.push([dist_tmp, segment_tmp.reverse()]);
        }

        [dist_tmp, segment_tmp] = point2point(arc1.start, arc2.start);
        dist_and_segment.push([dist_tmp, segment_tmp]);

        [dist_tmp, segment_tmp] = point2point(arc1.start, arc2.end);
        dist_and_segment.push([dist_tmp, segment_tmp]);

        [dist_tmp, segment_tmp] = point2point(arc1.end, arc2.start);
        dist_and_segment.push([dist_tmp, segment_tmp]);

        [dist_tmp, segment_tmp] = point2point(arc1.end, arc2.end);
        dist_and_segment.push([dist_tmp, segment_tmp]);

        sort(dist_and_segment);

        return dist_and_segment[0];
    }
}

/**
 * Calculate distance and shortest segment between point and polygon
 * @param point
 * @param polygon
 * @returns {Number | Segment} - distance and shortest segment
 */
export function point2polygon(point, polygon): [number, geom.Segment] {
    let min_dist_and_segment = [Number.POSITIVE_INFINITY, new geom.Segment()] as [number, geom.Segment];
    for (let edge of polygon.edges) {
        let [dist, shortest_segment] = (edge.shape instanceof geom.Segment) ?
            point2segment(point, edge.shape) : point2arc(point, edge.shape);
        if (Utils.LT(dist, min_dist_and_segment[0])) {
            min_dist_and_segment = [dist, shortest_segment] as [number, geom.Segment];
        }
    }
    return min_dist_and_segment;
}

export function shape2polygon(shape, polygon): [number, geom.Segment] {
    let min_dist_and_segment = [Number.POSITIVE_INFINITY, new geom.Segment()] as [number, geom.Segment];
    for (let edge of polygon.edges) {
        let [dist, shortest_segment] = shape.distanceTo(edge.shape);
        if (Utils.LT(dist, min_dist_and_segment[0])) {
            min_dist_and_segment = [dist, shortest_segment] as [number, geom.Segment];
        }
    }
    return min_dist_and_segment;
}

/**
 * Calculate distance and shortest segment between two polygons
 * @param polygon1
 * @param polygon2
 * @returns {Number | Segment} - distance and shortest segment
 */
export function polygon2polygon(polygon1, polygon2): [number, geom.Segment] {
    let min_dist_and_segment = [Number.POSITIVE_INFINITY, new geom.Segment()] as [number, geom.Segment];
    for (let edge1 of polygon1.edges) {
        for (let edge2 of polygon2.edges) {
            let [dist, shortest_segment] = edge1.shape.distanceTo(edge2.shape);
            if (Utils.LT(dist, min_dist_and_segment[0])) {
                min_dist_and_segment = [dist, shortest_segment] as [number, geom.Segment];
            }
        }
    }
    return min_dist_and_segment;
}

/**
 * Returns [mindist, maxdist] array of squared minimal and maximal distance between boxes
 * Minimal distance by x is
 *    (box2.xmin - box1.xmax), if box1 is left to box2
 *    (box1.xmin - box2.xmax), if box2 is left to box1
 *    0,                       if box1 and box2 are intersected by x
 * Minimal distance by y is defined in the same way
 *
 * Maximal distance is estimated as a sum of squared dimensions of the merged box
 *
 * @param box1
 * @param box2
 */
export function box2box_minmax(box1, box2) {
    let mindist_x = Math.max(Math.max(box1.xmin - box2.xmax, 0), Math.max(box2.xmin - box1.xmax, 0));
    let mindist_y = Math.max(Math.max(box1.ymin - box2.ymax, 0), Math.max(box2.ymin - box1.ymax, 0));
    let mindist = mindist_x * mindist_x + mindist_y * mindist_y;

    let box = box1.merge(box2);
    let dx = box.xmax - box.xmin;
    let dy = box.ymax - box.ymin;
    let maxdist = dx * dx + dy * dy;

    return [mindist, maxdist] as const;
}

export function minmax_tree_process_level(shape, level, min_stop, tree) {
    // Calculate minmax distance to each shape in current level
    // Insert result into the interval tree for further processing
    // update min_stop with maxdist, it will be the new stop distance
    let mindist, maxdist;
    for (let node of level) {

        // [mindist, maxdist] = box2box_minmax(shape.box, node.max);
        // if (Utils.GT(mindist, min_stop))
        //     continue;

        // Estimate min-max dist to the shape stored in the node.item, using node.item.key which is shape's box
        [mindist, maxdist] = box2box_minmax(shape.box, node.item.key);
        if (node.item.value instanceof geom.Edge) {
            tree.insert([mindist, maxdist], node.item.value.shape);
        } else {
            tree.insert([mindist, maxdist], node.item.value);
        }
        if (Utils.LT(maxdist, min_stop)) {
            min_stop = maxdist;                       // this will be the new distance estimation
        }
    }

    if (level.length === 0)
        return min_stop;

    // Calculate new level from left and right children of the current
    let new_level_left = level.map(node => node.left.isNil() ? undefined : node.left).filter(node => node !== undefined);
    let new_level_right = level.map(node => node.right.isNil() ? undefined : node.right).filter(node => node !== undefined);
    // Merge left and right subtrees and leave only relevant subtrees
    let new_level = [...new_level_left, ...new_level_right].filter(node => {
        // Node subtree quick reject, node.max is a subtree box
        let [mindist, maxdist] = box2box_minmax(shape.box, node.max);
        return (Utils.LE(mindist, min_stop));
    });

    min_stop = minmax_tree_process_level(shape, new_level, min_stop, tree);
    return min_stop;
}

/**
 * Calculates sorted tree of [mindist, maxdist] intervals between query shape
 * and shapes of the planar set.
 * @param shape
 * @param set
 */
export function minmax_tree(shape, set, min_stop) {
    let tree = new IntervalTree();
    let level = [set.index.root];
    let squared_min_stop = min_stop < Number.POSITIVE_INFINITY ? min_stop * min_stop : Number.POSITIVE_INFINITY;
    squared_min_stop = minmax_tree_process_level(shape, level, squared_min_stop, tree);
    return tree;
}

export function minmax_tree_calc_distance(shape, node, min_dist_and_segment) {
    let min_dist_and_segment_new, stop;
    if (node != null && !node.isNil()) {
        [min_dist_and_segment_new, stop] = minmax_tree_calc_distance(shape, node.left, min_dist_and_segment);

        if (stop) {
            return [min_dist_and_segment_new, stop];
        }

        if (Utils.LT(min_dist_and_segment_new[0], Math.sqrt(node.item.key.low))) {
            return [min_dist_and_segment_new, true];   // stop condition
        }

        let [dist, shortest_segment] = distance(shape, node.item.value);
        // console.log(dist)
        if (Utils.LT(dist, min_dist_and_segment_new[0])) {
            min_dist_and_segment_new = [dist, shortest_segment];
        }

        [min_dist_and_segment_new, stop] = minmax_tree_calc_distance(shape, node.right, min_dist_and_segment_new);

        return [min_dist_and_segment_new, stop];
    }

    return [min_dist_and_segment, false];
}

/**
 * Calculates distance between shape and Planar Set of shapes
 * @param shape
 * @param {PlanarSet} set
 * @param {Number} min_stop
 */
export function shape2planarSet(shape, set, min_stop = Number.POSITIVE_INFINITY) {
    let min_dist_and_segment = [min_stop, new geom.Segment()] as const;
    let stop = false;
    if (set instanceof PlanarSet) {
        let tree = minmax_tree(shape, set, min_stop);
        [min_dist_and_segment, stop] = minmax_tree_calc_distance(shape, tree.root, min_dist_and_segment);
    }
    return min_dist_and_segment;
}

export function sort(dist_and_segment) {
    dist_and_segment.sort((d1, d2) => {
        if (Utils.LT(d1[0], d2[0])) {
            return -1;
        }
        if (Utils.GT(d1[0], d2[0])) {
            return 1;
        }
        return 0;
    });
}

export function distance(shape1, shape2) {
    return shape1.distanceTo(shape2);
}

export function pythagore(width: number, height: number) {
    return Math.sqrt(width * width + height * height)
}