#include <cmath>
#include <iomanip>
#include <iostream>
#include <string>

#include "chrono/core/ChGlobal.h"
#include "chrono/physics/ChBody.h"
#include "chrono/physics/ChSystemSMC.h"
#include "chrono/utils/ChUtilsSamplers.h"

#include "chrono_gpu/physics/ChSystemGpu.h"
#include "chrono_gpu/utils/ChGpuJsonParser.h"
#include "chrono_gpu/ChGpuData.h"


// added for motor:
#include "chrono/motion_functions/ChFunction.h"
#include "chrono/physics/ChForce.h"
#include "chrono/timestepper/ChTimestepper.h"
#include "chrono/utils/ChUtilsCreators.h"
#include "chrono/physics/ChLinkMotorLinearForce.h"
#include "chrono/physics/ChLinkMotorLinearPosition.h"
#include "chrono/physics/ChLinkMotorLinearSpeed.h"
#include "chrono/physics/ChLinkMotorRotationAngle.h"
#include "chrono/physics/ChLinkMotorRotationSpeed.h"
#include "chrono/physics/ChLinkMotorRotationTorque.h"
#include "chrono_gpu/ChGpuData.h"
#include "chrono_thirdparty/filesystem/path.h"
#include "chrono/assets/ChBoxShape.h"



using namespace chrono;
using namespace chrono::gpu;
//////////////////////////////////////////////////////////////////////////////////////////////////////////
// Simulation parameters (same as those in the json file)
double step_size = 5e-7;   // time step s
double sphere_radius = 0.2;  // cm sphere radius (6 mm glass beads same as experiments )
double sphere_density = 0.92; // g/cm^3 density of sphere particle
double time_settle = 0.75;// time needed for bed to settle
double time_end = .05;// time needed for the drop phase 
double compress_end = .95; // compressing the bed
double decompress_end = 1.05; // bed decompression 

// Define Big domain size
double box_X = 60;
double box_Y = 60;
double box_Z = 200;


int run_mode_id = 0;//settling phase
//int run_mode_id = 1;//drop phase
//int run_mode_id = 2; // pull phase

int helix_angle = 20;
std::string planet = "_Earth_CubeSim";
double youngs_modulus1 = 1e10;

// Gravity
double grav_X = 0.0f;
double grav_Y = 0.0f;
double grav_Z = -981.0f;

// Cube mass
float cube_mass = 600; //g value from Solidworks 20helix Angle. The original mass


///////////////////////////////////////////////////////////////////////////////////////////////////////

// ONLY for a Square/Rectangular shaped bed (If Cylindar need to be changed)
double getVoidRatio(ChSystemGpuMesh& gpu_sys, double sphere_radius) {
    double vol_sphere = 4.0f / 3.0f * chrono::CH_C_PI * std::pow(sphere_radius, 3) * gpu_sys.GetNumParticles();
    double vol_box = (box_X * box_Y) * (gpu_sys.GetMaxParticleZ() - gpu_sys.GetMinParticleZ());

    double eta = vol_sphere / vol_box;
    double voidRatio = (1.0f - eta) / eta;
    return voidRatio;
}

