#include <iostream>

#include "catch.hpp"
#include "rbdlsim.h"

using namespace std;

using namespace RBDLSim;

TEST_CASE("Simple Box vs Sphere Collision", "[Collision]") {
  SimShape box;
  box.mType = SimShape::Box;
  box.pos.set(0.0, 0.5, 0.);
  box.scale.set(1., 1., 1.);
  box.orientation.set(0., 0., 0., 1.);

  SimShape sphere;
  sphere.mType = SimShape::Sphere;

  sphere.scale.set(0.5, 0.5, 0.5);
  sphere.orientation.set(0., 0., 0., 1.);

  bool cresult = false;
  CollisionInfo cinfo;

  SECTION("Box and Sphere Touching") {
    sphere.pos.set(0., 1.5, 0.);
    cresult = CheckPenetration(box, sphere, cinfo);

    REQUIRE(cresult == true);
  }

  SECTION("Box and Sphere Intersecting") {
    sphere.pos.set(0., 1.4, 0.);
    cresult = CheckPenetration(box, sphere, cinfo);

    REQUIRE(cresult == true);
  }

  SECTION("Box and Sphere Separated") {
    sphere.pos.set(0., 1.5001, 0.);
    cresult = CheckPenetration(box, sphere, cinfo);

    REQUIRE(cresult == false);
  }
}

TEST_CASE("CheckCollisionSphereVsPlane", "[Collision]") {
  SimShape plane;
  plane.mType = SimShape::Plane;
  plane.pos = Vector3d(0., 0., 0.);
  plane.orientation = Quaternion(0., 1., 0., 1.);
  plane.scale = Vector3d(1., 1., 1.);

  SimShape sphere;
  sphere.mType = SimShape::Sphere;

  sphere.scale = Vector3d(1.5, 1.5, 1.5);
  sphere.orientation = Quaternion(0., 0., 0., 1.);

  CollisionInfo cinfo;
  bool cresult = false;

  SECTION("Sphere above plane") {
    sphere.pos = Vector3d(0., 2.0, 0.);
    cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);

    REQUIRE(cresult == false);
  }

  SECTION("Sphere touching") {
    sphere.pos = Vector3d(0., 0.75, 0.);
    cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
    REQUIRE((cinfo.posA - Vector3d(0., 0.0, 0.)).norm() < 1.0e-12);
    REQUIRE((cinfo.posB - Vector3d(0., 0.0, 0.)).norm() < 1.0e-12);

    REQUIRE(cresult == true);
  }

  SECTION("Sphere penetration") {
    sphere.pos = Vector3d(1., -1., 0.);
    cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
    REQUIRE((cinfo.posA - Vector3d(1., -1.75, 0.)).norm() < 1.0e-12);
    REQUIRE((cinfo.posB - Vector3d(1., 0.0, 0.)).norm() < 1.0e-12);

    REQUIRE(cresult == true);
  }

  SECTION("Sphere touching shifted") {
    sphere.pos = Vector3d(3., 0.75, 0.);
    cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);
    REQUIRE((cinfo.posA - Vector3d(3., 0.0, 0.)).norm() < 1.0e-12);
    REQUIRE((cinfo.posB - Vector3d(3., 0.0, 0.)).norm() < 1.0e-12);

    REQUIRE(cresult == true);
  }

}

