/*
==============================================================================
KratosStructuralApplication
A library based on:
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).

Copyright 2007
Pooyan Dadvand, Riccardo Rossi, Janosch Stascheit, Felix Nagel
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
janosch.stascheit@rub.de
nagel@sd.rub.de
- CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain
- Ruhr-University Bochum, Institute for Structural Mechanics, Germany

Permission is hereby granted, free  of charge, to any person obtaining
a  copy  of this  software  and  associated  documentation files  (the
"Software"), to  deal in  the Software without  restriction, including
without limitation  the rights to  use, copy, modify,  merge, publish,
distribute,  sublicense and/or  sell copies  of the  Software,  and to
permit persons to whom the Software  is furnished to do so, subject to
the following condition:

Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNERS.

The  above  copyright  notice  and  this permission  notice  shall  be
included in all copies or substantial portions of the Software.

THE  SOFTWARE IS  PROVIDED  "AS  IS", WITHOUT  WARRANTY  OF ANY  KIND,
EXPRESS OR  IMPLIED, INCLUDING  BUT NOT LIMITED  TO THE  WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT  SHALL THE AUTHORS OR COPYRIGHT HOLDERS  BE LIABLE FOR ANY
CLAIM, DAMAGES OR  OTHER LIABILITY, WHETHER IN AN  ACTION OF CONTRACT,
TORT  OR OTHERWISE, ARISING  FROM, OUT  OF OR  IN CONNECTION  WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

==============================================================================
*/
/* *********************************************************
*
*   Last Modified by:    $Author: janosch $
*   Date:                $Date: 2009-01-14 09:23:49 $
*   Revision:            $Revision: 1.5 $
*
* ***********************************************************/

// System includes

// External includes

// Project includes
// #include "includes/define.h"
#include "custom_elements/kinematic_linear.h"
#include "utilities/math_utils.h"
#include "custom_utilities/sd_math_utils.h"

#include <boost/timer.hpp>

namespace Kratos
{
    namespace KinematicLinearAuxiliaries
    {
        Matrix msB(0,0);
#ifdef _OPENMP
#pragma omp threadprivate(msB)
#endif
        Matrix msTanC(0,0);
#ifdef _OPENMP
#pragma omp threadprivate(msTanC)
#endif
        Vector msStrainVector(0);
#ifdef _OPENMP
#pragma omp threadprivate(msStrainVector)
#endif
        Vector msStressVector(0);
#ifdef _OPENMP
#pragma omp threadprivate(msStressVector)
#endif
        Matrix msDN_DX(0,0);
#ifdef _OPENMP
#pragma omp threadprivate(msDN_DX)
#endif
        Matrix msCurrentDisp(0,0);
#ifdef _OPENMP
#pragma omp threadprivate(msCurrentDisp)
#endif
    }

    using namespace KinematicLinearAuxiliaries;

    KinematicLinear::KinematicLinear(IndexType NewId,
            GeometryType::Pointer pGeometry)
    : Element(NewId, pGeometry)
    {
        //DO NOT ADD DOFS HERE!!!
        //THIS IS THE DEFAULT CONSTRUCTOR
    }

    /**
    * A simple kinematic linear 3D element for the solution
    * of the momentum balance in structural mechanics.
    * This element is used for students training at the Ruhr University Bochum.
    * Therefore it may includes comments that are obvious for the
    * experienced user.
    */
    KinematicLinear::KinematicLinear(IndexType NewId,
            GeometryType::Pointer pGeometry,  PropertiesType::Pointer pProperties)
    : Element(NewId, pGeometry, pProperties)
    {
        //DEFINE HERE THE INTEGRATION METHOD (the number of integration points to be used)
        //see /kratos/source/integration_rules.cpp for further details
        mThisIntegrationMethod= GeometryData::GI_GAUSS_1;//default method
        //m... declares that this is a member variable initialized in the header file
        //select Integration Method for 8 node hexahedron
        if(GetGeometry().size()== 8 )
            mThisIntegrationMethod= GeometryData::GI_GAUSS_3;//methods for hexahedra elements

        //select Integration Method for 27 and 20 nodes
        if(GetGeometry().size()== 27 || GetGeometry().size()== 20)
            mThisIntegrationMethod= GeometryData::GI_GAUSS_3;//methods for hexahedra elements

        //select Integration Method for 10 and 4 node tetrahedron
        if(GetGeometry().size()== 4 || GetGeometry().size()== 10)
            mThisIntegrationMethod= GeometryData::GI_GAUSS_5;//methods for tetrahedra elements
    }

    Element::Pointer KinematicLinear::Create(IndexType NewId,
            NodesArrayType const& ThisNodes,  PropertiesType::Pointer pProperties) const
    {
        return Element::Pointer(new KinematicLinear(NewId, GetGeometry().Create(ThisNodes),
                                pProperties));
    }

	//************************************************************************************
	//************************************************************************************
	//THIS IS THE DESTRUCTOR, as we use UBLAS classes, we don't have to care about garbage collecting
	//************************************************************************************
	//************************************************************************************
    KinematicLinear::~KinematicLinear()
    {
    }

    void KinematicLinear::ResizeAndInitializeAuxiliaries()
    {
        unsigned int dimension = GetGeometry().WorkingSpaceDimension();
        unsigned int dim2 = GetGeometry().size()*dimension;
        unsigned int StrainSize;
        if (dimension==3)
            StrainSize = 6;
        else
            KRATOS_ERROR(std::logic_error, "KinematicLinear cannot be used for other than 3D problems" , "");
        if(msB.size2() != dim2)
        {
            msB.resize(StrainSize,dim2,false);
            msTanC.resize(StrainSize,StrainSize,false);
            msStrainVector.resize(StrainSize,false);
            msStressVector.resize(StrainSize,false);
            msDN_DX.resize(GetGeometry().size(),dimension,false);
            msCurrentDisp.resize(GetGeometry().size(),dimension,false);
        }
    }

	/**
    * Initialization of the element, called at the begin of each simulation.
    * Membervariables and the Material law are initialized here
    */
    void KinematicLinear::Initialize()
    {
        KRATOS_TRY//EXCEPTION HANDLING (see corresponing KRATOS_CATCH("") )

        //dimension of the problem
		unsigned int dim = GetGeometry().WorkingSpaceDimension();
        //number of integration points used, mThisIntegrationMethod refers to the
        //integration method defined in the constructor
       	const GeometryType::IntegrationPointsArrayType& integration_points =
                GetGeometry().IntegrationPoints(mThisIntegrationMethod);

        //initializing the Jacobian, the inverse Jacobian and Jacobians determinant in the reference
        // configuration
        GeometryType::JacobiansType J0(integration_points.size());
        mInvJ0.resize(integration_points.size());

        for(unsigned int i=0; i< integration_points.size(); i++)
        {
            mInvJ0[i].resize(dim,dim,false);
            noalias(mInvJ0[i])= ZeroMatrix(dim,dim);
        }

        mDetJ0.resize(integration_points.size(),false);
        noalias(mDetJ0)= ZeroVector(integration_points.size());

        //calculating the Jacobian
        J0 = GetGeometry().Jacobian(J0, mThisIntegrationMethod);

        //calculating the inverse Jacobian
        for(unsigned int PointNumber = 0; PointNumber<integration_points.size(); PointNumber++)
        {
            //calculating and storing inverse of the jacobian and the parameters needed
            MathUtils<double>::InvertMatrix(J0[PointNumber],mInvJ0[PointNumber],mDetJ0[PointNumber]);
        }

        //SetUpInitialisplacement for StressFreeActivation of Elements
        mInitialDisp.resize(GetGeometry().size(),dim,false);
        for(unsigned int node = 0; node< GetGeometry().size(); node++)
                for(unsigned int i = 0; i< 3; i++)
                        mInitialDisp(node, i)= GetGeometry()[node].GetSolutionStepValue(DISPLACEMENT)[i];
        //Initialization of the constitutive law vector and
        // declaration, definition and initialization of the material
        // lwas at each integration point
        if(mConstitutiveLawVector.size() != integration_points.size())
        {
            mConstitutiveLawVector.resize(integration_points.size());
            InitializeMaterial();
        }

//just for testing the unsaturated soil element
for(unsigned int i=0; i< GetGeometry().size(); i++)
{
        GetGeometry()[i].GetSolutionStepValue(TEMPERATURE)= 0.0;
}

        KRATOS_CATCH("")
    }

	/**
    * Calculate double Variables at each integration point, used for postprocessing etc.
    * @param rVariable Global name of the variable to be calculated
    * @param output Vector to store the values on the qudrature points, output of the method
    * @param rCurrentProcessInfo
    */
    void KinematicLinear::CalculateOnIntegrationPoints(const Variable<double>& rVariable, Vector& Output,
            const ProcessInfo& rCurrentProcessInfo)
    {
        if(Output.size() != GetGeometry().IntegrationPoints(mThisIntegrationMethod).size())
            Output.resize(GetGeometry().IntegrationPoints(mThisIntegrationMethod).size(),false);
        for(unsigned int ii = 0; ii<mConstitutiveLawVector.size(); ii++)
            Output[ii] = mConstitutiveLawVector[ii]->GetValue( rVariable );
    }

    /**
    * Calculate Vector Variables at each integration point, used for postprocessing etc.
    * @param rVariable Global name of the variable to be calculated
    * @param output Vector to store the values on the qudrature points, output of the method
    * @param rCurrentProcessInfo
    */
    void KinematicLinear::CalculateOnIntegrationPoints(const Variable<Vector>& rVariable,
            std::vector<Vector>& Output, const ProcessInfo& rCurrentProcessInfo)
    {
        if(Output.size() != GetGeometry().IntegrationPoints(mThisIntegrationMethod).size())
            Output.resize(GetGeometry().IntegrationPoints(mThisIntegrationMethod).size());
        for(unsigned int ii = 0; ii<mConstitutiveLawVector.size(); ii++)
            Output[ii] = mConstitutiveLawVector[ii]->GetValue( rVariable );
    }