/////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////
void write_csv(std::string filename, std::vector<std::pair<std::string, std::vector<float>>> dataset) {
    // Make a CSV file with one or more columns of integer values
    // Each column of data is represented by the pair <column name, column data>
    //   as std::pair<std::string, std::vector<int>>
    // The dataset is represented as a vector of these columns
    // Note that all columns should be the same size

    // Create an output filestream object
    std::ofstream myFile(filename);

    // Send column names to the stream
    for (int j = 0; j < dataset.size(); ++j)
    {
        myFile << dataset.at(j).first;
        if (j != dataset.size() - 1) myFile << ","; // No comma at end of line
    }
    myFile << "\n";

    // Send data to the stream
    for (int i = 0; i < dataset.at(0).second.size(); ++i)
    {
        for (int j = 0; j < dataset.size(); ++j)
        {
            myFile << dataset.at(j).second.at(i);
            if (j != dataset.size() - 1) myFile << ","; // No comma at end of line
        }
        myFile << "\n";
    }

    // Close the file
    myFile.close();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Set up GPU system -bed Particles 
void SetupGPUSystem(ChSystemGpuMesh& gpu_sys) {


    double cor_p = 0.36; // sphere restitution
    double cor_w = 0.36;  // mesh & wall restitution (value from Thoesen et al., 2019)
    double cor_m = 0.8; // mesh restitution
    double youngs_modulus_p = 1e10; //(cms^2)
    double youngs_modulus_m = 7.2e11;
    double youngs_modulus_w = 1e10;
    double poisson_ratio_m = 0.33;
    double poisson_ratio_p = 0.32;
    double poisson_ratio_w = 0.32;
    double mu_s2s = 0.5;  // 0.5 static friction coefficient sphere to sphere
    double mu_s2w = 0.5;  //0.5  static friction coefficient sphere to wall & mesh (value from Thoesen et al., 2019)
    double mu_s2m = 0.04;
    float roll_s2s = 1; //1
    float roll_s2w = 1;
    float roll_s2m = 0.1;

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    gpu_sys.UseMaterialBasedModel(true); // use material based model (Young's modulus, restitution, Poisson's ratio)

    gpu_sys.SetYoungModulus_SPH(youngs_modulus_p);
    gpu_sys.SetYoungModulus_WALL(youngs_modulus_w);
    gpu_sys.SetYoungModulus_MESH(youngs_modulus_m);

    gpu_sys.SetRestitution_SPH(cor_p);
    gpu_sys.SetRestitution_WALL(cor_w);
    gpu_sys.SetRestitution_MESH(cor_m);

    gpu_sys.SetPoissonRatio_SPH(poisson_ratio_p);
    gpu_sys.SetPoissonRatio_WALL(poisson_ratio_w);
    gpu_sys.SetPoissonRatio_MESH(poisson_ratio_m);


    gpu_sys.SetFrictionMode(chrono::gpu::CHGPU_FRICTION_MODE::MULTI_STEP);
    gpu_sys.SetStaticFrictionCoeff_SPH2SPH(mu_s2s);
    gpu_sys.SetStaticFrictionCoeff_SPH2WALL(mu_s2w);
    gpu_sys.SetStaticFrictionCoeff_SPH2MESH(mu_s2m);

    gpu_sys.SetRollingMode(CHGPU_ROLLING_MODE::SCHWARTZ);
    gpu_sys.SetRollingCoeff_SPH2SPH(roll_s2s);
    gpu_sys.SetRollingCoeff_SPH2WALL(roll_s2w);
    gpu_sys.SetRollingCoeff_SPH2MESH(roll_s2m);

    gpu_sys.SetParticleOutputMode(CHGPU_OUTPUT_MODE::CSV);
    gpu_sys.SetTimeIntegrator(CHGPU_TIME_INTEGRATOR::CENTERED_DIFFERENCE);
    gpu_sys.SetFixedStepSize(step_size);
    gpu_sys.SetBDFixed(true);
    /////////////////////////////////////////////////////////////////////////////////////////////
    // Rain particles
    double fill_top;
    double fill_bottom = -99.0f;
    double box_xy = 58;                      // 56 cm by 56 cm box
    double box_r = box_xy / 2;
    double spacing = 2.001 * sphere_radius;

    std::vector<ChVector<float>> body_points;

    utils::PDSampler<float> sampler(spacing);
    fill_top = -80; // bring it back to -60 later  
    //fill_top = box_Z / 2 - spacing;  
    ChVector<> hdims(box_r - sphere_radius, box_r - sphere_radius, 0);
    int counter = 0;
    for (double z = fill_bottom + spacing; z < fill_top; z += spacing) {
        ChVector<> center(0, 0, z);
        auto points = sampler.SampleBox(center, hdims);
        body_points.insert(body_points.end(), points.begin(), points.end());
        counter = counter + points.size();

    }
    std::cout << "Created " << body_points.size() << "spheres" << std::endl;
    gpu_sys.SetParticles(body_points);
    gpu_sys.SetGravitationalAcceleration(ChVector<float>(grav_X, grav_Y, grav_Z));


}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SetupGPUParams(ChSystemGpuMesh& gpu_sys) {
    double cor_p = 0.36; // sphere restitution
    double cor_w = 0.36;  // mesh & wall restitution (value from Thoesen et al., 2019)
    double cor_m = 0.8; // mesh restitution
    double youngs_modulus_p = 1e10; // 40Mpa = 4e7Pa = 4e8 g/(cms^2)
    double youngs_modulus_m = 7.2e11;
    double youngs_modulus_w = 1e10;
    double poisson_ratio_m = 0.33;
    double poisson_ratio_p = 0.32;
    double poisson_ratio_w = 0.32;
    double mu_s2s = 0.5;  // 0.5 static friction coefficient sphere to sphere
    double mu_s2w = 0.5;  //0.5  static friction coefficient sphere to wall & mesh (value from Thoesen et al., 2019)
    double mu_s2m = 0.04;
    float roll_s2s = 1; //1
    float roll_s2w = 1;
    float roll_s2m = 0.1;

    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    gpu_sys.UseMaterialBasedModel(true); // use material based model (Young's modulus, restitution, Poisson's ratio)

    gpu_sys.SetYoungModulus_SPH(youngs_modulus_p);
    gpu_sys.SetYoungModulus_WALL(youngs_modulus_w);
    gpu_sys.SetYoungModulus_MESH(youngs_modulus_m);

    gpu_sys.SetRestitution_SPH(cor_p);
    gpu_sys.SetRestitution_WALL(cor_w);
    gpu_sys.SetRestitution_MESH(cor_m);

    gpu_sys.SetPoissonRatio_SPH(poisson_ratio_p);
    gpu_sys.SetPoissonRatio_WALL(poisson_ratio_w);
    gpu_sys.SetPoissonRatio_MESH(poisson_ratio_m);


    gpu_sys.SetFrictionMode(chrono::gpu::CHGPU_FRICTION_MODE::MULTI_STEP);
    gpu_sys.SetStaticFrictionCoeff_SPH2SPH(mu_s2s);
    gpu_sys.SetStaticFrictionCoeff_SPH2WALL(mu_s2w);
    gpu_sys.SetStaticFrictionCoeff_SPH2MESH(mu_s2m);

    gpu_sys.SetRollingMode(CHGPU_ROLLING_MODE::SCHWARTZ);
    gpu_sys.SetRollingCoeff_SPH2SPH(roll_s2s);
    gpu_sys.SetRollingCoeff_SPH2WALL(roll_s2w);
    gpu_sys.SetRollingCoeff_SPH2MESH(roll_s2m);

    gpu_sys.SetGravitationalAcceleration(ChVector<float>(grav_X, grav_Y, grav_Z));


    gpu_sys.SetParticleOutputMode(CHGPU_OUTPUT_MODE::CSV);
    gpu_sys.SetTimeIntegrator(CHGPU_TIME_INTEGRATOR::CENTERED_DIFFERENCE);
    gpu_sys.SetFixedStepSize(step_size);
    gpu_sys.SetBDFixed(true);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[]) {

    // Output directory
    std::string out_dir = GetChronoOutputPath() + "GPU/";
    filesystem::create_directory(filesystem::path(out_dir));
    out_dir = out_dir + "ballcosim";
    filesystem::create_directory(filesystem::path(out_dir));
    std::string checkpoint_file = out_dir + "checkpoint.dat";


    // run_mode_id == 1, this is simulation phase
    if (run_mode_id == 1)
    {
        // Load Checkpoint file
        ChSystemGpuMesh gpu_sys(checkpoint_file);
        // Material based model is not included in the checkpoint file
        // Setup GPU system simulation parameters
        SetupGPUParams(gpu_sys);




        //auto cube_mesh = gpu_sys.AddMesh(GetChronoDataFile("models/cube.obj"), ChVector<float>(0), ChMatrix33<>(1), cube_mass);
        auto cube_mesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
        cube_mesh->LoadWavefrontMesh(std::string("../cube_thin.obj"), true, false);
        cube_mesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1));
        auto cube_mesh_id = gpu_sys.AddMesh(cube_mesh, cube_mass);

        gpu_sys.EnableMeshCollision(true);
        gpu_sys.Initialize();
        std::cout << gpu_sys.GetNumMeshes() << " meshes" << std::endl;
        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        ChSystemSMC sys_cube;
        sys_cube.SetContactForceModel(ChSystemSMC::ContactForceModel::Hooke);
        sys_cube.SetTimestepperType(ChTimestepper::Type::EULER_IMPLICIT);
        sys_cube.Set_G_acc(ChVector<>(grav_X, grav_Y, grav_Z));



        //cube inertias:
        float cube_inertia_x = .095;//
        float cube_inertia_y = .187;//
        float cube_inertia_z = .095;//


        ChVector<> cube_initial_pos(0, 0, -86.59);

        //cube body
        std::shared_ptr<ChBody> cube_body(sys_cube.NewBody());
        cube_body->SetMass(cube_mass);
        cube_body->SetInertiaXX(ChVector<>(cube_inertia_x, cube_inertia_y, cube_inertia_z));
        cube_body->SetPos(cube_initial_pos);
        ChQuaternion<> mesh_rot = Q_from_AngAxis(-90 * CH_C_DEG_TO_RAD, VECT_X);
        cube_body->SetRot(mesh_rot);
        cube_body->SetBodyFixed(false);
        sys_cube.AddBody(cube_body);

        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////         //Motor Guide:
        /*std::shared_ptr<ChBody> guide_body(sys_cube.NewBody());
        guide_body->SetMass(cube_mass);
        guide_body->SetInertiaXX(ChVector<>(cube_inertia_x, cube_inertia_y, cube_inertia_z));
        guide_body->SetPos(cube_initial_pos);
        guide_body->SetBodyFixed(true); //must be fixed
        sys_cube.AddBody(guide_body);

        ChQuaternion<> mesh_rot = Q_from_AngAxis(-90 * CH_C_DEG_TO_RAD, VECT_X);
        ChVector<> mesh_trans(cube_initial_pos);
        ChFrame<> Xb(mesh_trans,mesh_rot);
*/
//Motor Parameters:
//float lin_freq = 0.5f;
//float lin_period = 1.f / lin_freq;
//float lin_vel = 10;

// Create and initialize the linear motor on tip of the body
//auto motor_lin = chrono_types::make_shared<ChLinkMotorLinearSpeed>();
//motor_lin->Initialize(
//cube_body,                                    // body A (slave)
//guide_body,                                  // body B (master)
//Xb); // motor frame, in abs. coords
// Add the motor to the system
//sys_cube.AddLink(motor_lin);


    // Create a ChFunction to be used for the ChLinkMotorLinearSpeed
    // lin_rep_seq is a step function for the linear velocity control
    //auto lin_part = chrono_types::make_shared<ChFunction_Const>();
    //lin_part->Set_yconst(lin_vel);
    //auto lin_seq = chrono_types::make_shared<ChFunction_Sequence>();
    //lin_seq->InsertFunct(lin_part, time_end, 1, false); // fx, duration, weight, enforce C0 continuity
    //auto lin_rep_seq_0 = chrono_types::make_shared<ChFunction_Repeat>();
    //lin_rep_seq_0->Set_fa(lin_seq);
    //lin_rep_seq_0->Set_window_length(lin_period);
    //lin_rep_seq_0->Set_window_start(0.25);
    //lin_rep_seq_0->Set_window_phase(lin_period);
    // Let the linearmotor use this motion function:
    //motor_lin_0->SetSpeedFunction(lin_rep_seq_0);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 



// Setup detailed output options

        float out_fps = 200;
        std::cout << "Output at    " << out_fps << " FPS" << std::endl;
        unsigned int out_steps = (unsigned int)(1.0 / (out_fps * step_size));
        unsigned int total_frames = (unsigned int)(time_end * out_fps);


        int currframe = 0;
        unsigned int curr_step = 0;

        std::vector<float> cube_fx(total_frames);
        std::vector<float> cube_fy(total_frames);
        std::vector<float> cube_fz(total_frames);

        std::vector<float> cube_Tx(total_frames);
        std::vector<float> cube_Ty(total_frames);
        std::vector<float> cube_Tz(total_frames);

        std::vector<float> cube_Vx(total_frames);
        std::vector<float> cube_Vy(total_frames);
        std::vector<float> cube_Vz(total_frames);

        std::vector<float> cube_ax(total_frames);
        std::vector<float> cube_ay(total_frames);
        std::vector<float> cube_az(total_frames);

        std::vector<float> cube_Px(total_frames);
        std::vector<float> cube_Py(total_frames);
        std::vector<float> cube_Pz(total_frames);

        std::vector<float> cube_R0(total_frames);
        std::vector<float> cube_R1(total_frames);
        std::vector<float> cube_R2(total_frames);
        std::vector<float> cube_R3(total_frames);

        std::vector<float> cube_Rv0(total_frames);
        std::vector<float> cube_Rv1(total_frames);
        std::vector<float> cube_Rv2(total_frames);
        std::vector<float> cube_Rv3(total_frames);

        std::vector<float> cube_Ra0(total_frames);
        std::vector<float> cube_Ra1(total_frames);
        std::vector<float> cube_Ra2(total_frames);
        std::vector<float> cube_Ra3(total_frames);

        std::vector<float> KE(total_frames);



        clock_t start = std::clock();
        for (double t = 0; t < (double)time_end;
            t += step_size, curr_step++) {
            //// Apply motion to the meshes
            ChVector<float> cube_pos(0, 0, 0);
            cube_pos.z() = cube_body->GetPos().z();
            cube_pos.y() = cube_body->GetPos().y();
            cube_pos.x() = cube_body->GetPos().x();


            // initial positions and velocity 
            ChVector<float> mesh_pos(0, 0, 0);
            float rev_per_sec = 1;
            float ang_vel_Z = rev_per_sec * (float)CH_C_PI;
            ChVector<> mesh_lin_vel(0, 10, 0);
            ChQuaternion<> mesh_rot = Q_from_AngY(t * ang_vel_Z);

            ChVector<> mesh_ang_vel(0, 0, ang_vel_Z);

                gpu_sys.ApplyMeshMotion(0, cube_body->GetPos(), cube_body->GetRot(),
                    cube_body->GetPos_dt(), cube_body->GetWvel_par());
      

            //// calculate forces on the tip  ////
            ChVector<> cube_force;
            ChVector<> cube_torque;
            gpu_sys.CollectMeshContactForces(0, cube_force, cube_torque);
            cube_body->Empty_forces_accumulators();
            cube_body->Accumulate_force(cube_force, cube_body->GetPos(), false);
            cube_body->Accumulate_torque(cube_torque, false);


            if (t == 0.25) {
                std::cout << "Next Output frame is at 0.25s" << std::endl;
            }

            if (curr_step % out_steps == 0) {
                std::cout << "Output frame " << currframe + 1 << " of " << total_frames << std::endl;
                char filename[100];
                char mesh_filename[100];



                gpu_sys.SetParticleOutputFlags(ABSV | FORCE_COMPONENTS);

                KE[currframe] = gpu_sys.GetParticlesKineticEnergy();

                //const ChVector<>& pos= cube_body->GetPos();
                //std::cout << "Position of cube in z direction   " << pos  << std::endl;

                const ChQuaternion<>& Rot = cube_body->GetRot();
                //std::cout << "Rotation of cube    " << Rot << std::endl;

                const ChQuaternion<>& Rot_velocity = cube_body->GetRot_dt();

                const ChQuaternion<>& Rot_acc = cube_body->GetRot_dtdt();

                //const ChVector<>& pos= cube_body->GetPos_dt();
                //std::cout << "Velocity of cube in z direction   " << pos  << std::endl;


                cube_fx[currframe] = cube_force.x();
                cube_fy[currframe] = cube_force.y();
                cube_fz[currframe] = cube_force.z();

                cube_Tx[currframe] = cube_torque.x();
                cube_Ty[currframe] = cube_torque.y();
                cube_Tz[currframe] = cube_torque.z();

                cube_Px[currframe] = cube_body->GetPos().x();
                cube_Py[currframe] = cube_body->GetPos().y();
                cube_Pz[currframe] = cube_body->GetPos().z();

                cube_Vx[currframe] = cube_body->GetPos_dt().x();
                cube_Vy[currframe] = cube_body->GetPos_dt().y();
                cube_Vz[currframe] = cube_body->GetPos_dt().z();

                cube_ax[currframe] = cube_body->GetPos_dtdt().x();
                cube_ay[currframe] = cube_body->GetPos_dtdt().y();
                cube_az[currframe] = cube_body->GetPos_dtdt().z();

                cube_R0[currframe] = Rot.e0();
                cube_R1[currframe] = Rot.e1();
                cube_R2[currframe] = Rot.e2();
                cube_R3[currframe] = Rot.e3();

                cube_Rv0[currframe] = Rot_velocity.e0();
                cube_Rv1[currframe] = Rot_velocity.e1();
                cube_Rv2[currframe] = Rot_velocity.e2();
                cube_Rv3[currframe] = Rot_velocity.e3();

                cube_Ra0[currframe] = Rot_acc.e0();
                cube_Ra1[currframe] = Rot_acc.e1();
                cube_Ra2[currframe] = Rot_acc.e2();
                cube_Ra3[currframe] = Rot_acc.e3();


                sprintf(filename, "%s/step%06d.csv", out_dir.c_str(), currframe);
                sprintf(mesh_filename, "%s/step%06d_mesh", out_dir.c_str(), currframe++);
                gpu_sys.WriteParticleFile(std::string(filename));
                gpu_sys.WriteMeshes(std::string(mesh_filename));




            }
            gpu_sys.AdvanceSimulation(step_size);
            sys_cube.DoStepDynamics(step_size);

        }

        std::vector<std::pair<std::string, std::vector<float>>> vals = { {"Fx", cube_fx}, {"Fy", cube_fy}, {"Fz", cube_fz},{"Tx", cube_Tx}, {"Ty", cube_Ty}, {"Tz", cube_Tz},{"Px", cube_Px}, {"Py", cube_Py}, {"Pz", cube_Pz},{"Vx", cube_Vx}, {"Vy", cube_Vy}, {"Vz", cube_Vz},{"ax", cube_Px}, {"ay", cube_Py}, {"az", cube_Pz},{"R0", cube_R0},{"R1", cube_R1}, {"R2", cube_R2}, {"R3", cube_R3},{"Rv0", cube_R0},{"Rv1", cube_R1}, {"Rv2", cube_R2}, {"Rv3", cube_R3},{"Ra0", cube_R0},{"Ra1", cube_R1}, {"Ra2", cube_R2}, {"Ra3", cube_R3},{"KE", KE} };

        write_csv("simulation_outputs_" + std::to_string(helix_angle) + planet + std::to_string(run_mode_id) + ".csv", vals);

        clock_t end = std::clock();
        double total_time = ((double)(end - start)) / CLOCKS_PER_SEC;

        std::cout << "Time: " << total_time << " seconds" << std::endl;
        gpu_sys.WriteCheckpointFile(checkpoint_file);
        return 0;

    }
    else if (run_mode_id == 2) 
    {
    ChSystemGpuMesh gpu_sys(checkpoint_file);
    // Material based model is not included in the checkpoint file
    // Setup GPU system simulation parameters
    SetupGPUParams(gpu_sys);




    //auto cube_mesh = gpu_sys.AddMesh(GetChronoDataFile("models/cube.obj"), ChVector<float>(0), ChMatrix33<>(1), cube_mass);
    auto cube_mesh = chrono_types::make_shared<geometry::ChTriangleMeshConnected>();
    cube_mesh->LoadWavefrontMesh(std::string("../cube_thin.obj"), true, false);
    cube_mesh->Transform(ChVector<>(0, 0, 0), ChMatrix33<>(1));
    auto cube_mesh_id = gpu_sys.AddMesh(cube_mesh, cube_mass);

    gpu_sys.EnableMeshCollision(true);
    gpu_sys.Initialize();
    std::cout << gpu_sys.GetNumMeshes() << " meshes" << std::endl;
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ChSystemSMC sys_cube;
    sys_cube.SetContactForceModel(ChSystemSMC::ContactForceModel::Hooke);
    sys_cube.SetTimestepperType(ChTimestepper::Type::EULER_IMPLICIT);
    sys_cube.Set_G_acc(ChVector<>(grav_X, grav_Y, grav_Z));



    //cube inertias:
    float cube_inertia_x = .095;//
    float cube_inertia_y = .187;//
    float cube_inertia_z = .095;//


   ChVector<> ball_initial_pos(0,0,-86.8); 

    //cube body
    std::shared_ptr<ChBody> cube_body(sys_cube.NewBody());
    cube_body->SetMass(cube_mass);
    cube_body->SetInertiaXX(ChVector<>(cube_inertia_x, cube_inertia_y, cube_inertia_z));
    cube_body->SetPos(ball_initial_pos);
    ChQuaternion<> mesh_rot = Q_from_AngAxis(-90 * CH_C_DEG_TO_RAD, VECT_X);
    cube_body->SetRot(mesh_rot);
    cube_body->SetBodyFixed(false);
    sys_cube.AddBody(cube_body);

    float out_fps = 200;
    std::cout << "Output at    " << out_fps << " FPS" << std::endl;
    unsigned int out_steps = (unsigned int)(1.0 / (out_fps * step_size));
    unsigned int total_frames = (unsigned int)(time_end * out_fps);


    int currframe = 0;
    unsigned int curr_step = 0;

    std::vector<float> cube_fx(total_frames);
    std::vector<float> cube_fy(total_frames);
    std::vector<float> cube_fz(total_frames);

    std::vector<float> cube_Tx(total_frames);
    std::vector<float> cube_Ty(total_frames);
    std::vector<float> cube_Tz(total_frames);

    std::vector<float> cube_Vx(total_frames);
    std::vector<float> cube_Vy(total_frames);
    std::vector<float> cube_Vz(total_frames);

    std::vector<float> cube_ax(total_frames);
    std::vector<float> cube_ay(total_frames);
    std::vector<float> cube_az(total_frames);

    std::vector<float> cube_Px(total_frames);
    std::vector<float> cube_Py(total_frames);
    std::vector<float> cube_Pz(total_frames);

    std::vector<float> cube_R0(total_frames);
    std::vector<float> cube_R1(total_frames);
    std::vector<float> cube_R2(total_frames);
    std::vector<float> cube_R3(total_frames);

    std::vector<float> cube_Rv0(total_frames);
    std::vector<float> cube_Rv1(total_frames);
    std::vector<float> cube_Rv2(total_frames);
    std::vector<float> cube_Rv3(total_frames);

    std::vector<float> cube_Ra0(total_frames);
    std::vector<float> cube_Ra1(total_frames);
    std::vector<float> cube_Ra2(total_frames);
    std::vector<float> cube_Ra3(total_frames);

    std::vector<float> KE(total_frames);



    clock_t start = std::clock();
    for (double t = 0; t < (double)time_end;
        t += step_size, curr_step++) {
        //// Apply motion to the meshes
        ChVector<float> cube_pos(0, 0, 0);
        cube_pos.z() = cube_body->GetPos().z();
        cube_pos.y() = cube_body->GetPos().y();
        cube_pos.x() = cube_body->GetPos().x();


        // initial positions and velocity 
        ChVector<float> mesh_pos(0, 0, 0);
        float rev_per_sec = 1;
        float ang_vel_Z = rev_per_sec * (float)CH_C_PI;
        ChVector<> mesh_lin_vel(0, 10, 0);
        ChQuaternion<> mesh_rot = Q_from_AngY(t * ang_vel_Z);

        ChVector<> mesh_ang_vel(0, 0, ang_vel_Z);

        gpu_sys.ApplyMeshMotion(0, cube_body->GetPos(), cube_body->GetRot(),
        mesh_lin_vel, cube_body->GetWvel_par());

        ChVector<> cube_force;
        ChVector<> cube_torque;
        gpu_sys.CollectMeshContactForces(0, cube_force, cube_torque);
        cube_body->Empty_forces_accumulators();
        cube_body->Accumulate_force(cube_force, cube_body->GetPos(), false);
        cube_body->Accumulate_torque(cube_torque, false);


        if (t == 0.25) {
            std::cout << "Next Output frame is at 0.25s" << std::endl;
        }

        if (curr_step % out_steps == 0) {
            std::cout << "Output frame " << currframe + 1 << " of " << total_frames << std::endl;
            char filename[100];
            char mesh_filename[100];



            gpu_sys.SetParticleOutputFlags(ABSV | FORCE_COMPONENTS);

            KE[currframe] = gpu_sys.GetParticlesKineticEnergy();

            //const ChVector<>& pos= cube_body->GetPos();
            //std::cout << "Position of cube in z direction   " << pos  << std::endl;

            const ChQuaternion<>& Rot = cube_body->GetRot();
            //std::cout << "Rotation of cube    " << Rot << std::endl;

            const ChQuaternion<>& Rot_velocity = cube_body->GetRot_dt();

            const ChQuaternion<>& Rot_acc = cube_body->GetRot_dtdt();

            //const ChVector<>& pos= cube_body->GetPos_dt();
            //std::cout << "Velocity of cube in z direction   " << pos  << std::endl;


            cube_fx[currframe] = cube_force.x();
            cube_fy[currframe] = cube_force.y();
            cube_fz[currframe] = cube_force.z();

            cube_Tx[currframe] = cube_torque.x();
            cube_Ty[currframe] = cube_torque.y();
            cube_Tz[currframe] = cube_torque.z();

            cube_Px[currframe] = cube_body->GetPos().x();
            cube_Py[currframe] = cube_body->GetPos().y();
            cube_Pz[currframe] = cube_body->GetPos().z();

            cube_Vx[currframe] = cube_body->GetPos_dt().x();
            cube_Vy[currframe] = cube_body->GetPos_dt().y();
            cube_Vz[currframe] = cube_body->GetPos_dt().z();

            cube_ax[currframe] = cube_body->GetPos_dtdt().x();
            cube_ay[currframe] = cube_body->GetPos_dtdt().y();
            cube_az[currframe] = cube_body->GetPos_dtdt().z();

            cube_R0[currframe] = Rot.e0();
            cube_R1[currframe] = Rot.e1();
            cube_R2[currframe] = Rot.e2();
            cube_R3[currframe] = Rot.e3();

            cube_Rv0[currframe] = Rot_velocity.e0();
            cube_Rv1[currframe] = Rot_velocity.e1();
            cube_Rv2[currframe] = Rot_velocity.e2();
            cube_Rv3[currframe] = Rot_velocity.e3();

            cube_Ra0[currframe] = Rot_acc.e0();
            cube_Ra1[currframe] = Rot_acc.e1();
            cube_Ra2[currframe] = Rot_acc.e2();
            cube_Ra3[currframe] = Rot_acc.e3();


            sprintf(filename, "%s/step%06d.csv", out_dir.c_str(), currframe);
            sprintf(mesh_filename, "%s/step%06d_mesh", out_dir.c_str(), currframe++);
            gpu_sys.WriteParticleFile(std::string(filename));
            gpu_sys.WriteMeshes(std::string(mesh_filename));




        }
        gpu_sys.AdvanceSimulation(step_size);
        sys_cube.DoStepDynamics(step_size);

        }

        std::vector<std::pair<std::string, std::vector<float>>> vals = { {"Fx", cube_fx}, {"Fy", cube_fy}, {"Fz", cube_fz},{"Tx", cube_Tx}, {"Ty", cube_Ty}, {"Tz", cube_Tz},{"Px", cube_Px}, {"Py", cube_Py}, {"Pz", cube_Pz},{"Vx", cube_Vx}, {"Vy", cube_Vy}, {"Vz", cube_Vz},{"ax", cube_Px}, {"ay", cube_Py}, {"az", cube_Pz},{"R0", cube_R0},{"R1", cube_R1}, {"R2", cube_R2}, {"R3", cube_R3},{"Rv0", cube_R0},{"Rv1", cube_R1}, {"Rv2", cube_R2}, {"Rv3", cube_R3},{"Ra0", cube_R0},{"Ra1", cube_R1}, {"Ra2", cube_R2}, {"Ra3", cube_R3},{"KE", KE} };

        write_csv("simulation_outputs_" + std::to_string(helix_angle) + planet + std::to_string(run_mode_id) + ".csv", vals);

        clock_t end = std::clock();
        double total_time = ((double)(end - start)) / CLOCKS_PER_SEC;

        std::cout << "Time: " << total_time << " seconds" << std::endl;
        //gpu_sys.WriteCheckpointFile(checkpoint_file);
        return 0;

    }

    // run_mode_id == 0, this is the sample preparation phase
    ChSystemGpuMesh gpu_sys(sphere_radius, sphere_density, ChVector<float>(box_X, box_Y, box_Z));
    SetupGPUSystem(gpu_sys);
    gpu_sys.EnableMeshCollision(false);
    gpu_sys.Initialize();

    // compression implementation goes here


    float curr_timee = 0;
    unsigned int currframe = 0;
    float out_fps = 100;
    unsigned int out_steps = (unsigned int)(1 / (out_fps * step_size));
    unsigned int total_frames = (unsigned int)(time_settle * out_fps);
    unsigned int curr_step = 0;
    for (double t = 0; t < (double)time_settle; t += step_size, curr_step++) {
        if (curr_step % out_steps == 0) {
            std::cout << "Output frame " << currframe + 1 << " of " << total_frames << std::endl;
            char filename[100];
            sprintf(filename, "%s/step%06d.csv", out_dir.c_str(), currframe++);
            gpu_sys.WriteParticleFile(std::string(filename));
        }
        gpu_sys.AdvanceSimulation(step_size);
        curr_timee += step_size;
    }
    double vr1 = getVoidRatio(gpu_sys, sphere_radius);
    double pr1 = vr1 / (1.0f + vr1);
    std::cout << "Void_Ratio   " << vr1 << std::endl;
    std::cout << "Porosity  " << pr1 << std::endl;
    std::cout << "MAX_z   " << gpu_sys.GetMaxParticleZ() << std::endl;
    std::cout << "Min_z  " << gpu_sys.GetMinParticleZ() << std::endl;
    std::cout << "youngs_modulus  " << youngs_modulus1 << std::endl;
    //////////////////////////////////////////////////////////////////////////////////////////////////
        // This is settling phase, so output a checkpoint file

    gpu_sys.WriteCheckpointFile(checkpoint_file);
    return 0;
}