#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);
  }
}

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_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);
    CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0);
    double reference_impulseB = -sphere_a_mass * sphere_a_body.qdot[1];
    REQUIRE(cinfo.accumImpulseA < 1.0e-12);
    REQUIRE(cinfo.accumImpulseB - reference_impulseB < 1.0e-12);

    ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo);
    REQUIRE(sphere_a_body.qdot.norm() < 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);
    CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0);
    double reference_impulseA = -sphere_a_mass * sphere_a_body.qdot[1];
    REQUIRE(cinfo.accumImpulseA - reference_impulseA < 1.0e-12);
    REQUIRE(cinfo.accumImpulseB < 1.0e-12);

    ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo);
    REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
  }

  SECTION("SphereVsSphereCollision") {
    double sphere_b_mass = 1.5;
    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);
    CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0);
    //    REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12);
    //    REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12);

    cout << "pre impulse:  " << sphere_a_body.qdot.transpose() << endl;
    cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl;
    ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo);
    cout << "pst impulse: " << sphere_a_body.qdot.transpose() << endl;
    cout << "pst impulse2: " << sphere_b_body.qdot.transpose() << endl;

    REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12);
    REQUIRE(sphere_b_body.qdot.norm() < 1.0e-12);
  }
}