TEST_CASE("CheckCollisionSphereVsSphere", "[Collision]") {
  SimShape sphere_a;
  sphere_a.mType = SimShape::Sphere;
  sphere_a.scale = Vector3d(1.4, 1.4, 1.4);
  sphere_a.orientation = Quaternion(0., 0., 0., 1.);

  SimShape sphere_b;
  sphere_b.mType = SimShape::Sphere;
  sphere_b.scale = Vector3d(1.6, 1.6, 1.6);
  sphere_b.orientation = Quaternion(0., 0., 0., 1.);

  CollisionInfo cinfo;
  bool cresult = false;

  SECTION("Spheres non-overlapping") {
    sphere_a.pos = Vector3d(0., 4.0, 0.);
    sphere_b.pos = Vector3d(0., 0.0, 0.);
    cresult = CheckPenetrationSphereVsSphere(sphere_a, sphere_b, cinfo);

    REQUIRE(cresult == false);
  }

  SECTION("Spheres touching") {
    sphere_a.pos = Vector3d(0., 1.5, 0.);
    sphere_b.pos = Vector3d(0., 0.0, 0.);
    cresult = CheckPenetrationSphereVsSphere(sphere_a, sphere_b, cinfo);

    REQUIRE(cresult == true);
  }

  SECTION("Spheres overlapping") {
    sphere_a.pos = Vector3d(0., 1.0, 0.);
    sphere_b.pos = Vector3d(0., 0.0, 0.);
    cresult = CheckPenetrationSphereVsSphere(sphere_a, sphere_b, cinfo);

    REQUIRE(cresult == true);
    double err_pos_A = (cinfo.posA - Vector3d(0., 0.3, 0.)).norm();
    REQUIRE_THAT(
        (cinfo.dir - Vector3d(0., -1., 0.)).norm(),
        Catch::WithinRel(0., 1.0e-12));
    REQUIRE((cinfo.posA - Vector3d(0., 0.3, 0.)).norm() < 1.0e-12);
    REQUIRE((cinfo.posB - Vector3d(0., 0.8, 0.)).norm() < 1.0e-12);
    REQUIRE_THAT(cinfo.depth, Catch::WithinRel(0.5, 1.0e-12));
  }

  SECTION("Spheres overlapping reversed") {
    sphere_a.pos = Vector3d(0., 1.0, 0.);
    sphere_b.pos = Vector3d(0., 0.0, 0.);
    cresult = CheckPenetrationSphereVsSphere(sphere_b, sphere_a, cinfo);

    REQUIRE(cresult == true);
    REQUIRE_THAT(
        (cinfo.dir - Vector3d(0., 1., 0.)).norm(),
        Catch::WithinRel(0., 1.0e-12));
    REQUIRE((cinfo.posA - Vector3d(0., 0.8, 0.)).norm() < 1.0e-12);
    REQUIRE((cinfo.posB - Vector3d(0., 0.3, 0.)).norm() < 1.0e-12);
    REQUIRE_THAT(cinfo.depth, Catch::WithinRel(0.5, 1.0e-12));
  }
}

