use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::ContactManifold;
use crate::math::{
    AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
};
use crate::utils::{WAngularInertia, WCross, WDot};

pub(crate) struct PositionGroundConstraint {
    pub rb2: usize,
    // NOTE: the points are relative to the center of masses.
    pub p1: [Point<Real>; MAX_MANIFOLD_POINTS],
    pub local_p2: [Point<Real>; MAX_MANIFOLD_POINTS],
    pub dists: [Real; MAX_MANIFOLD_POINTS],
    pub n1: Vector<Real>,
    pub num_contacts: u8,
    pub im2: Real,
    pub ii2: AngularInertia<Real>,
    pub erp: Real,
    pub max_linear_correction: Real,
}

impl PositionGroundConstraint {
    pub fn generate(
        params: &IntegrationParameters,
        manifold: &ContactManifold,
        bodies: &RigidBodySet,
        out_constraints: &mut Vec<AnyPositionConstraint>,
        push: bool,
    ) {
        let mut rb1 = &bodies[manifold.data.body_pair.body1];
        let mut rb2 = &bodies[manifold.data.body_pair.body2];
        let flip = manifold.data.relative_dominance < 0;

        let n1 = if flip {
            std::mem::swap(&mut rb1, &mut rb2);
            -manifold.data.normal
        } else {
            manifold.data.normal
        };

        for (l, manifold_contacts) in manifold
            .data
            .solver_contacts
            .chunks(MAX_MANIFOLD_POINTS)
            .enumerate()
        {
            let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
            let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
            let mut dists = [0.0; MAX_MANIFOLD_POINTS];

            for k in 0..manifold_contacts.len() {
                p1[k] = manifold_contacts[k].point;
                local_p2[k] = rb2
                    .position
                    .inverse_transform_point(&manifold_contacts[k].point);
                dists[k] = manifold_contacts[k].dist;
            }

            let constraint = PositionGroundConstraint {
                rb2: rb2.active_set_offset,
                p1,
                local_p2,
                n1,
                dists,
                im2: rb2.effective_inv_mass,
                ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                num_contacts: manifold_contacts.len() as u8,
                erp: params.erp,
                max_linear_correction: params.max_linear_correction,
            };

            if push {
                out_constraints.push(AnyPositionConstraint::NonGroupedGround(constraint));
            } else {
                out_constraints[manifold.data.constraint_index + l] =
                    AnyPositionConstraint::NonGroupedGround(constraint);
            }
        }
    }

    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
        // FIXME: can we avoid most of the multiplications by pos1/pos2?
        // Compute jacobians.
        let mut pos2 = positions[self.rb2];
        let allowed_err = params.allowed_linear_error;

        for k in 0..self.num_contacts as usize {
            let target_dist = -self.dists[k] - allowed_err;
            let n1 = self.n1;
            let p1 = self.p1[k];
            let p2 = pos2 * self.local_p2[k];
            let dpos = p2 - p1;
            let dist = dpos.dot(&n1);

            if dist < target_dist {
                let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
                let dp2 = p2.coords - pos2.translation.vector;

                let gcross2 = -dp2.gcross(n1);
                let ii_gcross2 = self.ii2.transform_vector(gcross2);

                // Compute impulse.
                let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
                let impulse = err / inv_r;

                // Apply impulse.
                let tra2 = Translation::from(n1 * (-impulse * self.im2));
                let rot2 = Rotation::new(ii_gcross2 * impulse);
                pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
            }
        }

        positions[self.rb2] = pos2;
    }
}