    /**
    * Calculate Matrix Variables at each integration point, used for postprocessing etc.
    * @param rVariable Global name of the variable to be calculated
    * @param output Vector to store the values on the qudrature points, output of the method
    * @param rCurrentProcessInfo
    */
    void KinematicLinear::CalculateOnIntegrationPoints(const Variable<Matrix>& rVariable,
            std::vector<Matrix>& Output, const ProcessInfo& rCurrentProcessInfo)
    {
        KRATOS_TRY

//         unsigned int number_of_nodes = GetGeometry().size();
//         unsigned int dim= 3;

        ResizeAndInitializeAuxiliaries();

//         std::cout << "Calculating Values on integration points in element: " << Id() <<std::endl;

        //reading integration points and local gradients
        const GeometryType::IntegrationPointsArrayType& integration_points =
                GetGeometry().IntegrationPoints(mThisIntegrationMethod);
        const Matrix& Ncontainer = GetGeometry().ShapeFunctionsValues(mThisIntegrationMethod);

        if(Output.size() != integration_points.size())
            Output.resize(integration_points.size());

        const GeometryType::ShapeFunctionsGradientsType& DN_De =
                GetGeometry().ShapeFunctionsLocalGradients(mThisIntegrationMethod);

        //Current displacements
        for( unsigned int node=0; node < GetGeometry().size(); node++ )
        {
            msCurrentDisp(node,0) = GetGeometry()[node].GetSolutionStepValue( DISPLACEMENT_X );
            msCurrentDisp(node,1) = GetGeometry()[node].GetSolutionStepValue( DISPLACEMENT_Y );
            msCurrentDisp(node,2) = GetGeometry()[node].GetSolutionStepValue( DISPLACEMENT_Z );
        }

        //Declaration of the integration weight
        double Weight;

        //loop over all integration points
        for(unsigned int PointNumber = 0; PointNumber<integration_points.size(); PointNumber++)
        {
//             noalias(msDN_DX)= ZeroMatrix(number_of_nodes, dim);
            noalias(msDN_DX)= prod(DN_De[PointNumber],mInvJ0[PointNumber]);
            //Initializing B_Operator at the current integration point
            CalculateBoperator(msB, msDN_DX);

            //calculate strain
            CalculateStrain(msB, msCurrentDisp, msStrainVector);
            //assign the integration weight at the current integration point
            Weight= integration_points[PointNumber].Weight();

            //calculate stress
            mConstitutiveLawVector[PointNumber]->UpdateMaterial( msStrainVector,
                    GetProperties(), GetGeometry(), row(Ncontainer,PointNumber), rCurrentProcessInfo );
            mConstitutiveLawVector[PointNumber]->CalculateStress(msStrainVector, msStressVector);

            if(Output[PointNumber].size2() != msStrainVector.size())
                Output[PointNumber].resize(1,msStrainVector.size(),false);

            if(rVariable==GREEN_LAGRANGE_STRAIN_TENSOR)
            {
                for(unsigned int ii = 0; ii<msStrainVector.size(); ii++)
                    Output[PointNumber](0,ii) = msStrainVector[ii];
            }
            else if(rVariable==PK2_STRESS_TENSOR)
            {
                for(unsigned int ii = 0; ii<msStrainVector.size(); ii++)
                    Output[PointNumber](0,ii) = msStressVector[ii];
            }
            else if(rVariable==INSITU_STRESS)
            {
                row(Output[PointNumber],0) = mConstitutiveLawVector[PointNumber]->GetValue(INSITU_STRESS);
            }
//             std::cout << msStressVector[2] << "\t";
        }
//         std::cout << std::endl;
        KRATOS_CATCH("")
    }
	/**
    * Initialization of the Material law at each integration point
    */
    void KinematicLinear::InitializeMaterial()
    {
        KRATOS_TRY
        for( unsigned int i=0; i<mConstitutiveLawVector.size(); i++ )
        {
            mConstitutiveLawVector[i] = GetProperties()[CONSTITUTIVE_LAW]->Clone();
            mConstitutiveLawVector[i]->InitializeMaterial(GetProperties(), GetGeometry(), row(GetGeometry().ShapeFunctionsValues(mThisIntegrationMethod), i) );
        }
		KRATOS_CATCH("")
    }
	/**
    * THIS is the main method here the integration in space (loop over the integration points) is done,
    * the algorithmic tangent and the (inner and outer) load vector is computed
    * @param rLeftHandSideMatrix algorithmic tangent, size (number_of_nodes*dim)*(number_of_nodes*dim)
    * @param rRightHandSideVector (inner and outer) load vector, size (number_of_nodes*dim)
    * @param rCurrentProcessInfo
    * @param CalculateStiffnessMatrixFlag true: algorithmic tangent has to be computed
    * @param CalculateResidualVectorFlag true: load vector has to be computed
    */
    void KinematicLinear::CalculateAll(MatrixType& rLeftHandSideMatrix,
            VectorType& rRightHandSideVector,ProcessInfo& rCurrentProcessInfo,
            bool CalculateStiffnessMatrixFlag,bool CalculateResidualVectorFlag)
    {
         KRATOS_TRY

//          boost::timer element_time;

//          int counter = 0;

//          for( int i=0; i<1000; i++ )
//          {

         unsigned int number_of_nodes = GetGeometry().size();
         unsigned int dim = GetGeometry().WorkingSpaceDimension();

         ResizeAndInitializeAuxiliaries();

         //this is the size of the elements stiffness matrix/force vector
         unsigned int MatSize=(number_of_nodes*dim);

         if (CalculateStiffnessMatrixFlag == true) //calculation of the matrix is required
         {
             //resize the LHS=StiffnessMatrix if its size is not correct
             if(rLeftHandSideMatrix.size1() != MatSize)
                 rLeftHandSideMatrix.resize(MatSize,MatSize,false);
             noalias(rLeftHandSideMatrix) = ZeroMatrix(MatSize,MatSize); //resetting LHS
         }

         //resizing as needed the RHS
         if (CalculateResidualVectorFlag == true) //calculation of the matrix is required
         {
             //resize the RHS=force vector if its size is not correct
             if(rRightHandSideVector.size() != MatSize)
                 rRightHandSideVector.resize(MatSize,false);
             noalias(rRightHandSideVector) = ZeroVector(MatSize); //resetting RHS
         }

         //reading integration points and local gradients
         const GeometryType::IntegrationPointsArrayType& integration_points = GetGeometry().IntegrationPoints(mThisIntegrationMethod);
         const GeometryType::ShapeFunctionsGradientsType& DN_De = GetGeometry().ShapeFunctionsLocalGradients(mThisIntegrationMethod);
         const Matrix& Ncontainer = GetGeometry().ShapeFunctionsValues(mThisIntegrationMethod);

         //Current displacements
         for( unsigned int node=0; node < GetGeometry().size(); node++ )
         {
             msCurrentDisp(node,0) = GetGeometry()[node].GetSolutionStepValue( DISPLACEMENT_X );
             msCurrentDisp(node,1) = GetGeometry()[node].GetSolutionStepValue( DISPLACEMENT_Y );
             msCurrentDisp(node,2) = GetGeometry()[node].GetSolutionStepValue( DISPLACEMENT_Z );
         }
//         Vector N_DISP(number_of_nodes);

        //Declaration of the integration weight
        double Weight;

        //Body force
//         Vector BodyForce;

        //Declaration of the Strain Tensor at current integration point (3 times 3)
//         Vector StrainVector;

        //Declaration of the Stress Tensor at current integration point (3 times 3)
//         Vector StressVector;

        //Initialization of Jacobian at current integration point
//         double DetJ=0.0;

        /////////////////////////////////////////////////////////////////////////
        //// Integration in space over quadrature points
        /////////////////////////////////////////////////////////////////////////
        for(unsigned int PointNumber = 0; PointNumber<integration_points.size(); PointNumber++)
        {

			noalias(msDN_DX)= prod(DN_De[PointNumber],mInvJ0[PointNumber]);
            //Initializing B_Operator at the current integration point
            CalculateBoperator(msB, msDN_DX);

            //calculate strain
            CalculateStrain(msB, msCurrentDisp, msStrainVector);
			//assign the integration weight at the current integration point
            Weight= integration_points[PointNumber].Weight();

            //calculate stress
            mConstitutiveLawVector[PointNumber]->UpdateMaterial( msStrainVector,
                    GetProperties(), GetGeometry(), row(Ncontainer,PointNumber), rCurrentProcessInfo );
            mConstitutiveLawVector[PointNumber]->CalculateStress(msStrainVector, msStressVector);

            if (CalculateStiffnessMatrixFlag == true) //calculation of the matrix is required
            {
                //calculate material tangent
                mConstitutiveLawVector[PointNumber]->CalculateConstitutiveMatrix(msStrainVector, msTanC);
                //calculate stiffness matrix
                noalias(rLeftHandSideMatrix) +=
                        prod(trans(msB),(Weight*mDetJ0[PointNumber])*Matrix(prod(msTanC,msB)) );
                //CalculateStiffnesMatrix(rLeftHandSideMatrix, msTanC, msB, Weight, mDetJ0[PointNumber]);
            }
            if (CalculateResidualVectorFlag == true)
            {
//                 //contribution to external forces
//                 BodyForce = GetProperties()[BODY_FORCE];
                //Calculation of Loadvector
                AddBodyForcesToRHS(rRightHandSideVector, row(Ncontainer,PointNumber), Weight, mDetJ0[PointNumber] );
                AddInternalForcesToRHS(rRightHandSideVector, msB, msStressVector, Weight, mDetJ0[PointNumber]);
            }

			///////////////////////////////////////////////////////////////////////
			// END Integration in space sum_(beta=0)^(number of quadrature points)
			///////////////////////////////////////////////////////////////////////
        }
// KRATOS_WATCH(rRightHandSideVector);
// KRATOS_WATCH(rLeftHandSideMatrix);

//         counter++;

//        }//end of repetition
//         std::cout << "Element Time x " <<counter << ": " << element_time.elapsed() << std::endl;
//         std::cout << "number of integration points: " << GetGeometry().IntegrationPoints(mThisIntegrationMethod).size() << std::endl;

        KRATOS_CATCH("")
    }
	/**
    * THIS method is called from the scheme during the iteration loop, it calls the CalculateAll()
    * method with CalculateStiffnessMatrixFlag = false and CalculateResidualVectorFlag = true
    * @param rRightHandSideVector (inner and outer) load vector, size (number_of_nodes*dim)
    * @param rCurrentProcessInfo
    */
     void KinematicLinear::CalculateRightHandSide(VectorType& rRightHandSideVector,
             ProcessInfo& rCurrentProcessInfo)
      {
	   //calculation flags
           bool CalculateStiffnessMatrixFlag = false;
           bool CalculateResidualVectorFlag = true;
           MatrixType temp = Matrix();

           CalculateAll(temp, rRightHandSideVector, rCurrentProcessInfo, CalculateStiffnessMatrixFlag,  CalculateResidualVectorFlag);
       }
	/**
    * THIS method is called from the scheme during the iteration loop, it calls the CalculateAll()
    * method with CalculateStiffnessMatrixFlag = true and CalculateResidualVectorFlag = true
    * @param rLeftHandSideMatrix algorithmic tangent, size (number_of_nodes*dim)*(number_of_nodes*dim)
    * @param rRightHandSideVector (inner and outer) load vector, size (number_of_nodes*dim)
    * @param rCurrentProcessInfo
    */
    void KinematicLinear::CalculateLocalSystem(MatrixType& rLeftHandSideMatrix,
                VectorType& rRightHandSideVector, ProcessInfo& rCurrentProcessInfo)
    {
	   //calculation flags
            bool CalculateStiffnessMatrixFlag = true;
            bool CalculateResidualVectorFlag = true;
            CalculateAll(rLeftHandSideMatrix, rRightHandSideVector, rCurrentProcessInfo,
                     CalculateStiffnessMatrixFlag,CalculateResidualVectorFlag);
    }
	/**
    * THIS method is called from the scheme after each solution step, here the time step
    * start and end point variables can be transferred n --> n+1
    * @param rCurrentProcessInfo
    */
    void KinematicLinear::FinalizeSolutionStep(ProcessInfo& CurrentProcessInfo)
    {
         //reading integration points and local gradients
         const GeometryType::IntegrationPointsArrayType& integration_points = GetGeometry().IntegrationPoints(mThisIntegrationMethod);

         for(unsigned int Point=0; Point< integration_points.size(); Point++)
         {
            mConstitutiveLawVector[Point]->FinalizeSolutionStep(GetProperties(), GetGeometry(), row( GetGeometry().ShapeFunctionsValues(mThisIntegrationMethod),Point), CurrentProcessInfo);
         }
//                 Matrix Dummy_Matrix(3,3);
//                 noalias(Dummy_Matrix)= mConstitutiveLawVector[0]->GetValue(PK2_STRESS_TENSOR);
                Vector Dummy_Vector(5);
                noalias(Dummy_Vector)= mConstitutiveLawVector[0]->GetValue(INTERNAL_VARIABLES);
                for(unsigned int i=0; i< GetGeometry().size(); i++)
                {
                        GetGeometry()[i].GetSolutionStepValue(MOMENTUM_X)= Dummy_Vector(0);
                        GetGeometry()[i].GetSolutionStepValue(MOMENTUM_Y)= Dummy_Vector(1);
                        GetGeometry()[i].GetSolutionStepValue(MOMENTUM_Z)= Dummy_Vector(2);
                        GetGeometry()[i].GetSolutionStepValue(PRESSURE)= Dummy_Vector(3);
                        GetGeometry()[i].GetSolutionStepValue(ERROR_RATIO)= Dummy_Vector(4);
                }

     }