TEST_CASE("CalcConstraintImpulse", "[Collision]") {
  SimBody ground_body;
  SimShape ground_shape;
  ground_shape.mType = SimShape::Plane;
  ground_shape.pos = Vector3d::Zero();
  ground_shape.orientation = Quaternion(0., 0., 0., 1.);
  ground_shape.restitution = 1.0;
  ground_body.mCollisionShapes.push_back(
      SimBody::BodyCollisionInfo(-1, ground_shape));
  ground_body.mIsStatic = true;

  double sphere_a_mass = 1.5;
  double sphere_b_mass = 1.5;

  SimBody sphere_a_body = CreateSphereBody(
      sphere_a_mass,
      1.0,
      0.,
      Vector3d(0., 0.5, 0.),
      Vector3d(0., -1., 0.));
  SimBody sphere_b_body = CreateSphereBody(
      sphere_b_mass,
      1.0,
      0.,
      Vector3d(0., 0.5, 0.),
      Vector3d(0., -1., 0.));

  CollisionInfo cinfo;

  SECTION("SphereOnGroundButColliding") {
    sphere_a_body.q[1] = 0.5;
    sphere_a_body.qdot[1] = -1.23;

    sphere_a_body.updateCollisionShapes();
    std::vector<CollisionInfo> collisions;
    CalcCollisions(ground_body, sphere_a_body, collisions);
    REQUIRE(collisions.size() == 1);
    cinfo = collisions[0];

    bool cresult = CheckPenetration(
        ground_shape,
        sphere_a_body.mCollisionShapes[0].second,
        cinfo);
    REQUIRE(cresult == true);
    REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);

    PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);

    SECTION("EnsureImpulseDirection") {
      CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
      REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3);
      REQUIRE(cinfo.deltaImpulse > -1.0e-12);
    }

    SECTION("CheckForceAndJacobianDirections") {
      REQUIRE(cinfo.jacB * sphere_a_body.qdot == -1.23);
      cinfo.deltaImpulse = 1.0;
      VectorNd qdot_old = sphere_a_body.qdot;
      ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
      REQUIRE(sphere_a_body.qdot[1] > qdot_old[1]);
    }

    SECTION("CalculateImpulse") {
      CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
      double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1];
      REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12);
      ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
      REQUIRE(sphere_a_body.qdot[1] > -1.0e-12);
    }

    SECTION("ImpulseMustNotPull") {
      sphere_a_body.qdot[1] = 1.23;
      CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
      REQUIRE(fabs(cinfo.accumImpulse) < 1.0e-12);
    }

    SECTION("CheckBounce") {
      cinfo.effectiveRestitution = 1.0;
      PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
      VectorNd old_vel = sphere_a_body.qdot;
      CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
      ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
      REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel[1]) < 1.0e-12);
    }
  }

  SECTION("SphereOnGroundButCollidingReverseBodyOrder") {
    sphere_a_body.q[1] = 0.5;
    sphere_a_body.qdot[1] = -1.23;

    sphere_a_body.updateCollisionShapes();
    std::vector<CollisionInfo> collisions;
    CalcCollisions(sphere_a_body, ground_body, collisions);
    REQUIRE(collisions.size() == 1);
    cinfo = collisions[0];

    bool cresult = CheckPenetration(
        sphere_a_body.mCollisionShapes[0].second,
        ground_shape,
        cinfo);
    REQUIRE(cresult == true);
    REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);

    PrepareConstraintImpulse(&sphere_a_body, &ground_body, cinfo);

    SECTION("EnsureImpulseDirection") {
      CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
      REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3);
      REQUIRE(cinfo.deltaImpulse > -1.0e-12);
    }

    SECTION("CheckForceAndJacobianDirections") {
      REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
      cinfo.deltaImpulse = 1.0;
      VectorNd qdot_old = sphere_a_body.qdot;
      ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
      REQUIRE(sphere_a_body.qdot[1] > qdot_old[1]);
    }

    SECTION("CalculateImpulse") {
      CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
      double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1];
      REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12);
      ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
      REQUIRE(sphere_a_body.qdot[1] > -1.0e-12);
    }
  }

  SECTION("SphereVsSphereCollision") {
    SimBody sphere_b_body = CreateSphereBody(
        sphere_b_mass,
        1.0,
        0.,
        Vector3d(0., -0.5, 0.),
        Vector3d(0., 1., 0.));

    sphere_a_body.q[1] = 0.5;
    sphere_a_body.qdot[1] = -1.23;
    sphere_a_body.updateCollisionShapes();

    sphere_b_body.q[1] = -0.5;
    sphere_b_body.qdot[1] = 1.23;
    sphere_b_body.updateCollisionShapes();

    std::vector<CollisionInfo> collisions;
    CalcCollisions(sphere_a_body, sphere_b_body, collisions);
    REQUIRE(collisions.size() == 1);
    cinfo = collisions[0];

    REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12);

    PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);

    SECTION("CheckForceAndJacobianDirections") {
      REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
      cinfo.deltaImpulse = -1.0;
      VectorNd qdot_a_old = sphere_a_body.qdot;
      VectorNd qdot_b_old = sphere_b_body.qdot;
      ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
      REQUIRE(sphere_a_body.qdot[1] < qdot_a_old[1]);
      REQUIRE(sphere_b_body.qdot[1] > qdot_b_old[1]);
    }

    SECTION("CalculateImpulse") {
      CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
      ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
      REQUIRE(sphere_a_body.qdot[1] > -0.1);
      REQUIRE(sphere_b_body.qdot[1] < 0.1);
    }

    SECTION("CheckBounce") {
      cinfo.effectiveRestitution = 1.0;
      PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
      VectorNd old_vel_a = sphere_a_body.qdot;
      VectorNd old_vel_b = sphere_b_body.qdot;
      CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
      ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
      REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel_a[1]) < 1.0e-12);
      REQUIRE(fabs(sphere_b_body.qdot[1] + old_vel_b[1]) < 1.0e-12);
    }
  }

  SECTION("SphereVsSphereCollisionReversed") {
    SimBody sphere_b_body = CreateSphereBody(
        sphere_b_mass,
        1.0,
        0.,
        Vector3d(0., -0.5, 0.),
        Vector3d(0., 1., 0.));

    sphere_a_body.q[1] = -0.5;
    sphere_a_body.qdot[1] = 1.23;
    sphere_a_body.updateCollisionShapes();

    sphere_b_body.q[1] = 0.5;
    sphere_b_body.qdot[1] = -1.23;
    sphere_b_body.updateCollisionShapes();

    std::vector<CollisionInfo> collisions;
    CalcCollisions(sphere_a_body, sphere_b_body, collisions);
    REQUIRE(collisions.size() == 1);
    cinfo = collisions[0];

    REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12);

    PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);

    SECTION("CheckForceAndJacobianDirections") {
      REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23);
      cinfo.deltaImpulse = -1.0;
      VectorNd qdot_a_old = sphere_a_body.qdot;
      VectorNd qdot_b_old = sphere_b_body.qdot;
      ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
      REQUIRE(sphere_a_body.qdot[1] > qdot_a_old[1]);
      REQUIRE(sphere_b_body.qdot[1] < qdot_b_old[1]);
    }

    SECTION("CalculateImpulse") {
      CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
      ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
      REQUIRE(sphere_a_body.qdot[1] < 0.1);
      REQUIRE(sphere_b_body.qdot[1] > -0.1);
    }
  }
}