// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All rights reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Radu Serban, Asher Elmquist
// =============================================================================
//
// Main driver function for the Sedan full model.
//
// The vehicle reference frame has Z up, X towards the front of the vehicle, and
// Y pointing to the left.
//
// =============================================================================


#include <istream>
#include <fstream>

#include "chrono/core/ChStream.h"
#include "chrono/core/ChRealtimeStep.h"
#include "chrono/core/ChFrame.h"
#include "chrono/utils/ChUtilsInputOutput.h"
#include "chrono/utils/ChFilters.h"

#include "chrono_vehicle/ChConfigVehicle.h"
#include "chrono_vehicle/ChVehicleModelData.h"
#include "chrono_vehicle/terrain/FlatTerrain.h"
#include "chrono_vehicle/terrain/RigidTerrain.h"
#include "chrono_vehicle/driver/ChIrrGuiDriver.h"
#include "chrono_vehicle/driver/ChDataDriver.h"
#include "chrono_vehicle/wheeled_vehicle/utils/ChWheeledVehicleIrrApp.h"

#include "chrono_models/vehicle/sedan/Sedan.h"

#include "chrono_thirdparty/filesystem/path.h"

using namespace chrono;
using namespace chrono::irrlicht;
using namespace chrono::vehicle;
using namespace chrono::vehicle::sedan;

// =============================================================================

// Initial vehicle location and orientation
ChVector<> initLoc(0, 0, 0.2);
ChQuaternion<> initRot(1, 0, 0, 0);

CollisionType chassis_collision_type = CollisionType::NONE;
TireModelType tire_model = TireModelType::TMEASY;
ChContactMethod contact_method = ChContactMethod::SMC;

// Simulation step sizes
double step_size = 0.002;
double tire_step_size = 0.001;


