#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., 1.5, 0.);
    cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);

    REQUIRE(cresult == false);
  }

  SECTION ("Sphere penetration") {
    sphere.pos = Vector3d (0., -1., 0.);
    cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo);

    REQUIRE(cresult == false);
  }
}