        /**
    * THIS method is called from the scheme at the start of each solution step
    * @param rCurrentProcessInfo
    */
    void KinematicLinear::InitializeSolutionStep(ProcessInfo& CurrentProcessInfo)
    {
         //reading integration points and local gradients
         const GeometryType::IntegrationPointsArrayType& integration_points = GetGeometry().IntegrationPoints(mThisIntegrationMethod);

         for(unsigned int Point=0; Point< integration_points.size(); Point++)
         {
            mConstitutiveLawVector[Point]->InitializeSolutionStep(GetProperties(), GetGeometry(), row( GetGeometry().ShapeFunctionsValues(mThisIntegrationMethod),Point), CurrentProcessInfo);

//            mConstitutiveLawVector[Point]->SetValue(SUCTION, GetGeometry()[0].GetSolutionStepValue(TEMPERATURE),CurrentProcessInfo );
         }
    }
	/**
    * returns the used integration method
    */
	KinematicLinear::IntegrationMethod KinematicLinear::GetIntegrationMethod()
	{
			return mThisIntegrationMethod;
	}
	/**
    * not used
    */
    void KinematicLinear::CalculateAndAddExtForceContribution(const Vector& N,
                const ProcessInfo& CurrentProcessInfo, Vector& BodyForce, VectorType& rRightHandSideVector,
                double weight)
    {
            KRATOS_TRY

            KRATOS_CATCH("")
    }
	/**
    * Informations for the builder and solver to assemble the global vectors and matrices.
    * Here a Vector containing the EquationIds of the differnt Dofs is created
    * @param rResult Vector of the EquationIds
    * @param rCurrentProcessInfo
    */
    void KinematicLinear::EquationIdVector(EquationIdVectorType& rResult,
                ProcessInfo& CurrentProcessInfo)
    {
        unsigned int dim = (GetGeometry().WorkingSpaceDimension());//3 displacement dofs
        unsigned int MatSize = GetGeometry().size()*dim;
        if(rResult.size() != MatSize)
            rResult.resize(MatSize,false);

        for (unsigned int i=0 ;i<GetGeometry().size() ;i++)
        {
                    int index = i*dim;
                    rResult[index] = GetGeometry()[i].GetDof(DISPLACEMENT_X).EquationId();
                    rResult[index+1] = GetGeometry()[i].GetDof(DISPLACEMENT_Y).EquationId();
                    rResult[index+2] = GetGeometry()[i].GetDof(DISPLACEMENT_Z).EquationId();
         }
     }
	/**
    * Informations for the builder and solver to assemble the global vectors and matrices.
    * Here a Container containing the pointers rto the DOFs of this element is created
    * @param ElementalDofList Container with of the DOFs associated with the nodes
    *                           of this element
    * @param rCurrentProcessInfo
    */
    void KinematicLinear::GetDofList(DofsVectorType& ElementalDofList,ProcessInfo&
                CurrentProcessInfo)
    {
        ElementalDofList.resize(0);

        for (unsigned int i=0 ;i<GetGeometry().size() ;i++)
        {
            ElementalDofList.push_back(GetGeometry()[i].pGetDof(DISPLACEMENT_X));
            ElementalDofList.push_back(GetGeometry()[i].pGetDof(DISPLACEMENT_Y));
            ElementalDofList.push_back(GetGeometry()[i].pGetDof(DISPLACEMENT_Z));
        }
     }
	/**
    * A Vector containing the current values of the DOFs is created
    * @param values Vector with the current values of the DOFs
    * @param Step number of the time step
    */
    void KinematicLinear::GetValuesVector(Vector& values, int Step)
    {
        unsigned int dim = (GetGeometry().WorkingSpaceDimension());//3 displacement dofs
        unsigned int MatSize = GetGeometry().size()*dim;

        if(values.size() != MatSize)
            values.resize(MatSize);

        for (unsigned int i=0 ;i<GetGeometry().size();i++)
        {
            int index = i*dim;

            values(index) =
                GetGeometry()[i].GetSolutionStepValue(DISPLACEMENT_X,Step);
            values(index+1) =
                GetGeometry()[i].GetSolutionStepValue(DISPLACEMENT_Y,Step);
            values(index+2) =
                GetGeometry()[i].GetSolutionStepValue(DISPLACEMENT_Z,Step);
        }
    }
	/**
    * Adds the Body Forces to the load vector
    * @param R RHS Vector
    * @param N_DISP shape function values at the current integration points
    * @param Weight current integration weight
    * @param detJ current Determinant of the Jacobian
    */
    void KinematicLinear::AddBodyForcesToRHS(Vector& R, const Vector& N_DISP, double Weight, double detJ )
    {
        KRATOS_TRY

        unsigned int dim= GetGeometry().WorkingSpaceDimension();

        Vector gravity(dim);

        noalias(gravity)=GetProperties()[GRAVITY];

        double density= GetProperties()[DENSITY];

        for(unsigned int prim=0; prim< GetGeometry().size(); prim++)
        {
            for(unsigned int i=0; i< dim; i++)
            {
                R(prim*dim+i) += N_DISP(prim)*density*gravity(i)*detJ*Weight;
            }
        }

        KRATOS_CATCH("")
    }
	/**
    * Adds the Internal Forces to the load vector
    * @param R RHS Vector
    * @param B_Operator B-Operator at the current integration point
    * @param StressVector current stress vector
    * @param Weight current integration weight
    * @param detJ current Determinant of the Jacobian
    */
    void KinematicLinear::AddInternalForcesToRHS(Vector& R, const Matrix& B_Operator, Vector& StressVector, double Weight, double detJ)
    {
        KRATOS_TRY

        unsigned int dim= GetGeometry().WorkingSpaceDimension();

        for(unsigned int prim=0; prim< GetGeometry().size(); prim++)
        {
            for(unsigned int i=0; i< dim; i++)
            {
                for(unsigned int gamma=0; gamma<6; gamma++)
                {
                    R(prim*dim+i) +=(-1)*(B_Operator(gamma,prim*3+i)*StressVector(gamma)*
                                        detJ*Weight);
                }
            }
        }

//         noalias(R) -= detJ*Weight* prod(trans(B_Operator),StressVector);

        KRATOS_CATCH("")
    }
	/**
    * Adds the Contribution of the current quadrature point to the load vector
    * @param K LHS Matrix
    * @param tan_C 6*6 algorithmic tangent of the materia law (derivation of stresses
    *               regarding strains
    * @param B_Operator B-Operator at the current integration point
    * @param Weight current integration weight
    * @param detJ current Determinant of the Jacobian
    */
    void KinematicLinear::CalculateStiffnesMatrix(Matrix& K,const
                Matrix& tan_C,const Matrix& B_Operator, double Weight, double detJ)
    {
        KRATOS_TRY

        unsigned int dim= GetGeometry().WorkingSpaceDimension();

        for(unsigned int prim=0; prim< GetGeometry().size(); prim++)
        {
            for(unsigned int i=0; i< dim; i++)
            {
                for(unsigned int sec=0; sec< GetGeometry().size(); sec++)
                {
                    for(unsigned int j=0; j< dim; j++)
                    {
                        for(unsigned int alpha=0; alpha<6; alpha++)
                            for(unsigned int beta=0; beta<6; beta++)
                                K(prim*dim+i,sec*dim+j) += B_Operator(alpha,3*prim+i)
                                    *tan_C(alpha,beta)*B_Operator(beta,3*sec+j)*detJ*Weight;
                    }
                }
            }
        }

//         noalias(K)-= prod(trans(B_Operator),(Weight*detJ)*Matrix(prod(C,B_Operator)) );

        KRATOS_CATCH("")
    }
	/**
    * Computes the stress vector and the algorithmic tangent at the current quadrature points
    * regarding the current strain vector
    * @param StressVector output of the method, Cauchy stress vector
    * @param tanC_U algorithmic tangen (derivative Cauchy stress vector/StrainVector) [6*6]
    * @param StrainVector output: current strain vector
    * @param B_Operator current B-operator
    * @param PointNumber number of the current integration point
    * @param CurrentProcessInfo
    */
    void KinematicLinear::CalculateStressAndTangentialStiffness(Vector& StressVector, Matrix& tanC_U,
            Vector& StrainVector, const Matrix& B_Operator, int PointNumber,
            const ProcessInfo& CurrentProcessInfo)
    {
        KRATOS_TRY

        //Calculate the current strain vector using the B-Operator
        //CalculateStrainVector(B_Operator, StrainVector, PointNumber);
        //Call the material law to get the current Stress Vector
        //mConstitutiveLawVector[PointNumber]->CalculateStress(StrainVector, StressVector);
        //Call the material law to get the current algorithmic tangent
        //mConstitutiveLawVector[PointNumber]->CalculateConstitutiveMatrix(StrainVector, tanC_U);

        KRATOS_CATCH("")
    }

    /**
     * Computes the strain vector
     */
    void KinematicLinear::CalculateStrain( const Matrix& B, const Matrix& Displacements, Vector& StrainVector )
    {
        KRATOS_TRY
        noalias(StrainVector)=ZeroVector(6);
        for(unsigned int node=0; node<GetGeometry().size(); node++)
        {
            for( unsigned int item = 0; item < 6; item++ )
                for( unsigned int dim=0; dim<3; dim++ )
                    StrainVector[item]+=B(item,3*node+dim)*(Displacements(node,dim)-mInitialDisp(node,dim));
        }
        KRATOS_CATCH("")
    }