int main(int argc, char* argv[]) {
    std::fstream ifile("../input.txt", std::ios::out);
    ifile << "time==speed==acc" << std::endl;
    Sedan my_sedan;
    my_sedan.SetContactMethod(contact_method);
    my_sedan.SetChassisCollisionType(chassis_collision_type);
    my_sedan.SetChassisFixed(false);
    my_sedan.SetInitPosition(ChCoordsys<>(initLoc, initRot));
    my_sedan.SetTireType(tire_model);
    my_sedan.SetTireStepSize(tire_step_size);
    my_sedan.SetInitFwdVel(20.0);
    std::vector<double> wheel_v(4, 20.0);
    my_sedan.SetInitWheelAngVel(wheel_v);
    my_sedan.Initialize();
    
    ChVector<> r_t_init = my_sedan.GetChassis()->GetPos() ;
    std::cout << "r_t_init: " << r_t_init.x() << " " << r_t_init.y() << " " << r_t_init.z() << std::endl;


    // Create the terrain
    RigidTerrain terrain(my_sedan.GetSystem());

    // terrain.Initialize();
    // FlatTerrain terrain(0.0);

    MaterialInfo minfo;
    minfo.mu = 0.9f;
    minfo.cr = 0.01f;
    minfo.Y = 2e7f;
    auto patch_mat = minfo.CreateMaterial(contact_method);
    std::shared_ptr<RigidTerrain::Patch> patch;
    patch = terrain.AddPatch(patch_mat, ChVector<>(0, 0, 0), ChVector<>(0, 0, 1), 200, 200, 1, true, 10.0, false);
    terrain.Initialize();
    

    ChDriver::Inputs driver_inputs;
    driver_inputs.m_steering = 0.0;
    driver_inputs.m_throttle = 0.0;
    driver_inputs.m_braking = 0.0;

    // ChDriver driver(my_sedan.GetVehicle());
    // driver.SetSteering(0, -1, 1);
    // driver.SetThrottle(0, 0, 30);
    // driver.SetBraking(0, -30, 0);
    
    ChRealtimeStepTimer realtime_timer; 
    my_sedan.GetSystem()->SetSolverTolerance(0.01);
    // ChFrame<> cog_to_ref = my_sedan.GetChassisBody()->GetFrame_COG_to_REF();
    // ChVector<> t_cog_ref = cog_to_ref.GetPos();
    // ChQuaternion<> r_cog_ref = cog_to_ref.GetRot();
    // std::cout << "t_cog_ref: " << t_cog_ref.x() << " " << t_cog_ref.y() << " " << t_cog_ref.z() << std::endl; 
    // std::cout << "r_cog_ref: " << r_cog_ref.e0() << " " << r_cog_ref.e1() << " " << r_cog_ref.e2() << " " << r_cog_ref.e3() << std::endl;
    double avg_time = 0.0;
    int id = 0, num = 0;
    double last_time = 0.0, last_speed = 0.0;
    bool first_flag = true;
    for(int i = 0; i < 1000; i++)
    // for(int i = 0; ; i++)
    {
        // if(i % 2 == 1){
        //     ChVector<> setloc(100, 50, 0.2);
        //     ChQuaternion<> setrot(0.992, 0, 0.02, 0);
        //     my_sedan.SetPosition(ChCoordsys<>(setloc, setrot));
        //     ChVector<> r_t = my_sedan.GetChassis()->GetPos() ;
        //     std::cout << "R_t: " << r_t.x() << " " << r_t.y() << " " << r_t.z() << std::endl; 
        //     std::cout << "set success!" << std::endl;
        // }
        // if(i > 1 && i < 50){
        //     ChVector<> setloc(100, 50, 0.2);
        //     ChQuaternion<> setrot(0.992, 0, 0.02, 0);
        //     ChFrame<> frame(setloc, setrot);
        //     my_sedan.GetChassisBody()->SetFrame_REF_to_abs(frame);

        // }

        // cog_to_ref = my_sedan.GetChassisBody()->GetFrame_COG_to_REF();
        // t_cog_ref = cog_to_ref.GetPos();
        // r_cog_ref = cog_to_ref.GetRot();
        // std::cout << "t_cog_ref: " << t_cog_ref.x() << " " << t_cog_ref.y() << " " << t_cog_ref.z() << std::endl; 
        // std::cout << "r_cog_ref: " << r_cog_ref.e0() << " " << r_cog_ref.e1() << " " << r_cog_ref.e2() << " " << r_cog_ref.e3() << std::endl;

        double time = my_sedan.GetSystem()->GetChTime();
        // Update modules (process inputs from other modules)
        // terrain.Synchronize(time);
        // my_sedan.Synchronize(time, driver_inputs, terrain);
        // if(i % 2 == 1){                    
        // Advance simulation for one timestep for all modules
        int n = 0;
        while(n++ < 10 ){
            terrain.Advance(step_size);
            auto tp1 = std::chrono::steady_clock::now();
            my_sedan.Advance(step_size);
            auto tp2 = std::chrono::steady_clock::now();
            double cost_time = std::chrono::duration_cast<std::chrono::microseconds>(tp2 - tp1).count() / 1000.0;
            avg_time += cost_time;
            id ++;
            terrain.Synchronize(time);
            my_sedan.Synchronize(time, driver_inputs, terrain);
            // getchar();
        }

        double cur_time = my_sedan.GetSystem()->GetChTime();
        double cur_speed = my_sedan.GetChassis()->GetSpeed();
        double grad = (cur_speed - last_speed) / (cur_time - last_time);
        std::cout << "time: " <<   cur_time << " speed: " << cur_speed << " acc: " << grad << std::endl;
        ChVector<> r_t = my_sedan.GetChassis()->GetPos() ;
        std::cout << "R_t: " << r_t.x() << " " << r_t.y() << " " << r_t.z() << std::endl; 
        std::cout << "Inputs: " << driver_inputs.m_steering << " " << driver_inputs.m_throttle << " " << driver_inputs.m_braking << std::endl;
        

        if(first_flag){
            ifile << cur_time << "==" << cur_speed << std::endl;
            first_flag = false;
        }else{
            ifile << cur_time << "==" << cur_speed << "==" << grad << std::endl;
        }

        // double timer_step = my_sedan.GetSystem()->GetTimerStep();
        // double timer_advance = my_sedan.GetSystem()->GetTimerAdvance();
        // double timer_solve = my_sedan.GetSystem()->GetTimerLSsolve();
        // double timer_ls_setup = my_sedan.GetSystem()->GetTimerLSsetup();
        // double timer_collision = my_sedan.GetSystem()->GetTimerCollision();
        // double timer_setup = my_sedan.GetSystem()->GetTimerSetup();
        // double timer_update = my_sedan.GetSystem()->GetTimerUpdate();
        // std::cout << "timer_step: " << timer_step << " timer_advance: " << timer_advance << " timer_solve: " << 
                    // timer_solve << " timer_ls_setup: " << timer_ls_setup << " timer_collision: " << timer_collision <<
                    // " timer_setup: " << timer_setup << " timer_update: " << timer_update << std::endl;
        realtime_timer.Spin(step_size * 10);
        std::cout << "RTF: " << realtime_timer.RTF <<std::endl;
        last_time = cur_time;
        last_speed = cur_speed;
        std::cout << "==========end " <<  ++ num <<  " output======================" << std::endl;     
    }
    std::cout << "avg_time: " << avg_time / id << "ms " << std::endl;

    return 0;
}