#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_vsg/ChVisualSystemVSG.h"

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

// 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.4f);
    contact_mat->SetStaticFriction(0.0f);

    double omg1 = 50;
    double omg2 = 50;

    auto falling1 = chrono_types::make_shared<ChBodyEasyMesh>("../resource/spinningTop1.obj", 7000, true, true, true,
                                                              contact_mat, 0.005);
    ////falling1->SetFixed(true);
    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/spinningTop2.obj", 7000, true, true, true,
                                                              contact_mat, 0.005);
    ////falling2->SetFixed(true);
    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<ChVisualSystemVSG>();
    vis->AttachSystem(&sys);
    vis->SetWindowTitle("Collision");
    vis->SetCameraVertical(CameraVerticalDir::Y);
    vis->AddCamera(ChVector3d(0, -15, 1), ChVector3d(0, 0, 0));
    vis->SetWindowSize(1280, 800);
    vis->SetWindowPosition(100, 100);
    vis->SetCameraAngleDeg(40.0);
    vis->SetLightIntensity(1.0f);
    vis->SetLightDirection(1.5 * CH_PI_2, CH_PI_4);
    vis->EnableShadows();
    vis->Initialize();

    vis->ToggleRefFrameVisibility();
    vis->SetRefFrameScale(2.0);
    vis->SetContactNormalsVisibility(true);

    ChRealtimeStepTimer realtime_timer;

    double timestep = 5e-4;
    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;
}