	/**
    * Computes the B-Operator at the current quadrature point
    * @param B_Operator current B-operator
    * @param DN_DX shape function values at the current integration point
    */
    void KinematicLinear::CalculateBoperator
                   (Matrix& B_Operator, const Matrix& DN_DX)
    {
        KRATOS_TRY

        const unsigned int number_of_nodes = GetGeometry().PointsNumber();

//         if(B_Operator.size() != number_of_nodes)
//             B_Operator.resize(number_of_nodes);
        noalias(B_Operator)=ZeroMatrix(6,number_of_nodes*3);

        for (unsigned int i=0;i<number_of_nodes;i++)
		{
//             if(B_Operator[i].size1() != 6 || B_Operator[i].size2() != 3)
//                 B_Operator[i].resize(6,3);

//             noalias(B_Operator[i])= ZeroMatrix(6,3);

            B_Operator(0,i*3)= DN_DX(i,0);
            B_Operator(1,i*3+1)= DN_DX(i,1);
            B_Operator(2,i*3+2)= DN_DX(i,2);
            B_Operator(3,i*3)= DN_DX(i,1);
            B_Operator(3,i*3+1)= DN_DX(i,0);
            B_Operator(4,i*3+1)= DN_DX(i,2);
            B_Operator(4,i*3+2)= DN_DX(i,1);
            B_Operator(5,i*3)= DN_DX(i,2);
            B_Operator(5,i*3+2)= DN_DX(i,0);
        }

        return;

        KRATOS_CATCH("")
	}
	/**
    * Calculate Matrix Variables at each integration point, used for postprocessing etc.
    * @param rVariable Global name of the variable to be calculated
    * @param rValues Vector to store the values on the qudrature points, output of the method
    * @param rCurrentProcessInfo
    */
 	void KinematicLinear::GetValueOnIntegrationPoints(const Variable<Matrix>& rVariable, std::vector<Matrix>& rValues, const ProcessInfo& rCurrentProcessInfo)
	{
		if(rVariable== PK2_STRESS_TENSOR || rVariable == GREEN_LAGRANGE_STRAIN_TENSOR)
        {
            CalculateOnIntegrationPoints(rVariable, rValues, rCurrentProcessInfo);
        }
		return;
	}
	/**
    * Calculate Vector Variables at each integration point, used for postprocessing etc.
    * @param rVariable Global name of the variable to be calculated
    * @param rValues Vector to store the values on the qudrature points, output of the method
    * @param rCurrentProcessInfo
    */
	 void KinematicLinear::GetValueOnIntegrationPoints(const Variable<Vector>& rVariable, std::vector<Vector>& rValues, const ProcessInfo& rCurrentProcessInfo)
	{
// std::cout<<"GetValueOnIntegrationPoints"<<std::endl;
            if(rVariable==INSITU_STRESS)
            {
                for( unsigned int PointNumber = 0;
                     PointNumber < GetGeometry().IntegrationPoints(mThisIntegrationMethod).size();
                     PointNumber++ )
                {
                    rValues[PointNumber] =
                            mConstitutiveLawVector[PointNumber]->GetValue(INSITU_STRESS);
                }
            }
            if(rVariable==MATERIAL_PARAMETERS)
            {
                for( unsigned int PointNumber = 0;
                     PointNumber < GetGeometry().IntegrationPoints(mThisIntegrationMethod).size(); PointNumber++ )
                {
                    rValues[PointNumber] =
                            mConstitutiveLawVector[PointNumber]->GetValue(MATERIAL_PARAMETERS);
                }
            }
            if (rVariable==INTERNAL_VARIABLES)
            {
                for( unsigned int PointNumber = 0;
                     PointNumber<GetGeometry().IntegrationPoints(mThisIntegrationMethod).size();
                     PointNumber++ )
                {
                    rValues[PointNumber] =
                            mConstitutiveLawVector[PointNumber]->GetValue(INTERNAL_VARIABLES);
                }
            }
// std::cout<<"END::GetValueOnIntegrationPoints"<<std::endl;
	}
	/**
    * Calculate double Variables at each integration point, used for postprocessing etc.
    * @param rVariable Global name of the variable to be calculated
    * @param rValues Vector to store the values on the qudrature points, output of the method
    * @param rCurrentProcessInfo
    */
	 void KinematicLinear::GetValueOnIntegrationPoints(const Variable<double>& rVariable, std::vector<double>& rValues, const ProcessInfo& rCurrentProcessInfo)
	{
		if(!(rVariable== DP_EPSILON))
			return;
		if(rValues.size() != GetGeometry().IntegrationPoints().size())
		  	   rValues.resize(GetGeometry().IntegrationPoints().size());
        //reading integration points and local gradients
        for(unsigned int Point=0; Point< mConstitutiveLawVector.size(); Point++)
        {
			rValues[Point]= mConstitutiveLawVector[Point]->GetValue( rVariable);
        }
	}
	/**
    * Set a Matrix Variable from outside
    * @param rVariable Global name of the variable to be calculated
    * @param rValues Vector of the values on the quadrature points
    * @param rCurrentProcessInfo
    */
	void KinematicLinear::SetValueOnIntegrationPoints(const Variable<Matrix>& rVariable, std::vector<Matrix>& rValues, const ProcessInfo& rCurrentProcessInfo)
	{
		if(rValues.size() != mConstitutiveLawVector.size())
		{
			std::cout<<"In KinematicLinear:SetValueOnIntegrationPoints(...) Line 798, wrong size of the input vector, is = "<< rValues.size()<<" ; should be = "<<mConstitutiveLawVector.size()<<std::endl;
			return;
		}

		if(rVariable==ELASTIC_LEFT_CAUCHY_GREEN_OLD)
        {

			for( unsigned int i=0; i<mConstitutiveLawVector.size(); i++ )
            {
						mConstitutiveLawVector[i]->SetValue(ELASTIC_LEFT_CAUCHY_GREEN_OLD, rValues[i], rCurrentProcessInfo);
			}
		}

	}
	/**
    * Set a Vector Variable from outside
    * @param rVariable Global name of the variable to be calculated
    * @param rValues Vector of the values on the quadrature points
    * @param rCurrentProcessInfo
    */
        void KinematicLinear::SetValueOnIntegrationPoints(const Variable<Vector>& rVariable, std::vector<Vector>& rValues, const ProcessInfo& rCurrentProcessInfo)
        {
//              std::cout << mConstitutiveLawVector[0] << std::endl;
                if (rVariable==INSITU_STRESS)
                {
                    for( unsigned int PointNumber = 0; PointNumber<GetGeometry().IntegrationPoints(mThisIntegrationMethod).size(); PointNumber++ )
                        {
                                mConstitutiveLawVector[PointNumber]->SetValue(INSITU_STRESS, rValues[PointNumber],
                                                rCurrentProcessInfo );
                        }
                }
                if (rVariable==MATERIAL_PARAMETERS)
                {
                    for( unsigned int PointNumber = 0; PointNumber<GetGeometry().IntegrationPoints(mThisIntegrationMethod).size(); PointNumber++ )
                    {
                        mConstitutiveLawVector[PointNumber]->SetValue( MATERIAL_PARAMETERS,
                                rValues[PointNumber], rCurrentProcessInfo );
                    }
                }
        }
        /**
    * Set a Double Variable from outside
    * @param rVariable Global name of the variable to be calculated
    * @param rValue value on the quadrature points
    * @param rCurrentProcessInfo
    */
        void KinematicLinear::SetValueOnIntegrationPoints( const Variable<double>& rVariable,
                    std::vector<double>& rValues,const ProcessInfo& rCurrentProcessInfo)
        {
//              std::cout << mConstitutiveLawVector[0] << std::endl;
                if (rVariable==SUCTION)
                {
                    for( unsigned int PointNumber = 0; PointNumber<GetGeometry().IntegrationPoints(mThisIntegrationMethod).size(); PointNumber++ )
                        {
                                mConstitutiveLawVector[PointNumber]->SetValue(SUCTION, rValues[PointNumber],
                                                rCurrentProcessInfo );
                        }
                }
        }

} // Namespace Kratos


// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/StdVector>

#include <unordered_set>

#include <iostream>
#include <stdint.h>

#include "g2o/config.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/types/icp/types_icp.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"

#if defined G2O_HAVE_CHOLMOD
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#elif defined G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif

using namespace Eigen;
using namespace std;

class Sample
{
public:
  static int uniform(int from, int to);
  static double uniform();
  static double gaussian(double sigma);
};

static double uniform_rand(double lowerBndr, double upperBndr)
{
  return lowerBndr + ((double) std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr);
}

static double gauss_rand(double mean, double sigma)
{
  double x, y, r2;
  do {
    x = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
    y = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
    r2 = x * x + y * y;
  } while (r2 > 1.0 || r2 == 0.0);
  return mean + sigma * y * std::sqrt(-2.0 * log(r2) / r2);
}

int Sample::uniform(int from, int to)
{
  return static_cast<int>(uniform_rand(from, to));
}

double Sample::uniform()
{
  return uniform_rand(0., 1.);
}

double Sample::gaussian(double sigma)
{
  return gauss_rand(0., sigma);
}

int main(int argc, const char* argv[])
{
  if (argc<2)
  {
    cout << endl;
    cout << "Please type: " << endl;
    cout << "ba_demo [PIXEL_NOISE] [OUTLIER RATIO] [ROBUST_KERNEL] [STRUCTURE_ONLY] [DENSE]" << endl;
    cout << endl;
    cout << "PIXEL_NOISE: noise in image space (E.g.: 1)" << endl;
    cout << "OUTLIER_RATIO: probability of spuroius observation  (default: 0.0)" << endl;
    cout << "ROBUST_KERNEL: use robust kernel (0 or 1; default: 0==false)" << endl;
    cout << "STRUCTURE_ONLY: performe structure-only BA to get better point initializations (0 or 1; default: 0==false)" << endl;
    cout << "DENSE: Use dense solver (0 or 1; default: 0==false)" << endl;
    cout << endl;
    cout << "Note, if OUTLIER_RATIO is above 0, ROBUST_KERNEL should be set to 1==true." << endl;
    cout << endl;
    exit(0);
  }

  double PIXEL_NOISE = atof(argv[1]);

  double OUTLIER_RATIO = 0.0;

  if (argc>2)
  {
    OUTLIER_RATIO = atof(argv[2]);
  }

  bool ROBUST_KERNEL = false;
  if (argc>3)
  {
    ROBUST_KERNEL = atoi(argv[3]) != 0;
  }
  bool STRUCTURE_ONLY = false;
  if (argc>4)
  {
    STRUCTURE_ONLY = atoi(argv[4]) != 0;
  }

  bool DENSE = false;
  if (argc>5)
  {
    DENSE = atoi(argv[5]) != 0;
  }

  cout << "PIXEL_NOISE: " <<  PIXEL_NOISE << endl;
  cout << "OUTLIER_RATIO: " << OUTLIER_RATIO<<  endl;
  cout << "ROBUST_KERNEL: " << ROBUST_KERNEL << endl;
  cout << "STRUCTURE_ONLY: " << STRUCTURE_ONLY<< endl;
  cout << "DENSE: "<<  DENSE << endl;

  g2o::SparseOptimizer optimizer;
  optimizer.setVerbose(false);
  std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
  if (DENSE)
  {
    linearSolver = g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>>();
    cerr << "Using DENSE" << endl;
  }
  else
  {
#ifdef G2O_HAVE_CHOLMOD
	cerr << "Using CHOLMOD" << endl;
    linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
#elif defined G2O_HAVE_CSPARSE
    linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>>();
	cerr << "Using CSPARSE" << endl;
#else
#error neither CSparse nor Cholmod are available
#endif
  }

  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));

  optimizer.setAlgorithm(solver);

  // set up 500 points
  vector<Vector3d> true_points;
  for (size_t i=0;i<500; ++i)
  {
    true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
                                   Sample::uniform()-0.5,
                                   Sample::uniform()+10));
  }

  Vector2d focal_length(500,500); // pixels
  Vector2d principal_point(320,240); // 640x480 image
  double baseline = 0.075;      // 7.5 cm baseline

  vector<Eigen::Isometry3d,
      aligned_allocator<Eigen::Isometry3d> > true_poses;

  // set up camera params
  g2o::VertexSCam::setKcam(focal_length[0],focal_length[1],
                           principal_point[0],principal_point[1],
                           baseline);

  // set up 5 vertices, first 2 fixed
  int vertex_id = 0;
  for (size_t i=0; i<5; ++i)
  {

    Vector3d trans(i*0.04-1.,0,0);

    Eigen:: Quaterniond q;
    q.setIdentity();
    Eigen::Isometry3d pose;
    pose = q;
    pose.translation() = trans;

    g2o::VertexSCam * v_se3
        = new g2o::VertexSCam();

    v_se3->setId(vertex_id);
    v_se3->setEstimate(pose);
    v_se3->setAll();            // set aux transforms

    if (i<2)
      v_se3->setFixed(true);

    optimizer.addVertex(v_se3);
    true_poses.push_back(pose);
    vertex_id++;
  }

  int point_id=vertex_id;
  int point_num = 0;
  double sum_diff2 = 0;

  cout << endl;
  unordered_map<int,int> pointid_2_trueid;
  unordered_set<int> inliers;

  // add point projections to this vertex
  for (size_t i=0; i<true_points.size(); ++i)
  {
    g2o::VertexSBAPointXYZ * v_p
        = new g2o::VertexSBAPointXYZ();

    v_p->setId(point_id);
    v_p->setMarginalized(true);
    v_p->setEstimate(true_points.at(i)
        + Vector3d(Sample::gaussian(1),
                   Sample::gaussian(1),
                   Sample::gaussian(1)));

    int num_obs = 0;

    for (size_t j=0; j<true_poses.size(); ++j)
    {
      Vector3d z;
      dynamic_cast<g2o::VertexSCam*>
        (optimizer.vertices().find(j)->second)
        ->mapPoint(z,true_points.at(i));

      if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
      {
        ++num_obs;
      }
    }

    if (num_obs>=2)
    {
      optimizer.addVertex(v_p);

      bool inlier = true;
      for (size_t j=0; j<true_poses.size(); ++j)
      {
        Vector3d z;
        dynamic_cast<g2o::VertexSCam*>
          (optimizer.vertices().find(j)->second)
          ->mapPoint(z,true_points.at(i));

        if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
        {
          double sam = Sample::uniform();
          if (sam<OUTLIER_RATIO)
          {
            z = Vector3d(Sample::uniform(64,640),
                         Sample::uniform(0,480),
                         Sample::uniform(0,64)); // disparity
            z(2) = z(0) - z(2); // px' now

            inlier= false;
          }

          z += Vector3d(Sample::gaussian(PIXEL_NOISE),
                        Sample::gaussian(PIXEL_NOISE),
                        Sample::gaussian(PIXEL_NOISE/16.0));

          g2o::Edge_XYZ_VSC * e
              = new g2o::Edge_XYZ_VSC();

          e->vertices()[0]
              = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);

          e->vertices()[1]
              = dynamic_cast<g2o::OptimizableGraph::Vertex*>
              (optimizer.vertices().find(j)->second);

          e->setMeasurement(z);
          //e->inverseMeasurement() = -z;
          e->information() = Matrix3d::Identity();

          if (ROBUST_KERNEL) {
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            e->setRobustKernel(rk);
          }

          optimizer.addEdge(e);

        }

      }

      if (inlier)
      {
        inliers.insert(point_id);
        Vector3d diff = v_p->estimate() - true_points[i];

        sum_diff2 += diff.dot(diff);
      }
     // else
     //   cout << "Point: " << point_id <<  "has at least one spurious observation" <<endl;

      pointid_2_trueid.insert(make_pair(point_id,i));

      ++point_id;
      ++point_num;
    }

  }

  cout << endl;
  optimizer.initializeOptimization();

  optimizer.setVerbose(true);

  if (STRUCTURE_ONLY)
  {
    cout << "Performing structure-only BA:"   << endl;
    g2o::StructureOnlySolver<3> structure_only_ba;
    g2o::OptimizableGraph::VertexContainer points;
    for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
      g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
      if (v->dimension() == 3)
        points.push_back(v);
    }

    structure_only_ba.calc(points, 10);
  }

    cout << endl;
  cout << "Performing full BA:" << endl;
  optimizer.optimize(10);

  cout << endl;
  cout << "Point error before optimisation (inliers only): " << sqrt(sum_diff2/inliers.size()) << endl;

  point_num = 0;
  sum_diff2 = 0;

  for (unordered_map<int,int>::iterator it=pointid_2_trueid.begin();
       it!=pointid_2_trueid.end(); ++it)
  {

    g2o::HyperGraph::VertexIDMap::iterator v_it
        = optimizer.vertices().find(it->first);

    if (v_it==optimizer.vertices().end())
    {
      cerr << "Vertex " << it->first << " not in graph!" << endl;
      exit(-1);
    }

    g2o::VertexSBAPointXYZ * v_p
        = dynamic_cast< g2o::VertexSBAPointXYZ * > (v_it->second);

    if (v_p==0)
    {
      cerr << "Vertex " << it->first << "is not a PointXYZ!" << endl;
      exit(-1);
    }

    Vector3d diff = v_p->estimate()-true_points[it->second];

    if (inliers.find(it->first)==inliers.end())
      continue;

    sum_diff2 += diff.dot(diff);

    ++point_num;
  }

  cout << "Point error after optimisation (inliers only): " << sqrt(sum_diff2/inliers.size()) << endl;
  cout << endl;

}


