#include "chrono/physics/ChSystemNSC.h"
#include "chrono/physics/ChBodyEasy.h"
#include "chrono/physics/ChMassProperties.h"
#include "chrono/assets/ChTexture.h"
#include "chrono/assets/ChVisualShapeTriangleMesh.h"
#include "chrono/geometry/ChTriangleMeshConnected.h"
#include "chrono/core/ChRandom.h"
#include "chrono/core/ChRealtimeStep.h"
#include "chrono_irrlicht/ChVisualSystemIrrlicht.h"

using namespace chrono;
using namespace chrono::irrlicht;
// adapted from src/demos/mbs/demo_MBS_collision_trimesh.cpp
// resource from https://github.com/tizian/Spinning-Top-Simulation/

// -----------------------------------------------------------------------------
ChCollisionSystem::Type coll_sys_type = ChCollisionSystem::Type::BULLET;
// -----------------------------------------------------------------------------

int main(int argc, char* argv[]) {
    ChRandom::SetSeed(0);

    ChSystemNSC sys;
    sys.SetGravitationalAcceleration(ChVector3d(0, 0, -9.8));
    std::cout << "gravity is " << sys.GetGravitationalAcceleration() << "\n";
    ChCollisionModel::SetDefaultSuggestedEnvelope(0.0025);
    ChCollisionModel::SetDefaultSuggestedMargin(0.0025);
    sys.SetCollisionSystemType(coll_sys_type);

    auto contact_mat = chrono_types::make_shared<ChContactMaterialNSC>();
    contact_mat->SetRestitution(0.4f);
    contact_mat->SetStaticFriction(0.0f);

    double omg1 = 50;
    double omg2 = 50;

    auto falling1 = chrono_types::make_shared<ChBodyEasyMesh>("../resource/spinningTop5.obj", 7000, true, true, true,contact_mat, 0.005);
    falling1->SetFrameRefToAbs(ChFramed(ChVector3d(3, 0, 0), QuatFromAngleX(CH_PI_2)));
    falling1->SetAngVelLocal(ChVector3d(0, 0, omg1));
    sys.Add(falling1);

    auto falling2 = chrono_types::make_shared<ChBodyEasyMesh>("../resource/spinningTop6.obj", 7000, true, true, true, contact_mat, 0.005);
    falling2->SetFrameRefToAbs(ChFramed(ChVector3d(-3, 0, 0), QuatFromAngleX(CH_PI_2)));
    falling2->SetAngVelLocal(ChVector3d(0, 0, omg2));
    sys.Add(falling2);

    auto floor =
        chrono_types::make_shared<ChBodyEasyMesh>("../resource/floor.obj", 7000, true, true, true, contact_mat, 0.005);
    floor->SetPos(ChVector3d(0, 0, -3));
    floor->SetFixed(true);
    sys.Add(floor);

    auto vis = chrono_types::make_shared<ChVisualSystemIrrlicht>();
    vis->AttachSystem(&sys);
    vis->SetWindowSize(1280, 720);
    vis->SetWindowTitle("Collisions between objects");
    vis->Initialize();
    vis->AddLogo();
    vis->AddSkyBox();
    vis->AddCamera(ChVector3d(0, -5, 10));
    vis->AddLight(ChVector3d(30, 80, 30), 80, ChColor(0.7f, 0.7f, 0.7f));
    vis->AddLight(ChVector3d(-30, 80, 30), 80, ChColor(0.7f, 0.7f, 0.7f));
    vis->EnableShadows();

    ChRealtimeStepTimer realtime_timer;

    double timestep = 1e-3;
    int frame = 0;
    double next_save_time = 0;
    double save_interval = 0.01;

    while (vis->Run()) {
        vis->BeginScene();
        vis->Render();
        vis->EndScene();

        sys.DoStepDynamics(timestep);
        realtime_timer.Spin(timestep);

        double cur_time = vis->GetSimulationTime();
        char time_text[50];
        std::cout << "\rSimulation Time:" << vis->GetSimulationTime() << std::flush;
        if (cur_time >= next_save_time) {
           char filename[100];
           sprintf(filename, "frames/frame_%04d.png", frame++);
           vis->WriteImageToFile(filename);
           std::cout << "Saving at t = " << cur_time << std::endl;
           next_save_time += save_interval;
        }
    }

    return 0;
}