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