/**
 * @file isam.cpp
 * @brief Main isam program.
 * @author Michael Kaess
 * @version $Id: isam.cpp 6377 2012-03-30 20:06:44Z kaess $
 *
 * Copyright (C) 2009-2013 Massachusetts Institute of Technology.
 * Michael Kaess, Hordur Johannsson, David Rosen,
 * Nicholas Carlevaris-Bianco and John. J. Leonard
 *
 * This file is part of iSAM.
 *
 * iSAM is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the
 * Free Software Foundation; either version 2.1 of the License, or (at
 * your option) any later version.
 *
 * iSAM is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 * License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with iSAM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <iostream>

const std::string usage = "\n"
    "Usage:\n"
    "  iSAM [OPTION...] FILE\n"
    "\n"
    "Options:\n"
    "  -h  -?       show help options\n"
    "  -v           verbose - additional output\n"
    "  -q           quiet - no output\n"
    "  -n <number>  max. number of lines to read, 0=all\n"
    "  -G           GUI: show in 3D viewer\n"
    "  -L           LCM: send data to external process\n"
    "  -S [fname]   save statistics\n"
    "  -W [fname]   write out final result\n"
    "  -F           force use of numerical derivatives\n"
    "  -C           calculate marginal covariances\n"
    "  -B           batch processing\n"
    "  -M           use Levenberg-Marquardt for batch\n"
    "  -P           use Powell's Dog-Leg algorithm for optimization\n"
    "  -N           no optimization\n"
    "  -R           use robust (pseudo-Huber) cost function\n"
    "  -d <number>  #steps between drawing/sending data\n"
    "  -u <number>  #steps between any updates (batch or incremental)\n"
    "  -b <number>  #steps between batch steps, 0=never\n"
    "  -s <number>  #steps between solution (backsubstitution)\n"
    "\n";

const std::string intro = "\n"
    "Incremental Smoothing and Mapping (iSAM) version 1.6\n"
    "(C) 2009-2012 Massachusetts Institute of Technology\n"
    "Michael Kaess, Hordur Johannsson, David M. Rosen, and John J. Leonard\n"
    "\n";

#include <stdio.h>
#include <cstring>
#include <map>
#include <Eigen/Dense>

#include <isam/isam.h>
#include <isam/robust.h>

#include "Loader.h"
#ifdef USE_LCM
#include "Lcm.h"
#endif
#ifdef USE_GUI
// needed in file with main() function to work on Mac...
#include "SDL.h"
#include "Viewer.h"
#endif

// for getopt / command line options processing
#include <unistd.h>
extern int optind;
extern char *optarg;

using namespace std;
using namespace isam;
using namespace Eigen;

const int FNAME_MAX = 500;
char fname[FNAME_MAX];
char fname_stats[FNAME_MAX] = "isam_stats.txt";
char fname_result[FNAME_MAX] = "isam_result.txt";

bool use_gui = false;
bool use_lcm = false;
bool save_stats = false;
bool write_result = false;
bool calculate_covariances = false;
bool batch_processing = false;
bool no_optimization = false;
int parse_num_lines = 0;

// draw state every mod_draw steps
int mod_draw = 1;

Slam slam;
Properties prop;
#ifdef USE_LCM
Lcm lcm;
bool lcm_first = true;
#endif
#ifdef USE_GUI
Viewer viewer;
#endif
Loader* loader;

// for recording of statistics for each time step (for visualization)
class Stats {
public:
  double time;
  double chi2;
  unsigned int nnz;
  double local_chi2;
  unsigned int nnodes;
  unsigned int nconstraints;
};
vector<class Stats> stats;

// hard coded for now, affecting all constraints
double robust_cost_function(double d) {
  return cost_pseudo_huber(d, .5);
}

/**
 * Command line argument processing.
 */
void process_arguments(int argc, char* argv[]) {
  int c;
  while ((c = getopt(argc, argv, ":h?vqn:GLS:W:FCBMPNRd:u:b:s:")) != -1) {
    // Each option character has to be in the string in getopt();
    // the first colon changes the error character from '?' to ':';
    // a colon after an option means that there is an extra
    // parameter to this option; 'W' is a reserved character
    switch (c) {
    case 'h':
    case '?':
      cout << intro;
      cout << usage;
      exit(0);
      break;
    case 'v':
      prop.quiet = false;
      prop.verbose = true;
      break;
    case 'q':
      prop.quiet = true;
      prop.verbose = false;
      break;
    case 'n':
      parse_num_lines = atoi(optarg);
      require(parse_num_lines>0, "Number of lines (-n) must be positive (>0).");
      break;
    case 'G':
#ifndef USE_GUI
      require(false, "GUI support (-G) was disabled at compile time");
#endif
      use_gui = true;
      break;
    case 'L':
#ifndef USE_LCM
      require(false, "LCM support (-L) was disabled at compile time");
#endif
      use_lcm = true;
      break;
    case 'S':
      save_stats = true;
      if (optarg != NULL) {
        strncpy(fname_stats, optarg, FNAME_MAX);
      }
      break;
    case 'W':
      write_result = true;
      if (optarg != NULL) {
        strncpy(fname_result, optarg, FNAME_MAX);
      }
      break;
    case 'F':
      prop.force_numerical_jacobian = true;
      break;
    case 'C':
      calculate_covariances = true;
      break;
    case 'B':
      batch_processing = true;
      break;
    case 'M':
      prop.method = LEVENBERG_MARQUARDT;
      break;
    case 'P':
      prop.method = DOG_LEG;
      break;
    case 'N':
      no_optimization = true;
      break;
    case 'R':
      slam.set_cost_function(&robust_cost_function);
      break;
    case 'd':
      mod_draw = atoi(optarg);
      require(mod_draw>0,
          "Number of steps between drawing (-d) must be positive (>0).");
      break;
    case 'u':
      prop.mod_update = atoi(optarg);
      require(prop.mod_update>0,
          "Number of steps between updates (-u) must be positive (>0).");
      break;
    case 'b':
      prop.mod_batch = atoi(optarg);
      require(
          prop.mod_batch>=0,
          "Number of steps between batch steps (-b) must be positive or zero (>=0).");
      break;
    case 's':
      prop.mod_solve = atoi(optarg);
      require(prop.mod_solve>0,
          "Number of steps between solving (-s) must be positive (>0).");
      break;
    case ':': // unknown option, from getopt
      cout << intro;
      cout << usage;
      exit(1);
      break;
    }
  }

  if ((prop.method == LEVENBERG_MARQUARDT) && (!batch_processing)) {
    cout << "Error:  Levenberg-Marquardt optimization has no incremental mode."
        << endl;
    exit(1);
  }

  if (argc > optind + 1) {
    cout << intro;
    cout << endl;
    cout << "Error: Too many arguments." << endl;
    cout << usage;
    exit(1);
  } else if (argc == optind + 1) {
    strncpy(fname, argv[optind], FNAME_MAX);
  }

}

/**
 * Save statistics for each time step for external visualization.
 */
void save_statistics(const string& fname) {
  ofstream out(fname.c_str(), ios::out | ios::binary);
  require(out, "Cannot open statistics file.");
  for (unsigned int i = 0; i < stats.size(); i++) {
    out << i << " " << stats[i].time << " " << stats[i].chi2 << " "
        << stats[i].nnz;
    out << " " << stats[i].local_chi2;
    out << " " << stats[i].nconstraints << " " << stats[i].nnodes;
    out << endl;
  }
  out.close();
}

/**
 * Calculate covariances for both points and poses up to the given time step.
 */
void covariances(unsigned int step, list<MatrixXd>& point_marginals,
    list<MatrixXd>& pose_marginals) {
  // make sure return arguments are empty
  point_marginals.clear();
  pose_marginals.clear();

  // combining everything into one call is faster,
  // as it avoids recalculating commonly needed entries
  Covariances::node_lists_t node_lists;
  for (unsigned int i = 0; i < step; i++) {
    list<Node*> entry;
    entry.push_back(loader->pose_nodes()[i]);
    node_lists.push_back(entry);
  }
  for (unsigned int i = 0; i < loader->num_points(step); i++) {
    list<Node*> entry;
    entry.push_back(loader->point_nodes()[i]);
    node_lists.push_back(entry);
  }
  pose_marginals = slam.covariances().marginal(node_lists);

  // split into points and poses
  if (pose_marginals.size() > 0) {
    list<MatrixXd>::iterator center = pose_marginals.begin();
    for (unsigned int i = 0; i < step; i++, center++)
      ;
    point_marginals.splice(point_marginals.begin(), pose_marginals, center,
        pose_marginals.end());
  }
}

/**
 * Visualize data during optimization in internal viewer
 * or send data via LCM (to an external viewer).
 */
void visualize(unsigned int step) {
  list<MatrixXd> point_marginals;
  list<MatrixXd> pose_marginals;
  if (calculate_covariances && (use_gui || use_lcm) && (step % mod_draw == 0)) {
    covariances(step, point_marginals, pose_marginals);
  }

#ifdef USE_LCM
  {
    // ids also determine color in viewer
    const int id_trajectory = 0;
    const int id_landmarks = 1;
    const int id_constraints = 2;
    const int id_measurements = 3;
    const int id_pose_covs = 4;
    const int id_point_covs = 5;

    // send to LCM viewer
    if (use_lcm && lcm_first) {
      lcm.send_reset();
      lcm_first = false;
    }
    if (use_lcm && step%mod_draw==0) {
      lcm.send_nodes(loader->poses(step), id_trajectory, (char*)"Trajectory", 1);
      lcm.send_nodes(loader->points(step), id_landmarks, (char*)"Landmarks", loader->is_3d() ? 3 : 2);
      lcm.send_links(loader->constraints(step), id_constraints,
          (char*)"Odometry", id_trajectory, id_trajectory);
      lcm.send_links(loader->measurements(step), id_measurements,
          (char*)"Measurements", id_trajectory, id_landmarks);
      if (calculate_covariances) {
        lcm.send_covariances(pose_marginals, id_pose_covs,
            (char*)"Pose Covs", id_trajectory, loader->is_3d());
        lcm.send_covariances(point_marginals, id_point_covs,
            (char*)"Point Covs", id_landmarks, loader->is_3d());
      }
    }
  }
#endif

#ifdef USE_GUI
  {
    // display in internal 3D viewer
    const int id_trajectory = 0;
    const int id_landmarks = 1;
    const int id_constraints = 2;
    const int id_measurements = 3;
    const int id_pose_covs = 4;
    const int id_point_covs = 5;
    if (use_gui && step%mod_draw==0) {
      viewer.set_nodes(loader->poses(step), id_trajectory,
          (char*)"Trajectory", VIEWER_OBJ_POSE3D);
      viewer.set_nodes(loader->points(step), id_landmarks,
          (char*)"Landmarks", loader->is_3d() ? VIEWER_OBJ_POINT3D : VIEWER_OBJ_TREE);
      viewer.set_links(loader->constraints(step), id_constraints,
          "Odometry", id_trajectory, id_trajectory);
      viewer.set_links(loader->measurements(step), id_measurements,
          "Measurements", id_trajectory, id_landmarks);
      if (calculate_covariances) {
        viewer.set_covariances(pose_marginals, id_pose_covs,
            (char*)"Pose Covs", id_trajectory, loader->is_3d());
        viewer.set_covariances(point_marginals, id_point_covs,
            (char*)"Point Covs", id_landmarks, loader->is_3d());
      }
    }
  }
#endif
}

