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

// Use the namespaces of Chrono
using namespace chrono;
using namespace chrono::irrlicht;

// adapted from src/demos/mbs/demo_MBS_collision_trimesh.cpp
// texture 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.4);
    contact_mat->SetStaticFriction(0.0);
    auto falling = chrono_types::make_shared<ChBodyEasyMesh>(
        "../resource/spinningTop1.obj",7000,true,true,true,contact_mat,0.005);
    falling->SetRot(QuatFromAngleY(180 * CH_DEG_TO_RAD));
    falling->SetPos(ChVector3d(3, 0, 0)); 
    falling->SetAngVelLocal(ChVector3d(0,0, 5));
    sys.Add(falling);

    auto falling2 = chrono_types::make_shared<ChBodyEasyMesh>(
        "../resource/spinningTop2.obj",7000,true,true,true,contact_mat,0.005);
    falling2->SetRot(QuatFromAngleY(180 * CH_DEG_TO_RAD));
    falling2->SetPos(ChVector3d(-3, 0, 0)); 
    falling2->SetAngVelLocal(ChVector3d(0,0, 5));
    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, 1));
    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 = 0.0005;
    double next_save_time = 0;
    double save_interval = 0.01;
    int frame = 0;
    while (vis->Run()) {
        vis->BeginScene(true, true, ChColor(0.55f, 0.63f, 0.75f));
        vis->Render();
        vis->EndScene();
        sys.DoStepDynamics(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;
}