use crate::geometry::{Contact, ContactManifold, CuboidFeatureFace, Triangle};
use crate::math::{Isometry, Point, Vector};
use crate::utils::WBasis;
use na::Point2;
use ncollide::shape::Segment;

#[derive(Debug)]
pub struct PolyhedronFace {
    pub vertices: [Point<f32>; 4],
    pub vids: [u8; 4], // Feature ID of the vertices.
    pub eids: [u8; 4], // Feature ID of the edges.
    pub fid: u8,       // Feature ID of the face.
    pub num_vertices: usize,
}

impl From<CuboidFeatureFace> for PolyhedronFace {
    fn from(face: CuboidFeatureFace) -> Self {
        Self {
            vertices: face.vertices,
            vids: face.vids,
            eids: face.eids,
            fid: face.fid,
            num_vertices: 4,
        }
    }
}

impl From<Triangle> for PolyhedronFace {
    fn from(tri: Triangle) -> Self {
        Self {
            vertices: [tri.a, tri.b, tri.c, tri.c],
            vids: [0, 2, 4, 4],
            eids: [1, 3, 5, 5],
            fid: 0,
            num_vertices: 3,
        }
    }
}

impl From<Segment<f32>> for PolyhedronFace {
    fn from(seg: Segment<f32>) -> Self {
        Self {
            vertices: [seg.a, seg.b, seg.b, seg.b],
            vids: [0, 2, 2, 2],
            eids: [1, 1, 1, 1],
            fid: 0,
            num_vertices: 2,
        }
    }
}

impl PolyhedronFace {
    pub fn transform_by(&mut self, iso: &Isometry<f32>) {
        for v in &mut self.vertices[0..self.num_vertices] {
            *v = iso * *v;
        }
    }

    pub fn contacts(
        prediction_distance: f32,
        face1: &PolyhedronFace,
        sep_axis1: &Vector<f32>,
        face2: &PolyhedronFace,
        pos21: &Isometry<f32>,
        manifold: &mut ContactManifold,
    ) {
        // Project the faces to a 2D plane for contact clipping.
        // The plane they are projected onto has normal sep_axis1
        // and contains the origin (this is numerically OK because
        // we are not working in world-space here).
        let basis = sep_axis1.orthonormal_basis();
        let projected_face1 = [
            Point2::new(
                face1.vertices[0].coords.dot(&basis[0]),
                face1.vertices[0].coords.dot(&basis[1]),
            ),
            Point2::new(
                face1.vertices[1].coords.dot(&basis[0]),
                face1.vertices[1].coords.dot(&basis[1]),
            ),
            Point2::new(
                face1.vertices[2].coords.dot(&basis[0]),
                face1.vertices[2].coords.dot(&basis[1]),
            ),
            Point2::new(
                face1.vertices[3].coords.dot(&basis[0]),
                face1.vertices[3].coords.dot(&basis[1]),
            ),
        ];
        let projected_face2 = [
            Point2::new(
                face2.vertices[0].coords.dot(&basis[0]),
                face2.vertices[0].coords.dot(&basis[1]),
            ),
            Point2::new(
                face2.vertices[1].coords.dot(&basis[0]),
                face2.vertices[1].coords.dot(&basis[1]),
            ),
            Point2::new(
                face2.vertices[2].coords.dot(&basis[0]),
                face2.vertices[2].coords.dot(&basis[1]),
            ),
            Point2::new(
                face2.vertices[3].coords.dot(&basis[0]),
                face2.vertices[3].coords.dot(&basis[1]),
            ),
        ];

        // Also find all the vertices located inside of the other projected face.
        if face2.num_vertices > 2 {
            let normal2 = (face2.vertices[2] - face2.vertices[1])
                .cross(&(face2.vertices[0] - face2.vertices[1]));

            let last_index2 = face2.num_vertices as usize - 1;
            'point_loop1: for i in 0..face1.num_vertices as usize {
                let p1 = projected_face1[i];

                let sign = (projected_face2[0] - projected_face2[last_index2])
                    .perp(&(p1 - projected_face2[last_index2]));
                for j in 0..last_index2 {
                    let new_sign = (projected_face2[j + 1] - projected_face2[j])
                        .perp(&(p1 - projected_face2[j]));
                    if new_sign * sign < 0.0 {
                        // The point lies outside.
                        continue 'point_loop1;
                    }
                }

                // All the perp had the same sign: the point is inside of the other shapes projection.
                // Output the contact.
                let denom = normal2.dot(&sep_axis1);
                let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom;
                let local_p1 = face1.vertices[i];
                let local_p2 = face1.vertices[i] + dist * sep_axis1;

                if dist <= prediction_distance {
                    manifold.points.push(Contact {
                        local_p1,
                        local_p2: pos21 * local_p2,
                        impulse: 0.0,
                        tangent_impulse: Contact::zero_tangent_impulse(),
                        fid1: face1.vids[i],
                        fid2: face2.fid,
                        dist,
                    });
                }
            }
        }