/**
 * Quit if viewer was closed.
 */
void check_quit() {
#ifdef USE_GUI
  if (viewer.exit_requested()) {
    cout << endl << "Aborted by user..." << endl;
    exit(0);
  }
#endif
}

/**
 * Incrementally process factors.
 */
void incremental_slam() {

// AJOUT CODE
char fname_result_incr[FNAME_MAX];
strcpy(fname_result_incr,fname_result);
strcat(fname_result_incr,"_incremental.txt");
ofstream out(fname_result_incr, ios::out | ios::binary );
require(out, "Slam.save: Cannot open output file.");
out << "\0";
out.close();

// AJOUT CODE
char fname_result_cov[FNAME_MAX];
strcpy(fname_result_cov,fname_result);
strcat(fname_result_cov,"_cov.txt");
ofstream out_cov(fname_result_cov, ios::out | ios::binary );
require(out_cov, "Slam.save: Cannot open output file.");
out_cov << "\0";
out_cov.close();

  unsigned int step = 0;

  unsigned int next_step = step;
  // step by step after reading log file
  for (; loader->more_data(&next_step); step = next_step) {
    check_quit();

// cout << " AJOUT CODE : Une ligne de plus !" <<endl;

    double t0 = tic();

    tic("setup");

    // add new variables and constraints for current step
    for (unsigned int s = step; s < next_step; s++) {
      for (list<Node*>::const_iterator it = loader->nodes(s).begin();
          it != loader->nodes(s).end(); it++) {
        if (prop.verbose)
          cout << **it << endl;
        slam.add_node(*it);

      }

      for (list<Factor*>::const_iterator it = loader->factors(s).begin();
          it != loader->factors(s).end(); it++) {
        if (prop.verbose)
          cout << **it << endl;
        slam.add_factor(*it);
      }
    }

// cout << endl << slam.get_nodes().back()->name() << ' ' << slam.get_nodes().back()->unique_id() << endl;

/*
cout << endl << (*loader->nodes(step).begin())->name() << ' ' << (*loader->nodes(step).begin())->unique_id() << endl;
cout<<step<<' ';
(*loader->nodes(step).begin())->write(cout);
cout<<endl;
sleep(1);
*/

// cout << (*it)->name() << (*it)->unique_id() << endl;
// system("PAUSE");
// AJOUT CODE cout << it->name() << "_Node " << it->_id << endl;

// slam.get_nodes().back()->write( cout);
// cout << endl;

    toc("setup");
    tic("incremental");

    if (!(batch_processing || no_optimization)) {
      slam.update();
    }

// AJOUT CODE
/*
cout << endl << (*loader->nodes(step).begin())->name() << ' ' << (*loader->nodes(step).begin())->unique_id() << endl;
cout<<step<<' ';
(*loader->nodes(step).begin())->write(cout);
cout<<endl;
sleep(1);
*/

// AJOUT CODE
char fname_result_incr[FNAME_MAX];
strcpy(fname_result_incr,fname_result);
strcat(fname_result_incr,"_incremental.txt");
ofstream out(fname_result_incr, ios::out | ios::binary | ios::app);
require(out, "Slam.save: Cannot open output file.");
//out << "test " << endl;
(*loader->nodes(step).begin())->write(out);
out << endl;
// slam.get_nodes().back()->write(out);
out.close();

// AJOUT CODE : Covariances

Node* last_node(0); // dernier noeud
last_node = *loader->nodes(step).begin();
std::list<Node*> last_node_as_list;
last_node_as_list.push_back(last_node);

MatrixXd cov_full = slam.covariances().marginal(last_node_as_list);
ofstream out_cov(fname_result_cov, ios::out | ios::binary | ios::app);
require(out_cov, "Slam.save: Cannot open output file.");
out_cov << cov_full << endl;
out_cov.close();

/*
std::list<std::pair<Node*, Node*> > node_pair_list_t; // Liste des noeuds calcules
MatrixXd cov_last = slam.covariances().marginal(loader());
cout << cov_last << endl << endl;
sleep(1);
*/

  if (write_result) {

// AJOUT CODE
//char fname_result_incr[FNAME_MAX];
//strcpy(fname_result_incr,fname_result);
//strcat(fname_result_incr,"_incremental");
//ofstream out(fname_result_incr, ios::out | ios::binary | ios::app);
//require(out, "Slam.save: Cannot open output file.");
//out << "test " << endl;
//slam.get_nodes().back()->write(out);
//out.close();

    // cout << "Saving last node to " << fname_result << endl;
    // slam.get_nodes().back()->write(fname_result);
    // slam.save(fname_result);
    // cout << endl;
  }

    toc("incremental");

    if (save_stats) {
      stats.resize(step + 1);
      stats[step].time = toc(t0);
      stats[step].chi2 = slam.normalized_chi2();
      stats[step].nnz = slam.get_R().nnz();
      stats[step].local_chi2 = slam.local_chi2(100); // last 100 constraints
      stats[step].nnodes = slam.get_nodes().size();
      stats[step].nconstraints = slam.get_factors().size();
    }

    // visualization is not counted in timing
    if (!(batch_processing || no_optimization)) {
      visualize(step);
    }
  }

  visualize(step - 1);

  if (!no_optimization) {
    if (batch_processing) {
      tic("batch");
      slam.batch_optimization();
      toc("batch");
    } else {
      // end with a batch step/relinearization
      prop.mod_batch = 1;
      slam.set_properties(prop);
      tic("final");
      slam.update();
      toc("final");
    }
  }

  visualize(step - 1);
}

/**
 * The actual processing of data, in separate thread if GUI enabled.
 */
int process(void* unused) {

  // incrementally process data
  slam.set_properties(prop);
  incremental_slam();

  toc("all");

// AJOUT CODE
  // recovering the block-diagonals only of the full covariance matrix
/*
  cout << "Block-diagonals only:" << endl;
  Covariances::node_lists_t node_lists;
  list<Node*> nodes;
  nodes.push_back(pose_node_1);
  node_lists.push_back(nodes);
  nodes.clear();
  nodes.push_back(pose_node_2);
  node_lists.push_back(nodes);
  list<MatrixXd> cov_blocks = covariances.marginal(node_lists);
  int i = 1;
  for (list<MatrixXd>::iterator it = cov_blocks.begin(); it!=cov_blocks.end(); it++, i++) {
    cout << "block " << i << endl;
    cout << *it << endl;
  }
*/

// AJOUT CODE
char fname_result_features[FNAME_MAX];
strcpy(fname_result_features,fname_result);
strcat(fname_result_features,"_features.txt");
ofstream out_features(fname_result_features, ios::out | ios::binary);
require(out_features, "Slam.save: Cannot open output file.");
out_features << "\0";

// AJOUT CODE COV AMERS

char fname_result_cov_final[FNAME_MAX];
strcpy(fname_result_cov_final,fname_result);
strcat(fname_result_cov_final,"_cov_final.txt");
ofstream out_cov_final(fname_result_cov_final, ios::out | ios::binary );
require(out_cov_final, "Slam.save: Cannot open output file.");
out_cov_final << "\0";

// Node* node_i(0); // noeud
// last_node = *loader->nodes(step).begin();
std::list<Node*> node_i_as_list;
// last_node_as_list.push_back(last_node);

// int res;

  const Covariances& covariances = slam.covariances().clone();
  // recovering the full covariance matrix
  cout << "Full covariance matrix:" << endl;
  MatrixXd cov_full ;
      for (list<Node*>::const_iterator it = slam.get_nodes().begin();
          it != slam.get_nodes().end(); it++) {
if ( strcmp ( (*it)->name(), "Point2d" )==0 ){
node_i_as_list.push_back(*it);
(*it)->write(out_features);
out_features << endl;

// cov_full = covariances.marginal(node_i_as_list);
// cout << cov_full << endl << endl;

}
// res = strcmp ( (*it)->name(), "Point2d" );
// cout << res << endl;
// node_i_as_list.push_back(*it);
// cout << (*it)->name() << endl;

//  MatrixXd cov_full = covariances.marginal(slam.get_nodes());
}
  cov_full = covariances.marginal(node_i_as_list);
  out_cov_final << cov_full << endl << endl;

out_cov_final.close();
out_features.close();

  if (!prop.quiet) {
    if (!batch_processing) {
      cout << endl;
    }
    double accumulated = tictoc("setup") + tictoc("incremental")
        + tictoc("batch");
    cout << "Accumulated computation time: " << accumulated << "s" << endl;
    cout << "(Overall execution time: " << tictoc("all") << "s)" << endl;
    slam.print_stats();
    cout << endl;
  }

  if (save_stats) {
    cout << "Saving statistics to " << fname_stats << endl;
    save_statistics(fname_stats);
    cout << endl;
  }
  if (write_result) {
    cout << "Saving result to " << fname_result << endl;
    slam.save(fname_result);
    cout << endl;
  }

#ifdef USE_GUI
  if (use_gui) {
    while (true) {
      if (viewer.exit_requested()) {
        exit(0);
      }
      SDL_Delay(100);
    }
  }
#endif

  exit(0);
}

/**
 * Everything starts here.
 */
int main(int argc, char* argv[]) {

  tic("all");

  process_arguments(argc, argv);

  cout << intro;

  if (!prop.quiet) {
    cout << "Reading " << fname;
    if (parse_num_lines > 0) {
      cout << " (only " << parse_num_lines << " lines)";
    }
    cout << endl;
  }
  // parse all data and get into suitable format for incremental processing
  Loader loader_(fname, parse_num_lines, prop.verbose);
  loader = &loader_;
  if (!prop.quiet) {
    loader_.print_stats();
    cout << endl;
  }

  if (!prop.quiet) {
    if (batch_processing) {
      cout << "Performing SAM (batch processing)\n";
    } else {
      cout << "Performing iSAM with parameters:\n";
      cout << "  Draw/send every " << mod_draw << " steps\n";
      cout << "  Update every " << prop.mod_update << " steps\n";
      cout << "  Solve every " << prop.mod_solve << " steps\n";
      cout << "  Batch every " << prop.mod_batch << " steps\n";
    }
    cout << endl;
  }

#ifdef USE_GUI
  if (use_gui) {
    cout << "3D viewer:\n";
    cout << "  Exit: Esc or \"q\"\n";
    cout << "  Reset view: \"r\"\n";
    cout << "  Toggle background color \"c\"\n";
    cout << "  Rotate: left mouse button\n";
    cout << "  Translate: middle mouse button or CTRL+left mouse button\n";
    cout << "  Scale: right mouse button or SHIFT+left mouse button or mouse wheel\n";
    cout << endl;
    // process will be run in separate thread
    viewer.init(process);
  } else {
    process(NULL);
  }
#else
  // no threads needed, simply run process
  process(NULL);
#endif

  return 0;
}


/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

//- Class:       NPSOLOptimizer
//- Description: Implementation code for the NPSOLOptimizer class
//- Owner:       Mike Eldred
//- Checked by:

#include "dakota_system_defs.hpp"
#include "DakotaModel.hpp"
#include "DakotaResponse.hpp"
#include "NPSOLOptimizer.hpp"
#include "ProblemDescDB.hpp"
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <sstream>

static const char rcsId[]="@(#) $Id: NPSOLOptimizer.cpp 7029 2010-10-22 00:17:02Z mseldre $";

#define NPSOL_F77   F77_FUNC(npsol,NPSOL)
// BMA (20160315): Changed to use Fortran 2003 ISO C bindings.
// The Fortran symbol will be lowercase with same name as if in C
//#define NPOPTN2_F77 F77_FUNC(npoptn2,NPOPTN2)
#define NPOPTN2_F77 npoptn2

