// =============================================================================
// 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: Alessandro Tasora, Radu Serban
// =============================================================================
//
// FEA for 3D beams of 'cable' type (ANCF gradient-deficient beams)
//
// =============================================================================
#define WRITE_OUTFILE 1
#include <ctime>
#include "chrono/physics/ChSystemSMC.h"
#include "chrono/solver/ChDirectSolverLS.h"
#include "chrono/solver/ChIterativeSolverLS.h"
#include "chrono/timestepper/ChTimestepper.h"

#include "chrono_irrlicht/ChVisualSystemIrrlicht.h"

#include "myFEAcables.h"

using namespace chrono;
using namespace chrono::fea;
using namespace chrono::irrlicht;

using namespace irr;

// Select solver type (SPARSE_QR, SPARSE_LU, or MINRES).
ChSolver::Type solver_type = ChSolver::Type::MINRES;

int main(int argc, char* argv[]) {
    GetLog() << "Copyright (c) 2017 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";

    // Settings
    bool do_animate = true;
    double current_time = 0;
    double end_time = 16;
    double time_step = 0.0002;
    int number_of_chains = 10;
    int segments_per_chain = 10;

    // Create a Chrono::Engine physical system
    ChSystemSMC sys;

    // Create a mesh, that is a container for groups of elements and
    // their referenced nodes.
    auto mesh = chrono_types::make_shared<ChMesh>();

    // Create one of the available models (defined in FEAcables.h)
    //auto model = Model1(sys, mesh);
    auto model = Model6(sys, mesh, number_of_chains, segments_per_chain);
    //auto model = Model5(sys, mesh, number_of_chains, segments_per_chain);
    //auto model = Model3(sys, mesh, number_of_chains);
    // Remember to add the mesh to the system!
    sys.Add(mesh);

    // Visualization of the FEM mesh.
    // This will automatically update a triangle mesh (a ChTriangleMeshShape asset that is internally managed) by
    // setting  proper coordinates and vertex colors as in the FEM elements. Such triangle mesh can be rendered by
    // Irrlicht or POVray or whatever postprocessor that can handle a colored ChTriangleMeshShape).
    //auto vis_beam_A = chrono_types::make_shared<ChVisualShapeFEA>(mesh);
    //vis_beam_A->SetFEMdataType(ChVisualShapeFEA::DataType::ELEM_BEAM_MZ);
    //vis_beam_A->SetColorscaleMinMax(-0.4, 0.4);
    //vis_beam_A->SetSmoothFaces(true);
    //vis_beam_A->SetWireframe(true);
    //mesh->AddVisualShapeFEA(vis_beam_A);

    //auto vis_beam_B = chrono_types::make_shared<ChVisualShapeFEA>(mesh);
    //vis_beam_B->SetFEMglyphType(ChVisualShapeFEA::GlyphType::NODE_DOT_POS);
    //vis_beam_B->SetFEMdataType(ChVisualShapeFEA::DataType::NONE);
    //vis_beam_B->SetSymbolsThickness(0.06);
    //vis_beam_B->SetSymbolsScale(0.01);
    //vis_beam_B->SetZbufferHide(false);
    //mesh->AddVisualShapeFEA(vis_beam_B);

    // Set solver and solver settings
    switch (solver_type) {
    case ChSolver::Type::SPARSE_QR: {
        std::cout << "Using SparseQR solver" << std::endl;
        auto solver = chrono_types::make_shared<ChSolverSparseQR>();
        sys.SetSolver(solver);
        solver->UseSparsityPatternLearner(true);
        solver->LockSparsityPattern(true);
        solver->SetVerbose(false);
        break;
    }
    case ChSolver::Type::SPARSE_LU: {
        std::cout << "Using SparseLU solver" << std::endl;
        auto solver = chrono_types::make_shared<ChSolverSparseLU>();
        sys.SetSolver(solver);
        solver->UseSparsityPatternLearner(true);
        solver->LockSparsityPattern(true);
        solver->SetVerbose(false);
        break;
    }
    case ChSolver::Type::MINRES: {
        std::cout << "Using MINRES solver" << std::endl;
        auto solver = chrono_types::make_shared<ChSolverMINRES>();
        sys.SetSolver(solver);
        solver->SetMaxIterations(200);
        solver->SetTolerance(1e-10);
        solver->EnableDiagonalPreconditioner(true);
        solver->EnableWarmStart(true);  // IMPORTANT for convergence when using EULER_IMPLICIT_LINEARIZED
        solver->SetVerbose(false);
        break;
    }
    default: {
        std::cout << "Solver type not supported." << std::endl;
        break;
    }
    }

    // Create the Irrlicht visualization system
    //auto vis = chrono_types::make_shared<ChVisualSystemIrrlicht>();
    //vis->SetWindowSize(800, 600);
    //vis->SetWindowTitle("Cables FEM");
    //vis->Initialize();
    //vis->AddLogo();
    //vis->AddSkyBox();
    //vis->AddTypicalLights();
    ////vis->AddCamera(ChVector<>(0, 0.6, -1.0));
    //vis->AddCamera(ChVector<>(0, -1.0, 0.0), ChVector<>(0,0,0));
    //sys.SetVisualSystem(vis);

    // Set integrator
    //sys.SetTimestepperType(ChTimestepper::Type::EULER_IMPLICIT_LINEARIZED);
    sys.SetTimestepperType(ChTimestepper::Type::RUNGEKUTTA45);
    //sys.SetTimestepperType(ChTimestepper::Type::NEWMARK);
    //if (auto mystepper = std::dynamic_pointer_cast<ChTimestepperNewmark>(sys.GetTimestepper())) {
    //    mystepper->SetGammaBeta(0.5, 0.5);
    //}
 
    //sys.SetTimestepperType(ChTimestepper::Type::HHT);
    //if (auto mystepper = std::dynamic_pointer_cast<ChTimestepperHHT>(sys.GetTimestepper())) {
    //    mystepper->SetAlpha(-0.2);
    //    mystepper->SetMinStepSize(0.00000001);
    //    mystepper->SetStepIncreaseFactor(1.2);
    //    mystepper->SetStepDecreaseFactor(0.8);
    //}
    sys.SetSolverForceTolerance(1e-13);
    std::cout << "Num threads: " << sys.GetNumThreadsChrono() << std::endl;
    sys.SetNumThreads(36);
    std::cout << "Num threads: " << sys.GetNumThreadsChrono() << std::endl;
    //std::system("pause");
    time_t start = time(nullptr);
    ofstream outfile;
    int step = 0;
    int frames_per_sec = 30;
    int output_step = (end_time / time_step) / (end_time * frames_per_sec); // Output so as to have frames_per_sec outputs
    while (current_time < end_time) {
        //vis->BeginScene();
        //vis->DrawAll();
        //vis->EndScene();
        sys.DoStepDynamics(time_step);
        current_time += time_step;
        std::cout << "Simulation Time " << current_time << '\n';
        
#ifdef WRITE_OUTFILE
        // Print node positions:
        if (step % output_step == 0) {
            outfile.open("longline.out", std::ios_base::app);
            outfile << current_time << "\t";
            for (unsigned int inode = 0; inode < mesh->GetNnodes(); ++inode) {
                if (auto mnode = std::dynamic_pointer_cast<ChNodeFEAxyz>(mesh->GetNode(inode))) {
                    /*if (mnode->GetPos().x() < 0.01) {
                        GetLog() << "Node at y=" << mnode->GetPos().y() << " has T=" << mnode->GetP() << "\n";
                    }*/
                    outfile << "(" << mnode->GetPos().x() << ", " << mnode->GetPos().y() << ", " << mnode->GetPos().z() << ")\t";
                }
            }
            outfile << "\n";
            outfile.close();
        }
#endif

        ++step;
        //std::cout << "Wall Clock time: " << clock() / CLOCKS_PER_SEC << " seconds" << std::endl;
    }
    time_t end = time(nullptr);
    std::cout << "Finished time: " << end - start << " seconds" << std::endl;
    std::system("pause");
    return 0;
}