        if face1.num_vertices > 2 {
            let normal1 = (face1.vertices[2] - face1.vertices[1])
                .cross(&(face1.vertices[0] - face1.vertices[1]));

            let last_index1 = face1.num_vertices as usize - 1;
            'point_loop2: for i in 0..face2.num_vertices as usize {
                let p2 = projected_face2[i];

                let sign = (projected_face1[0] - projected_face1[last_index1])
                    .perp(&(p2 - projected_face1[last_index1]));
                for j in 0..last_index1 {
                    let new_sign = (projected_face1[j + 1] - projected_face1[j])
                        .perp(&(p2 - projected_face1[j]));

                    if new_sign * sign < 0.0 {
                        // The point lies outside.
                        continue 'point_loop2;
                    }
                }

                // All the perp had the same sign: the point is inside of the other shapes projection.
                // Output the contact.
                let denom = -normal1.dot(&sep_axis1);
                let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom;
                let local_p2 = face2.vertices[i];
                let local_p1 = face2.vertices[i] - dist * sep_axis1;

                if true {
                    // dist <= prediction_distance {
                    manifold.points.push(Contact {
                        local_p1,
                        local_p2: pos21 * local_p2,
                        impulse: 0.0,
                        tangent_impulse: Contact::zero_tangent_impulse(),
                        fid1: face1.fid,
                        fid2: face2.vids[i],
                        dist,
                    });
                }
            }
        }

        // Now we have to compute the intersection between all pairs of
        // edges from the face 1 and from the face2.
        for j in 0..face2.num_vertices {
            let projected_edge2 = [
                projected_face2[j],
                projected_face2[(j + 1) % face2.num_vertices],
            ];

            for i in 0..face1.num_vertices {
                let projected_edge1 = [
                    projected_face1[i],
                    projected_face1[(i + 1) % face1.num_vertices],
                ];
                if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
                    if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 {
                        // Found a contact between the two edges.
                        let edge1 = (
                            face1.vertices[i],
                            face1.vertices[(i + 1) % face1.num_vertices],
                        );
                        let edge2 = (
                            face2.vertices[j],
                            face2.vertices[(j + 1) % face2.num_vertices],
                        );
                        let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0;
                        let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1;
                        let dist = (local_p2 - local_p1).dot(&sep_axis1);

                        if dist <= prediction_distance {
                            manifold.points.push(Contact {
                                local_p1,
                                local_p2: pos21 * local_p2,
                                impulse: 0.0,
                                tangent_impulse: Contact::zero_tangent_impulse(),
                                fid1: face1.eids[i],
                                fid2: face2.eids[j],
                                dist,
                            });
                        }
                    }
                }
            }
        }
    }
}

/// Compute the barycentric coordinates of the intersection between the two given lines.
/// Returns `None` if the lines are parallel.
fn closest_points_line2d(edge1: [Point2<f32>; 2], edge2: [Point2<f32>; 2]) -> Option<(f32, f32)> {
    use approx::AbsDiffEq;

    // Inspired by Real-time collision detection by Christer Ericson.
    let dir1 = edge1[1] - edge1[0];
    let dir2 = edge2[1] - edge2[0];
    let r = edge1[0] - edge2[0];

    let a = dir1.norm_squared();
    let e = dir2.norm_squared();
    let f = dir2.dot(&r);

    let eps = f32::default_epsilon();

    if a <= eps && e <= eps {
        Some((0.0, 0.0))
    } else if a <= eps {
        Some((0.0, f / e))
    } else {
        let c = dir1.dot(&r);
        if e <= eps {
            Some((-c / a, 0.0))
        } else {
            let b = dir1.dot(&dir2);
            let ae = a * e;
            let bb = b * b;
            let denom = ae - bb;

            // Use absolute and ulps error to test collinearity.
            let parallel = denom <= eps || approx::ulps_eq!(ae, bb);

            let s = if !parallel {
                (b * f - c * e) / denom
            } else {
                0.0
            };

            if parallel {
                None
            } else {
                Some((s, (b * s + f) / e))
            }
        }
    }
}