extern "C" {

void NPSOL_F77( int& n, int& nclin, int& ncnln, int& nrowa, int& nrowj,
		int& nrowr, double* a, double* bl, double* bu,
		void (*funcon)(int& mode, int& ncnln, int& n, int& nrowj,
			       int* needc, double* x, double* c, double* cjac,
			       int& nstate),
		void (*funobj)(int& mode, int& n, double* x, double& f,
			       double* gradf, int& nstate),
		int& inform, int& iter, int* istate, double* c, double* cjac,
		double* clambda, double& objf, double* grad, double* r,
		double* x, int* iw, int& leniw, double* w, int& lenw );

// NOTE: replacing void* function pointers with void(NPSOLOptimizer::*) member
// function pointers in the declaration above (in order to try to remove static
// requirement) will compile and link, but this pointer format is not
// acceptible to the Fortran and the call to NPSOL core dumps with an "Illegal
// Instruction" message.  Also, trying to cast from a member function pointer
// to a function pointer is not allowed by the compiler.

void NPOPTN2_F77( const char* option_string );

//void mcenvn( int& idmy1, int& idmy2, double& eps, int& idmy3, double& rdmy);

}

namespace Dakota {

NPSOLOptimizer* NPSOLOptimizer::npsolInstance(NULL);

/** This is the primary constructor.  It accepts a Model reference. */
NPSOLOptimizer::NPSOLOptimizer(ProblemDescDB& problem_db, Model& model):
  Optimizer(problem_db, model, std::shared_ptr<TraitsBase>(new NPSOLTraits())),
  SOLBase(model), setUpType("model")
{
  // invoke SOLBase set function (shared with NLSSOLLeastSq)
  set_options(speculativeFlag, vendorNumericalGradFlag, outputLevel,
              probDescDB.get_int("method.npsol.verify_level"),
              probDescDB.get_real("method.function_precision"),
              probDescDB.get_real("method.npsol.linesearch_tolerance"),
              maxIterations, constraintTol, convergenceTol,
	      iteratedModel.gradient_type(),
	      iteratedModel.fd_gradient_step_size());
}

/** This is an alternate constructor which accepts a Model but does
    not have a supporting method specification from the ProblemDescDB. */
NPSOLOptimizer::NPSOLOptimizer(Model& model):
  Optimizer(NPSOL_SQP, model, std::shared_ptr<TraitsBase>(new NPSOLTraits())),
  SOLBase(model), setUpType("model")
{
  // invoke SOLBase set function (shared with NLSSOLLeastSq)
  set_options(speculativeFlag, vendorNumericalGradFlag, outputLevel, -1,
	      1.e-10, 0.9, maxIterations, constraintTol, convergenceTol,
	      iteratedModel.gradient_type(),
	      iteratedModel.fd_gradient_step_size());
}

/** This is an alternate constructor for instantiations on the fly
    using a Model but no ProblemDescDB. */
NPSOLOptimizer::
NPSOLOptimizer(Model& model, const int& derivative_level, const Real& conv_tol):
  Optimizer(NPSOL_SQP, model, std::shared_ptr<TraitsBase>(new NPSOLTraits())),
  SOLBase(model), setUpType("model")
{
  // Set NPSOL options (mostly use defaults)
  std::string vlevel_s("Verify Level                = -1");
  vlevel_s.resize(72, ' ');
  NPOPTN2_F77( vlevel_s.data() ); // NO Null terminator with std::string::data()

  std::string plevel_s("Major Print Level           = 0");
  plevel_s.resize(72, ' ');
  NPOPTN2_F77( plevel_s.data() );

  // assign the derivative_level passed in
  std::string dlevel_s("Derivative Level            = ");
  dlevel_s += boost::lexical_cast<std::string>(derivative_level);
  dlevel_s.resize(72, ' ');
  NPOPTN2_F77( dlevel_s.data() );

  // assign the conv_tol passed in
  if (conv_tol > 0.) { // conv_tol < 0 can be passed to use the NPSOL default
    std::ostringstream ctol_stream;
    ctol_stream << "Optimality Tolerance        = "
                << std::setiosflags(std::ios::left) << std::setw(26)<< conv_tol;
    std::string ctol_s( ctol_stream.str() );
    ctol_s.resize(72, ' ');
    NPOPTN2_F77( ctol_s.data() );
  }
}

/** This is an alternate constructor for performing an optimization using
    the passed in objective function and constraint function pointers. */
NPSOLOptimizer::NPSOLOptimizer(const RealVector& initial_point,
  const RealVector& var_lower_bnds, const RealVector& var_upper_bnds,
  const RealMatrix& lin_ineq_coeffs,
  const RealVector& lin_ineq_lower_bnds,
  const RealVector& lin_ineq_upper_bnds,
  const RealMatrix& lin_eq_coeffs,
  const RealVector& lin_eq_targets,
  const RealVector& nonlin_ineq_lower_bnds,
  const RealVector& nonlin_ineq_upper_bnds,
  const RealVector& nonlin_eq_targets,
  void (*user_obj_eval) (int&, int&, double*, double&, double*, int&),
  void (*user_con_eval) (int&, int&, int&, int&, int*, double*, double*,
			 double*, int&),
  const int& derivative_level, const Real& conv_tol): // SOLBase default ctor
  Optimizer(NPSOL_SQP, initial_point.length(), 0, 0, 0,
	    lin_ineq_coeffs.numRows(), lin_eq_coeffs.numRows(),
	    nonlin_ineq_lower_bnds.length(), nonlin_eq_targets.length(),
            std::shared_ptr<TraitsBase>(new NPSOLTraits())),
  setUpType("user_functions"), initialPoint(initial_point),
  lowerBounds(var_lower_bnds), upperBounds(var_upper_bnds),
  userObjectiveEval(user_obj_eval), userConstraintEval(user_con_eval)
{
  // invoke SOLBase allocate/set functions (shared with NLSSOLLeastSq)
  allocate_arrays(numContinuousVars, numNonlinearConstraints, lin_ineq_coeffs,
		  lin_eq_coeffs);
  allocate_workspace(numContinuousVars, numNonlinearConstraints,
		     numLinearConstraints, 0);
  augment_bounds(lowerBounds, upperBounds, lin_ineq_lower_bnds,
                 lin_ineq_upper_bnds, lin_eq_targets, nonlin_ineq_lower_bnds,
                 nonlin_ineq_upper_bnds, nonlin_eq_targets);

  // Set NPSOL options (mostly use defaults)
  std::string vlevel_s("Verify Level                = -1");
  vlevel_s.resize(72, ' ');
  NPOPTN2_F77( vlevel_s.data() );

  std::string plevel_s("Major Print Level           = 0");
  plevel_s.resize(72, ' ');
  NPOPTN2_F77( plevel_s.data() );

  // Set Derivative Level = 3 for user-supplied gradients, 0 for NPSOL
  // vendor-numerical, ...
  std::string dlevel_s("Derivative Level            = ");
  dlevel_s += boost::lexical_cast<std::string>(derivative_level);
  dlevel_s.resize(72, ' ');
  NPOPTN2_F77( dlevel_s.data() );

  // assign the conv_tol passed in.
  if (conv_tol > 0.) { // conv_tol < 0 can be passed to use the NPSOL default
    std::ostringstream ctol_stream;
    ctol_stream << "Optimality Tolerance        = "
                << std::setiosflags(std::ios::left) << std::setw(26)<< conv_tol;
    std::string ctol_s( ctol_stream.str() );
    ctol_s.resize(72, ' ');
    NPOPTN2_F77( ctol_s.data() );
  }
}

NPSOLOptimizer::~NPSOLOptimizer()
{
  // Virtual destructor handles referenceCount at Iterator level.

  // invoke SOLBase deallocate function (shared with NLSSOLLeastSq)
  if (setUpType == "user_functions")
    deallocate_arrays();
}

#ifdef HAVE_DYNLIB_FACTORIES
NPSOLOptimizer* new_NPSOLOptimizer(ProblemDescDB& problem_db)
{
#ifdef DAKOTA_DYNLIB
  not_available("NPSOL");
  return 0;
#else
  return new NPSOLOptimizer(problem_db);
#endif
}

NPSOLOptimizer* new_NPSOLOptimizer1(Model& model)
{
#ifdef DAKOTA_DYNLIB
  not_available("NPSOL");
  return 0;
#else
  return new NPSOLOptimizer(model);
#endif
}

NPSOLOptimizer* new_NPSOLOptimizer2(Model& model, const int& derivative_level,
                                    const Real& conv_tol)
{
#ifdef DAKOTA_DYNLIB
  not_available("NPSOL");
  return 0;
#else
  return new NPSOLOptimizer(model, derivative_level, conv_tol);
#endif
}

NPSOLOptimizer* new_NPSOLOptimizer3(const RealVector& initial_point,
  const RealVector& var_lower_bnds,
  const RealVector& var_upper_bnds,
  const RealMatrix& lin_ineq_coeffs,
  const RealVector& lin_ineq_lower_bnds,
  const RealVector& lin_ineq_upper_bnds,
  const RealMatrix& lin_eq_coeffs,
  const RealVector& lin_eq_targets,
  const RealVector& nonlin_ineq_lower_bnds,
  const RealVector& nonlin_ineq_upper_bnds,
  const RealVector& nonlin_eq_targets,
  void (*user_obj_eval) (int&, int&, double*, double&, double*, int&),
  void (*user_con_eval) (int&, int&, int&, int&, int*, double*, double*,
                         double*, int&),
  const int& derivative_level, const Real& conv_tol)
{
#ifdef DAKOTA_DYNLIB
  not_available("NPSOL");
  return 0;
#else
  return new NPSOLOptimizer(initial_point, var_lower_bnds, var_upper_bnds,
	       lin_ineq_coeffs, lin_ineq_lower_bnds, lin_ineq_upper_bnds,
	       lin_eq_coeffs, lin_eq_targets, nonlin_ineq_lower_bnds,
	       nonlin_ineq_upper_bnds, nonlin_eq_targets, user_obj_eval,
	       user_con_eval, derivative_level, conv_tol);
#endif // DAKOTA_DYNLIB
}
#endif // HAVE_DYNLIB_FACTORIES

void NPSOLOptimizer::
objective_eval(int& mode, int& n, double* x, double& f, double* gradf,
	       int& nstate)
{
  // NPSOL computes constraints first, then the objective function.  However,
  // Dakota assumes that the objective and constraint function values are all
  // computed in a single fn. evaluation. A numNonlinearConstraints check is
  // therefore needed to ensure that 1 and only 1 mapping occurs.

  // Handle special cases with asv_request (see SOLBase::constraint_eval)
  short asv_request = mode + 1; // default definition of asv_request

  //if ( !(solInstance->derivLevel & 1) && (asv_request & 2) ) { // more general
  if (npsolInstance->vendorNumericalGradFlag && (asv_request & 2) ) {
    asv_request -= 2; // downgrade request
    if (npsolInstance->numNonlinearConstraints == 0) { // else already printed
      Cout << "NPSOL has requested objective gradient for case of vendor "
	   << "numerical gradients.\n";
      if (asv_request)
	Cout << "Request will be downgraded to objective value alone.\n"
             << std::endl;
      else
	Cout << "Request will be ignored and no evaluation performed.\n"
             << std::endl;
    }
  }

  if (asv_request && npsolInstance->numNonlinearConstraints == 0) {
    // constraint_eval has not been called.  Therefore, set vars/asv
    // and perform an evaluate() prior to data recovery.
    RealVector local_des_vars(n, false);
    copy_data(x, n, local_des_vars);
    npsolInstance->iteratedModel.continuous_variables(local_des_vars);
    npsolInstance->activeSet.request_values(asv_request);
    npsolInstance->
      iteratedModel.evaluate(npsolInstance->activeSet);
    if (++npsolInstance->fnEvalCntr == npsolInstance->maxFunctionEvals) {
      mode = -1; // terminate NPSOL (see mode discussion in "User-Supplied
	         // Subroutines" section of NPSOL manual)
      Cout << "Iteration terminated: max_function_evaluations limit has been "
	   << "met." << std::endl;
    }
  }

  const Response& local_response
    = npsolInstance->iteratedModel.current_response();
  // Any MOO/NLS recasting is responsible for setting the scalar min/max
  // sense within the recast.
  const BoolDeque& max_sense
    = npsolInstance->iteratedModel.primary_response_fn_sense();
  bool max_flag = (!max_sense.empty() && max_sense[0]);
  if (asv_request & 1)
    f = (max_flag) ? -local_response.function_value(0) :
                      local_response.function_value(0);
  if (asv_request & 2) {
    const Real* local_grad = local_response.function_gradient(0);
    if (max_flag)
      for (size_t i=0; i<n; ++i)
	gradf[i] = -local_grad[i];
    else
      std::copy(local_grad, local_grad + n, gradf);
  }
}

void NPSOLOptimizer::core_run()
{
  if (setUpType == "model")
    find_optimum_on_model();
  else if (setUpType == "user_functions")
    find_optimum_on_user_functions();
  else {
    Cerr << "Error: bad setUpType in NPSOLOptimizer::core_run()."
         << std::endl;
    abort_handler(-1);
  }
}

void NPSOLOptimizer::find_optimum_on_model()
{
  //------------------------------------------------------------------
  //     Solve the problem.
  //------------------------------------------------------------------

  // set the object instance pointers for use within the static member fns
  NPSOLOptimizer* prev_nps_instance = npsolInstance;
  SOLBase*        prev_sol_instance = solInstance;
  npsolInstance = this; solInstance = this; optLSqInstance = this;

  // Augmentation of bounds appears here rather than in the constructor because
  // set the constraint offset used in SOLBase::constraint_eval()
  constrOffset = numObjectiveFns;

  fnEvalCntr = 0; // prevent current iterator from continuing previous counting

  // casts for Fortran interface
  int num_cv = numContinuousVars;
  int num_linear_constraints = numLinearConstraints;
  int num_nonlinear_constraints = numNonlinearConstraints;

  double     local_f_val = 0.;
  RealVector local_f_grad(numContinuousVars, true);

  allocate_arrays(numContinuousVars, numNonlinearConstraints,
		  iteratedModel.linear_ineq_constraint_coeffs(),
		  iteratedModel.linear_eq_constraint_coeffs());
  allocate_workspace(numContinuousVars, numNonlinearConstraints,
                     numLinearConstraints, 0);

  // NPSOL requires a non-zero array size.  Therefore, size the local
  // constraint arrays and matrices to a size of 1 if there are no nonlinear
  // constraints and to the proper size otherwise.
  RealVector local_c_vals(nlnConstraintArraySize);

  // initialize local_des_vars with DB initial point.  Variables are updated
  // in constraint_eval/objective_eval
  RealVector local_des_vars;
  copy_data(iteratedModel.continuous_variables(), local_des_vars);

  // these bounds must be updated from model bounds each time an iterator is
  // run within the B&B minimizer.
  RealVector augmented_l_bnds, augmented_u_bnds;
  copy_data(iteratedModel.continuous_lower_bounds(), augmented_l_bnds);
  copy_data(iteratedModel.continuous_upper_bounds(), augmented_u_bnds);
  augment_bounds(augmented_l_bnds, augmented_u_bnds, iteratedModel);

  NPSOL_F77( num_cv, num_linear_constraints, num_nonlinear_constraints,
	     linConstraintArraySize, nlnConstraintArraySize, num_cv,
	     linConstraintMatrixF77, augmented_l_bnds.values(),
	     augmented_u_bnds.values(), constraint_eval, objective_eval,
	     informResult, numberIterations, &constraintState[0],
	     local_c_vals.values(), constraintJacMatrixF77, &cLambda[0],
	     local_f_val, local_f_grad.values(), upperFactorHessianF77,
	     local_des_vars.values(), &intWorkSpace[0], intWorkSpaceSize,
	     &realWorkSpace[0], realWorkSpaceSize );

  // NPSOL completed. Do post-processing/output of final NPSOL info and data:
  Cout << "\nNPSOL exits with INFORM code = " << informResult
       << " (see \"Interpretation of output\" section in NPSOL manual)\n";

  // invoke SOLBase deallocate function (shared with NLSSOLLeastSq)
  deallocate_arrays();

  // Set best variables and response for use at higher levels.
  // local_des_vars, local_f_val, & local_c_vals contain the optimal design
  // (not the final fn. eval) since NPSOL performs this assignment internally
  // prior to exiting (see "Subroutine npsol" section of NPSOL manual).
  bestVariablesArray.front().continuous_variables(local_des_vars);
  if (!localObjectiveRecast) { // else local_objective_recast_retrieve()
                               // is used in Optimizer::post_run()
    RealVector best_fns(numFunctions, false);
    const BoolDeque& max_sense = iteratedModel.primary_response_fn_sense();
    best_fns[0] = (!max_sense.empty() && max_sense[0]) ?
      -local_f_val : local_f_val;
    if (numNonlinearConstraints)
      //copy_data_partial(local_c_vals, best_fns, 1);
      std::copy(local_c_vals.values(),
                local_c_vals.values() + nlnConstraintArraySize,
                best_fns.values() + 1);
    bestResponseArray.front().function_values(best_fns);
  }

  /*
  // For better post-processing, could append fort.9 to dakota.out line
  // by line, but: THERE IS A PROBLEM WITH GETTING ALL OF THE FILE!
  // (FORTRAN output is lacking a final buffer flush?)
  Cout << "\nEcho NPSOL's iteration output from fort.9 file:\n" << std::endl;
  ifstream npsol_fort_9( "fort.9" );
  char fort_9_line[255];
  while (npsol_fort_9) {
    npsol_fort_9.getline( fort_9_line, 255 );
    Cout << fort_9_line << '\n';
  }
  Cout << std::endl;
  */
  Cout << "\nNOTE: see Fortran device 9 file (fort.9 or ftn09)"
       << "\n      for complete NPSOL iteration history." << std::endl;

  // restore in case of recursion
  npsolInstance  = prev_nps_instance;
  solInstance    = prev_sol_instance;
  optLSqInstance = prevMinInstance;
}

void NPSOLOptimizer::find_optimum_on_user_functions()
{
  //------------------------------------------------------------------
  //     Solve the problem.
  //------------------------------------------------------------------

  int i;

  // casts for Fortran interface
  int num_cv = numContinuousVars;
  int num_linear_constraints = numLinearConstraints;
  int num_nonlinear_constraints = numNonlinearConstraints;

  double     local_f_val = 0.;
  RealVector local_f_grad(numContinuousVars, true);
  RealVector local_c_vals(nlnConstraintArraySize);

  NPSOL_F77( num_cv, num_linear_constraints, num_nonlinear_constraints,
	     linConstraintArraySize, nlnConstraintArraySize, num_cv,
	     linConstraintMatrixF77, lowerBounds.values(), upperBounds.values(),
	     userConstraintEval, userObjectiveEval, informResult,
	     numberIterations, &constraintState[0], local_c_vals.values(),
	     constraintJacMatrixF77, &cLambda[0], local_f_val,
	     local_f_grad.values(), upperFactorHessianF77,
	     initialPoint.values(), &intWorkSpace[0], intWorkSpaceSize,
	     &realWorkSpace[0], realWorkSpaceSize );

  bestVariablesArray.front().continuous_variables(initialPoint);
  // user-functions mode is restricted to single-objective optimization
  RealVector best_fns(numFunctions, false);
  best_fns[0] = local_f_val;
  if (numNonlinearConstraints)
    //copy_data_partial(local_c_vals, best_fns, 1);
    std::copy(local_c_vals.values(),
              local_c_vals.values() + nlnConstraintArraySize,
              best_fns.values() + 1);
  bestResponseArray.front().function_values(best_fns);
}

} // namespace Dakota


