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