/***
第四节 可变参模板
（4）可变参类模板：允许模板定义中包含0到多个（任意个）模板参数。
参数包展开方式有多种。介绍典型的展开方式。
（4.1）通过递归继承方式展开类型、非类型、模板模板参数包
1：类型模板参数包的展开一例
有时只写类模板声明而不写类模板定义的手法非常重要,靠这种手段能够帮助排错。
目前讲解的范例取材于C++标准库中的tuple(元组)
2：非类型模板参数包展开一例
3：模板模板参数包的展开一例
//ParentMM<int>
//ParentMM<int,deque>
//ParentMM<int,list,deque>
//ParentMM<int,vector,list,deque>
//myclasst3<int, vector, list, deque>
***/

#include <iostream>
#include <vector>
#include <list>
#include <deque>

//#include <boost/type_index.hpp>
using namespace std;
//#pragma warning(disable : 4996)

//非类型模板参数包展开一例
namespace _nmsp1
{
	//主模板定义（泛化版本的类模板）
	template <typename ...Args>
	class myclasst
	{
	public:
		myclasst()
		{
			printf("myclasst::myclasst()泛化版本执行了,this = %p\n", this);
		}
	};
	//template <typename ...Args>class myclasst; //主模板声明(前向声明/前置声明)

	template <typename First, typename ... Others>
	class myclasst<First, Others...> :private myclasst<Others...>  //偏特化
	{
	public:
		myclasst() :m_i(0)
		{
			printf("myclasst::myclasst()偏特化版本执行了，this = %p,sizeof...(Others)=%d\n", this, sizeof...(Others));
		}

		myclasst(First parf, Others... paro) :m_i(parf), myclasst<Others...>(paro...)
		{
			printf("myclasst::myclasst(parf,...paro)执行了,this = %p\n", this);
			cout << "m_i = " << m_i << endl;
		}

		First m_i;
	};

	template<>
	class myclasst<>    //一个特殊的特化版本,看起来象全特化,不是全特化,可变参模板不存在全特化
	{
	public:
		myclasst()
		{
			printf("myclasst::myclasst()特殊的特化版本执行了,this = %p\n", this);
		}
	};

	/*
	template <typename ...Args1, typename ... Args2>
	class myclasst2 {};

	template <typename ...Args, typename U>
	class myclasst3 {};
	*/
}

//类型模板参数包的展开一例
namespace _nmsp2
{
	//主模板定义(泛化版本的类模板)
	template <int ...FTArgs>            //int 替换为auto也可以
	class myclasst2
	{
	public:
		myclasst2()
		{
			printf("myclasst2::myclasst2()泛化版本执行了,this = %p\n", this);
		}
	};

	template <int First,int ...Others> //int替换成auto也没问题
	class myclasst2<First, Others...> :private myclasst2<Others...> //偏特化
	{
	public:
		myclasst2()
		{
			printf("myclasst2::myclasst2()偏特化版本执行了，this = %p,sizeof...(Others)=%d,First=%d\n", this, sizeof...(Others),First);
		}
	};

}

//模板模板参数包的展开一例
namespace _nmsp3
{
	//泛化版本
	template<typename T,
			 template<typename> typename... Container>
	class ParentMM
	{
	public:
		ParentMM()
		{
			printf("ParentMM::ParentMM()泛化版本执行了,this = %p\n", this);
		}
	};

	template<typename T,
		     template<typename> typename FirstContainer,
		     template<typename> typename... OtherContainers>
	class ParentMM<T, FirstContainer, OtherContainers...> :private ParentMM<T, OtherContainers...> //偏特化
	{
	public:
		ParentMM()
		{
			printf("ParentMM::ParentMM()偏特化版本执行了，this = %p,sizeof...(OtherContainers)=%d\n", this, sizeof...(OtherContainers));
			m_container.push_back(12);
		}

		FirstContainer<T>  m_container;
	};

	template<typename T,
		     template<typename> typename... Container>
	class myclasst3 :private ParentMM<T, Container...>
	{
	public:
		myclasst3()
		{
			printf("myclasst3::myclasst3()执行了，this = %p,T的类型是:%s，Container参数个数是%d个\n",
				   this, typeid(T).name(), sizeof...(Container));   //以后会用boost库中的type_id_with_cvr<.......>().pretty_name()。
		}
	};

}

int main()
{
	_nmsp1::myclasst<int, float, double> myc;
	//_nmsp1::myclasst<int, float, double> myc(12,13.5,23);

	_nmsp2::myclasst2<12, 18, 23> myc2;
	//_nmsp3::myclasst3<int, vector, list, deque> myc3;

	return 0;
}