//============================================================================
// Name         : dnatemplatematrixfuncs.hpp
// Author       : Roger Fraser
// Contributors :
// Version      : 1.00
// Copyright    : Copyright 2017 Geoscience Australia
//
//                Licensed under the Apache License, Version 2.0 (the "License");
//                you may not use this file except in compliance with the License.
//                You may obtain a copy of the License at
//
//                http ://www.apache.org/licenses/LICENSE-2.0
//
//                Unless required by applicable law or agreed to in writing, software
//                distributed under the License is distributed on an "AS IS" BASIS,
//                WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//                See the License for the specific language governing permissions and
//                limitations under the License.
//
// Description  : Common functions involving matrix_2d operations
//============================================================================

#ifndef DNATEMPLATEMATRIXFUNCS_H_
#define DNATEMPLATEMATRIXFUNCS_H_

#if defined(_MSC_VER)
	#if defined(LIST_INCLUDES_ON_BUILD)
		#pragma message("  " __FILE__)
	#endif
#endif

#include <algorithm>
#include <functional>
#include <sstream>
#include <string>
#include <vector>
#include <stdlib.h>
#include <math.h>
#include <iostream>

#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>

#include <include/parameters/dnaellipsoid.hpp>
#include <include/config/dnatypes.hpp>
#include <include/math/dnamatrix_contiguous.hpp>
#include <include/measurement_types/dnameasurement.hpp>

using namespace std;
using namespace boost;

using namespace dynadjust::datum_parameters;
using namespace dynadjust::math;

// msr_t_Iterator = vector<measurement_t>::iterator
template<typename msr_t_Iterator>
// Fills upper triangle
void GetDirectionsVarianceMatrix(msr_t_Iterator begin, matrix_2d* vmat)
{
	msr_t_Iterator bmsRecord(begin);
	UINT32 a, angle_count(bmsRecord->vectorCount1 - 1);		// number of directions excluding the RO

	vmat->zero();
	vmat->redim(angle_count, angle_count);

	bmsRecord++;

	for (a=0; a<angle_count; ++a)
	{
		vmat->put(a, a, bmsRecord->scale2);				// derived angle variance
		if (a+1 < angle_count)
			vmat->put(a, a+1, bmsRecord->scale3);		// derived angle covariance
		bmsRecord++;
	}
}

// M = measurement_t, U = matrix_2d
// Fills upper triangle
template<typename msr_t_Iterator>
void GetGPSVarianceMatrix(const msr_t_Iterator begin, matrix_2d* vmat)
{
	msr_t_Iterator bmsRecord(begin);
	UINT32 variance_dim(bmsRecord->vectorCount1 * 3), covariance_dim, cov;
	vmat->zero();
	vmat->redim(variance_dim, variance_dim);

	for (UINT32 var(0), cov_elem; var<variance_dim; var+=3)
	{
		covariance_dim = bmsRecord->vectorCount2 * 3;

		vmat->put(var, var, (bmsRecord++)->term2);				// XX

		vmat->put(var, var+1, bmsRecord->term2);				// XY
		vmat->put(var+1, var+1, (bmsRecord++)->term3);			// YY

		vmat->put(var, var+2, bmsRecord->term2);				// XZ
		vmat->put(var+1, var+2, bmsRecord->term3);				// YZ
		vmat->put(var+2, var+2, (bmsRecord++)->term4);			// ZZ

		for (cov_elem=0; cov_elem<covariance_dim; cov_elem+=3)
		{
			cov = var + 3 + cov_elem;
			vmat->put(var, cov, bmsRecord->term1);				// m11
			vmat->put(var, cov+1, bmsRecord->term2);			// m12
			vmat->put(var, cov+2, (bmsRecord++)->term3);		// m13

			vmat->put(var+1, cov, bmsRecord->term1);			// m21
			vmat->put(var+1, cov+1, bmsRecord->term2);			// m22
			vmat->put(var+1, cov+2, (bmsRecord++)->term3);		// m23

			vmat->put(var+2, cov, bmsRecord->term1);			// m31
			vmat->put(var+2, cov+1, bmsRecord->term2);			// m32
			vmat->put(var+2, cov+2, (bmsRecord++)->term3);		// m33
		}
	}
}

// msr_t_Iterator = vector<measurement_t>::iterator
template<typename msr_t_Iterator>
// Sets values based on upper triangle
void SetDirectionsVarianceMatrix(msr_t_Iterator begin, const matrix_2d& vmat)
{
	msr_t_Iterator bmsRecord(begin);
	UINT32 a, angle_count(bmsRecord->vectorCount1 - 1);		// number of directions excluding the RO

	bmsRecord->scale2 = 0.;	// variance (angle)
	bmsRecord->scale3 = 0.;	// covariance (angle)

	bmsRecord++;

	for (a=0; a<angle_count; ++a)
	{
		bmsRecord->scale2 = vmat.get(a, a);				// derived angle variance
		if (a+1 < angle_count)
			bmsRecord->scale3 = vmat.get(a, a+1);		// derived angle covariance
		else
			bmsRecord->scale3 = 0.;						// not necessary, but in the interest of
														// preventing confusion, set to zero
		bmsRecord++;
	}
}

// msr_t_Iterator = vector<measurement_t>::iterator
template<typename msr_t_Iterator>
// Sets values based on upper triangle
void SetGPSVarianceMatrix(msr_t_Iterator begin, const matrix_2d& vmat)
{
	msr_t_Iterator bmsRecord(begin);
	UINT32 variance_dim(bmsRecord->vectorCount1 * 3), covariance_dim, cov;

	for (UINT32 var(0), cov_elem; var<variance_dim; var+=3)
	{
		covariance_dim = bmsRecord->vectorCount2 * 3;

		(bmsRecord++)->term2 = vmat.get(var, var);				// XX

		bmsRecord->term2 = vmat.get(var, var+1);				// XY
		(bmsRecord++)->term3 = vmat.get(var+1, var+1);			// YY

		bmsRecord->term2 = vmat.get(var, var+2);				// XZ
		bmsRecord->term3 = vmat.get(var+1, var+2);				// YZ
		(bmsRecord++)->term4 = vmat.get(var+2, var+2);			// ZZ

		for (cov_elem=0; cov_elem<covariance_dim; cov_elem+=3)
		{
			cov = var + 3 + cov_elem;
			bmsRecord->term1 = vmat.get(var, cov);				// m11
			bmsRecord->term2 = vmat.get(var, cov+1);			// m12
			(bmsRecord++)->term3 = vmat.get(var, cov+2);		// m13

			bmsRecord->term1 = vmat.get(var+1, cov);			// m21
			bmsRecord->term2 = vmat.get(var+1, cov+1);			// m22
			(bmsRecord++)->term3 = vmat.get(var+1, cov+2);		// m23

			bmsRecord->term1 = vmat.get(var+2, cov);			// m31
			bmsRecord->term2 = vmat.get(var+2, cov+1);			// m32
			(bmsRecord++)->term3 = vmat.get(var+2, cov+2);		// m33
		}
	}
}

template <class T>
void FormCarttoGeoRotationMatrix(const T& latitude, const T& longitude, const T& height,
	matrix_2d& mrotations, const CDnaEllipsoid* ellipsoid, bool CLUSTER=false, const UINT32& n=0)
{
	if (!CLUSTER)
		mrotations.redim(3, 3);

	T coslat(cos(latitude));
	T sinlat(sin(latitude));
	T coslon(cos(longitude));
	T sinlon(sin(longitude));

	T term1_a(ellipsoid->GetSemiMajor() * ellipsoid->GetE1sqd());
	T one_minus_esq(1. - ellipsoid->GetE1sqd());

	T nu_plus_h(primeVertical(ellipsoid, latitude) + height);
	T nu_1minuse2_plus_h((primeVertical(ellipsoid, latitude) * (one_minus_esq) + height));
	T term1_b(term1_a * sinlat * coslat);
	T term1_c(pow((1. - ellipsoid->GetE1sqd() * sinlat * sinlat), 1.5));

	// set up Rotation matrix (geo to cart)
	mrotations.put(n,   n,   (term1_b * coslat * coslon / term1_c) - ((nu_plus_h) * sinlat * coslon));
	mrotations.put(n,   n+1, -(nu_plus_h) * coslat * sinlon);
	mrotations.put(n,   n+2, coslat * coslon);
	mrotations.put(n+1, n,   (term1_b * coslat * sinlon / term1_c) - ((nu_plus_h) * sinlat * sinlon));
	mrotations.put(n+1, n+1, (nu_plus_h) * coslat * coslon);
	mrotations.put(n+1, n+2, coslat * sinlon);
	mrotations.put(n+2, n,   (term1_b * one_minus_esq * sinlat / term1_c) + (nu_1minuse2_plus_h * coslat));
	mrotations.put(n+2, n+1, 0.);
	mrotations.put(n+2, n+2, sinlat);
}

template <class T>
void FormCarttoGeoRotationMatrix_Cluster(const matrix_2d& mpositions, matrix_2d& mrotations,
									  const CDnaEllipsoid* ellipsoid)
{
	mrotations.redim(mpositions.rows(), mpositions.rows());

	for (UINT32 i(0); i<mpositions.rows(); i+=3)
	{
		FormCarttoGeoRotationMatrix<T>(
			mpositions.get(i, 0),
			mpositions.get(i+1, 0),
			mpositions.get(i+2, 0),
			mrotations, ellipsoid, true, i);
	}
}

// Assumes design elements for station 1 and station 2 are always
// -1 and 1 respectively.
template <class T>
void Precision_Adjusted_GNSS_bsl(const matrix_2d& mvariances,
	const UINT32& stn1, const UINT32& stn2,
	matrix_2d* mvariances_mod, bool FILLLOWER=true)
{
	matrix_2d tmp(3, 6);
	mvariances_mod->zero();
	UINT32 i, j, k;

	// 1. Form A * V
	for (i=0, j=0; i<3; ++i, ++j)
	{
		// variance-covariance
		for (j=0; j<3; ++j)
		{
			// stn 11 variance
			tmp.elementadd(i, j, -mvariances.get(stn1+i, stn1+j));
			// stn 21 covariance
			tmp.elementadd(i, j, mvariances.get(stn2+i, stn1+j));
		}
		k=j;
		// variance-covariance
		for (j=0; j<3; ++j, ++k)
		{
			// stn 12 covariance
			tmp.elementadd(i, k, -mvariances.get(stn1+i, stn2+j));
			// stn 22 variance
			tmp.elementadd(i, k, mvariances.get(stn2+i, stn2+j));
		}
	}

	//tmp.trace("A x V", "%.16G ");

	// 2. Form AV * AT (upper triangular variance-covariance)
	for (i=0; i<3; ++i)
		for (j=i; j<3; ++j)
			// Sum the variances & covariances
			mvariances_mod->put(i, j, tmp.get(i, j+3) - tmp.get(i, j));

	if (FILLLOWER)
		mvariances_mod->filllower();

	//mvariances_mod->trace("AV X At", "%.16G ");
}

template <class T>
void Prpagate_Variances_Geo_Cart(const matrix_2d& mvariances, matrix_2d mrotations, matrix_2d* mvariances_mod, bool FORWARD=true)
{
	// the rotation matrix is in the direction geo to cart (forward)
	// so to go cart to geo, perform inverse
	if (!FORWARD)
		mrotations = mrotations.sweepinverse();		// allows negative diagonal terms

	matrix_2d mV(mrotations);
	//mV.multiply(mvariances);		// original variance matrix
	mV.multiply_mkl("N", mvariances, "N");		// original variance matrix
	//mvariances_mod->multiply_square_t(mV, mrotations);
	mvariances_mod->multiply_mkl(mV, "N", mrotations, "T");
}

template <class T>
void PropagateVariances_GeoCart(const matrix_2d mvariances, matrix_2d* mvariances_mod,
								const T& latitude, const T& longitude, const T& height,
								matrix_2d& mrotations,
								const CDnaEllipsoid* ellipsoid, bool GEO_TO_CART, bool CALCULATE_ROTATIONS)
{

	if (CALCULATE_ROTATIONS)
		FormCarttoGeoRotationMatrix<T>(latitude, longitude, height, mrotations, ellipsoid);

	Prpagate_Variances_Geo_Cart<T>(mvariances, mrotations, mvariances_mod, GEO_TO_CART);
}

template <class T>
void PropagateVariances_GeoCart(const matrix_2d mvariances, matrix_2d* mvariances_mod,
								const T& latitude, const T& longitude, const T& height,
								const CDnaEllipsoid* ellipsoid, bool GEO_TO_CART)
{

	matrix_2d mrotations;
	PropagateVariances_GeoCart<T>(mvariances, mvariances_mod,
		latitude, longitude, height,
		mrotations,
		ellipsoid, GEO_TO_CART,
		true);		// Calculate rotations
}

template <class T>
void PropagateVariances_GeoCart_Cluster(const matrix_2d& mvariances, matrix_2d* mvariances_mod,
								const matrix_2d& mpositions, matrix_2d& mrotations,
								const CDnaEllipsoid* ellipsoid, bool GEO_TO_CART, bool CALCULATE_ROTATIONS)
{
	if (CALCULATE_ROTATIONS)
		FormCarttoGeoRotationMatrix_Cluster<T>(mpositions, mrotations, ellipsoid);

	Prpagate_Variances_Geo_Cart<T>(mvariances, mrotations, mvariances_mod, GEO_TO_CART);
}

template <class T>
void PropagateVariances_GeoCart_Cluster(const matrix_2d& mvariances, matrix_2d* mvariances_mod,
								const matrix_2d& mpositions_rad,
								const CDnaEllipsoid* ellipsoid, bool GEO_TO_CART)
{
	matrix_2d mrotations;
	FormCarttoGeoRotationMatrix_Cluster<T>(mpositions_rad, mrotations, ellipsoid);

	Prpagate_Variances_Geo_Cart<T>(mvariances, mrotations, mvariances_mod, GEO_TO_CART);
}

template <class T>
void ScaleMatrix(const matrix_2d mvariances, matrix_2d* mvariances_mod, const matrix_2d& scalars)
{
	//matrix_2d mV(mvariances_mod->multiply(scalars, mvariances));
	matrix_2d mV(mvariances_mod->multiply_mkl(scalars, "N", mvariances, "N"));
	//mvariances_mod->multiply_square_t(mV, scalars);
	mvariances_mod->multiply_mkl(mV, "N", scalars, "T");
}

template <class T>
void ScaleGPSVCV(const matrix_2d& mvariances, matrix_2d* mvariances_mod,
						const T& latitude, const T& longitude, const T& height,
						const CDnaEllipsoid* ellipsoid,
						const T& pScale, const T& lScale, const T& hScale)
{
	matrix_2d mrotations(3, 3);
	PropagateVariances_GeoCart<T>(mvariances, mvariances_mod,
		latitude, longitude, height, mrotations, ellipsoid,
		false, 		// Geographic -> Cartesian ?
		true);		// create the rotation matrix

	matrix_2d var_scalars(3, 3);
	var_scalars.put(0, 0, sqrt(pScale));
	var_scalars.put(1, 1, sqrt(lScale));
	var_scalars.put(2, 2, sqrt(hScale));
	ScaleMatrix<T>(*mvariances_mod, mvariances_mod, var_scalars);

	PropagateVariances_GeoCart<T>(*mvariances_mod, mvariances_mod,
		latitude, longitude, height, mrotations, ellipsoid,
		true,		// Geographic -> Cartesian ?
		false);		// don't create a rotation matrix
}

template <class T>
// coordType is passed so that ScaleGPSVCV_Cluster knows whether
// mvariances needs to be propagated to geographic first.  Hence,
// if coordType == LLH_type_i, no propagation is undertaken
void ScaleGPSVCV_Cluster(const matrix_2d& mvariances, matrix_2d* mvariances_mod,
						const matrix_2d& mpositions, const CDnaEllipsoid* ellipsoid,
						const T& pScale, const T& lScale, const T& hScale, _COORD_TYPE_ coordType=XYZ_type_i)
{
	matrix_2d mrotations;
	// form rotation matrix
	FormCarttoGeoRotationMatrix_Cluster<T>(mpositions, mrotations, ellipsoid);

	// Don't propagate if already in geographic
	if (coordType == XYZ_type_i)
		// propagate variances in cartesian system to geographic
		PropagateVariances_GeoCart_Cluster<T>(mvariances, mvariances_mod,
			mpositions, mrotations, ellipsoid,
			false, 		// Cartesian -> Geographic
			false);		// don't create a rotation matrix

	// scale matrix
	matrix_2d var_scalars(mvariances.rows(), mvariances.columns());

	for (UINT32 r(0); r<var_scalars.rows(); r+=3)
	{
		var_scalars.put(r, r, sqrt(pScale));
		var_scalars.put(r+1, r+1, sqrt(lScale));
		var_scalars.put(r+2, r+2, sqrt(hScale));
	}

	// perform the scaling
	ScaleMatrix<T>(*mvariances_mod, mvariances_mod, var_scalars);

	// propagate variances in geographic system to cartesian
	PropagateVariances_GeoCart_Cluster<T>(*mvariances_mod, mvariances_mod,
		mpositions, mrotations, ellipsoid,
		true, 		// Geographic -> Cartesian
		false);		// don't create a rotation matrix
}

template <class T>
void FormLocaltoCartRotationMatrix(const T& latitude, const T& longitude,
	matrix_2d& mrotations, bool LOCAL_TO_CART=true)
{
	mrotations.redim(3, 3);

	T coslat(cos(latitude));
	T sinlat(sin(latitude));
	T coslon(cos(longitude));
	T sinlon(sin(longitude));

	if (LOCAL_TO_CART)
	{
		// set up Rotation matrix for local to cart
		mrotations.put(0, 0, -sinlon);
		mrotations.put(0, 1, -sinlat*coslon);
		mrotations.put(0, 2, coslat*coslon);
		mrotations.put(1, 0, coslon);
		mrotations.put(1, 1, -sinlat*sinlon);
		mrotations.put(1, 2, coslat*sinlon);
		mrotations.put(2, 0, 0.);
		mrotations.put(2, 1, coslat);
		mrotations.put(2, 2, sinlat);
	}
	else
	{
		// set up Rotation matrix for cart to local, which is
		// transpose of local to cart!
		mrotations.put(0, 0, -sinlon);
		mrotations.put(0, 1, coslon);
		mrotations.put(0, 2, 0.);
		mrotations.put(1, 0, -sinlat*coslon);
		mrotations.put(1, 1, -sinlat*sinlon);
		mrotations.put(1, 2, coslat);
		mrotations.put(2, 0, coslat*coslon);
		mrotations.put(2, 1, coslat*sinlon);
		mrotations.put(2, 2, sinlat);
	}
}

template <class T>
void FormLocaltoPolarRotationMatrix(const T& azimuth, const T& elevation, const T& distance,
	matrix_2d& mrotations, bool LOCAL_TO_POLAR=true)
{
	mrotations.redim(3, 3);

	T cos_azimuth(cos(azimuth));
	T sin_azimuth(sin(azimuth));
	T cos_elevation(cos(elevation));
	T sin_elevation(sin(elevation));

	if (LOCAL_TO_POLAR)
	{
		// set up Jacobian matrix for local to polar
		mrotations.put(0, 0, cos_azimuth/distance);
		mrotations.put(0, 1, -sin_azimuth/distance);
		mrotations.put(0, 2, 0.);
		mrotations.put(1, 0, -sin_azimuth*sin_elevation/distance);
		mrotations.put(1, 1, -cos_azimuth*sin_elevation/distance);
		mrotations.put(1, 2, cos_elevation/distance);
		mrotations.put(2, 0, sin_azimuth*cos_elevation);
		mrotations.put(2, 1, cos_azimuth*cos_elevation);
		mrotations.put(2, 2, sin_elevation);
	}
	else
	{
		// Set up Jacobian matrix for polar to local, which is
		// transpose of local to polar!
		mrotations.put(0, 0, cos_azimuth/distance);
		mrotations.put(0, 1, -sin_azimuth*sin_elevation/distance);
		mrotations.put(0, 2, sin_azimuth*cos_elevation);
		mrotations.put(1, 0, -sin_azimuth/distance);
		mrotations.put(1, 1, -cos_azimuth*sin_elevation/distance);
		mrotations.put(1, 2, cos_azimuth*cos_elevation);
		mrotations.put(2, 0, 0.);
		mrotations.put(2, 1, cos_elevation/distance);
		mrotations.put(2, 2, sin_elevation);
	}
}

template <class T>
void PropagateVariances_CartLocal_Diagonal(const matrix_2d& mvariances, matrix_2d& mvariances_mod,
								  const T& latitude, const T& longitude,
								  matrix_2d& mrotations, bool CALCULATE_ROTATIONS=false)
{
	if (CALCULATE_ROTATIONS)
		FormLocaltoCartRotationMatrix<T>(latitude, longitude, mrotations);

	//matrix_2d mrotations_T(mrotations.rows(), mrotations.columns());
	matrix_2d rtv(mrotations.rows(), mrotations.columns());

	//mrotations_T.transpose(mrotations);

	mvariances_mod.redim(3, 3);

	// RtV
	//rtv.multiply(mrotations_T, mvariances);
	rtv.multiply_mkl(mrotations, "T", mvariances, "N");

	UINT32 row, col, i;

	// RtVR
	for (row=0; row<3; ++row) {
		for (col=0; col<3; ++col) {
			mvariances_mod.put(row, col, 0.0);
			// diagonals only
			if (row != col)
				continue;
			for (i=0; i<3; ++i)
				mvariances_mod.elementadd(row, col, rtv.get(row, i) * mrotations.get(i, col));
		}
	}
}

template <class T>
void PropagateVariances_LocalPolar_Diagonal(const matrix_2d& mvariances, matrix_2d& mvariances_mod,
								  const T& azimuth, const T& elevation, const T& distance,
								  matrix_2d& mrotations, bool CALCULATE_ROTATIONS=false)
{
	if (CALCULATE_ROTATIONS)
		FormLocaltoPolarRotationMatrix<T>(azimuth, elevation, distance, mrotations);

	matrix_2d mrotations_T(mrotations.rows(), mrotations.columns());
	matrix_2d rtv(mrotations.rows(), mrotations.columns());

	mrotations_T.transpose(mrotations);

	mvariances_mod.redim(3, 3);

	// RtV
	//rtv.multiply(mrotations, mvariances);
	rtv.multiply_mkl(mrotations, "N", mvariances, "N");

	UINT32 row, col, i;

	// RtVR
	for (row=0; row<3; ++row) {
		for (col=0; col<3; ++col) {
			mvariances_mod.put(row, col, 0.0);
			// diagonals only
			if (row != col)
				continue;
			for (i=0; i<3; ++i)
				mvariances_mod.elementadd(row, col, rtv.get(row, i) * mrotations_T.get(i, col));
		}
	}
}

template <class T>
void PropagateVariances_LocalCart(const matrix_2d& mvariances, matrix_2d& mvariances_mod,
								  const T& latitude, const T& longitude, bool LOCAL_TO_CART,
								  matrix_2d& mrotations, bool CALCULATE_ROTATIONS=false)
{
	if (CALCULATE_ROTATIONS)
		FormLocaltoCartRotationMatrix<T>(latitude, longitude, mrotations);

	// form transpose, from either passed in matrix or newly formed matrix
	matrix_2d mrotations_T(mrotations.rows(), mrotations.columns());
	mrotations_T.transpose(mrotations);

	// the rotation matrix is in the direction local to cart (forward)
	if (LOCAL_TO_CART)
	{
		// Vc = R * Vl * RT
		//matrix_2d mV(mvariances_mod.multiply(mrotations, mvariances));
		matrix_2d mV(mvariances_mod.multiply_mkl(mrotations, "N", mvariances, "N"));
		//mvariances_mod.multiply_square(mV, mrotations_T);
		mvariances_mod.multiply_mkl(mV, "N", mrotations, "T");
	}
	else
	{
		// Vc = R-1 * Vc * [R-1]T
		//    = RT * Vc * R (since R is orthogonal)
		//matrix_2d mV(mvariances_mod.multiply(mrotations_T, mvariances));
		matrix_2d mV(mvariances_mod.multiply_mkl(mrotations, "T", mvariances, "N"));
		//mvariances_mod.multiply_square(mV, mrotations);
		mvariances_mod.multiply_mkl(mV, "N", mrotations, "N");
	}
}

template <class T>
void PropagateVariances_LocalCart(const matrix_2d& mvariances, matrix_2d& mvariances_mod,
								  const T& latitude, const T& longitude, bool LOCAL_TO_CART)
{
	matrix_2d mrotations;

	PropagateVariances_LocalCart<T>(mvariances, mvariances_mod,
		latitude, longitude, LOCAL_TO_CART,
		mrotations, true);	// calculate rotations
}

template <class T>
void Rotate_LocalCart(const matrix_2d mvector, matrix_2d* mvector_mod,
								  const T& latitude, const T& longitude)
{
	matrix_2d mrotations(3, 3);
	FormLocaltoCartRotationMatrix<T>(latitude, longitude, mrotations);

	mvector_mod->redim(3, 1);
	//mvector_mod->multiply(mrotations, mvector);
	mvector_mod->multiply_mkl(mrotations, "N", mvector, "N");
}

template <class T>
void Rotate_CartLocal(const matrix_2d mvector, matrix_2d* mvector_mod,
								  const T& latitude, const T& longitude)
{
	// Helps
	T sin_lat(sin(latitude));
	T cos_lat(cos(latitude));
	T sin_long(sin(longitude));
	T cos_long(cos(longitude));

	mvector_mod->redim(3, 1);

	mvector_mod->put(0, 0, -sin_long * mvector.get(0,0) + cos_long * mvector.get(1,0));
	mvector_mod->put(1, 0, -sin_lat * cos_long * mvector.get(0,0) -
		sin_lat * sin_long * mvector.get(1,0) +
		cos_lat * mvector.get(2,0));
	mvector_mod->put(2, 0, cos_lat * cos_long * mvector.get(0,0) +
		cos_lat * sin_long * mvector.get(1,0) +
		sin_lat * mvector.get(2,0));
}

template <class T>
void Rotate_LocalPolar(const matrix_2d mvector, matrix_2d* mvector_mod,
								  const T& azimuth, const T& elevation, const T& distance)
{
	// Helps
	T sin_azimuth(sin(azimuth));
	T cos_azimuth(cos(azimuth));
	T sin_elevation(sin(elevation));
	T cos_elevation(cos(elevation));

	mvector_mod->redim(3, 1);

	mvector_mod->put(0, 0, -cos_azimuth * mvector.get(0,0) - sin_azimuth * mvector.get(1,0));
	mvector_mod->put(1, 0, -sin_elevation * sin_azimuth * mvector.get(0,0) -
		sin_elevation * cos_azimuth * mvector.get(1,0) +
		cos_elevation * mvector.get(2,0));
	mvector_mod->put(2, 0, cos_elevation * sin_azimuth * mvector.get(0,0) +
		cos_elevation * cos_azimuth * mvector.get(1,0) +
		sin_elevation * mvector.get(2,0));
}

template <class T>
// rotx is x rotation in radians
// roty is y rotation in radians
// rotz is z rotation in radians
void FormHelmertRotationMatrix(const T& rotx, const T& roty, const T& rotz, matrix_2d& mrotations, bool RIGOROUS=false)
{
	mrotations.redim(3, 3);

	// Which rotation matrix is required?
	// Convert to seconds for the test
	if (RIGOROUS)
	{
		// rigorous formula for large (> 10 seconds) rotations
		mrotations.put(0, 0, cos(roty) * cos(rotz));
		mrotations.put(0, 1, cos(roty) * sin(rotz));
		mrotations.put(0, 2, -sin(roty));
		mrotations.put(1, 0, (sin(rotx) * sin(roty) * cos(rotz)) - (cos(rotx) * sin(rotz)));
		mrotations.put(1, 1, (sin(rotx) * sin(roty) * sin(rotz)) + (cos(rotx) * cos(rotz)));
		mrotations.put(1, 2, sin(rotx) * cos(roty));
		mrotations.put(2, 0, (cos(rotx) * sin(roty) * cos(rotz)) + (sin(rotx) * sin(rotz)));
		mrotations.put(2, 1, (cos(rotx) * sin(roty) * sin(rotz)) - (sin(rotx) * cos(rotz)));
		mrotations.put(2, 2, cos(rotx) * cos(roty));
	}
	else
	{
		mrotations.put(0, 0, 1.);
		mrotations.put(0, 1, rotz);
		mrotations.put(0, 2, -roty);
		mrotations.put(1, 0, -rotz);
		mrotations.put(1, 1, 1.);
		mrotations.put(1, 2, rotx);
		mrotations.put(2, 0, roty);
		mrotations.put(2, 1, -rotx);
		mrotations.put(2, 2, 1.);
	}
}

template <class T>
void ReduceParameters(const T* parameters, T* reduced_parameters, const T& elapsedTime, bool DYNAMIC=true)
{
	// translations (reduce to metres)
	reduced_parameters[0] = parameters[0] / 1000.;
	reduced_parameters[1] = parameters[1] / 1000.;
	reduced_parameters[2] = parameters[2] / 1000.;
	// scale
	reduced_parameters[3] = parameters[3] / 1E9;
	// rotations
	reduced_parameters[4] = parameters[4];
	reduced_parameters[5] = parameters[5];
	reduced_parameters[6] = parameters[6];

	if (DYNAMIC)
	{
		// apply rates to translations
		reduced_parameters[0] += parameters[7] / 1000. * elapsedTime;
		reduced_parameters[1] += parameters[8] / 1000. * elapsedTime;
		reduced_parameters[2] += parameters[9] / 1000. * elapsedTime;
		// apply rate to scale
		reduced_parameters[3] += parameters[10] / 1E9 * elapsedTime;
		// apply rates to rotations
		reduced_parameters[4] += parameters[11] * elapsedTime;
		reduced_parameters[5] += parameters[12] * elapsedTime;
		reduced_parameters[6] += parameters[13] * elapsedTime;
	}

	// reduce rotations from milli-arc-seconds to radians
	reduced_parameters[4] = SecondstoRadians(reduced_parameters[4]) / 1000.;
	reduced_parameters[5] = SecondstoRadians(reduced_parameters[5]) / 1000.;
	reduced_parameters[6] = SecondstoRadians(reduced_parameters[6]) / 1000.;
}

template <class T>
// No check or safe guard in place to test if mcoordinates_mod is a reference to mcoordinates
void TransformCartesian(const matrix_2d& mcoordinates, matrix_2d& mcoordinates_mod,
	const matrix_2d& parameters, const matrix_2d& mrotations)
{
	// Add rotation and scale contributions
	for (UINT16 i(0), j; i<3; ++i)
	{
		// Initialise 'to datum' matrix
		mcoordinates_mod.put(i, 0, 0.0);

		for (j=0; j<3; ++j)		// For each column in the row
			mcoordinates_mod.elementadd(i, 0, mrotations.get(i, j) * mcoordinates.get(j, 0));	// Sum partial products

		// Apply scale
		mcoordinates_mod.elementmultiply(i, 0, 1.0 + parameters.get(3, 0));

		// Add translations
		mcoordinates_mod.elementadd(i, 0, parameters.get(i, 0));
	}
}

template <class T>
void Transform_7parameter(const matrix_2d& mcoordinates, matrix_2d& mcoordinates_mod, const T parameters[])
{
	mcoordinates_mod.redim(3, 1);

	bool RIGOROUS(false);
	if (parameters[4] > 10. || parameters[5] > 10. || parameters[6] > 10.)
		RIGOROUS = true;

	// Form rotation matrix
	matrix_2d mrotations(3, 3);
	FormHelmertRotationMatrix<T>(parameters[4], parameters[5], parameters[6], mrotations, RIGOROUS);

	// Put translations and scale into reducedParameters
	matrix_2d mtrans_scale(4, 1, parameters, 4);

	// Transform
	TransformCartesian<T>(mcoordinates, mcoordinates_mod,
		mtrans_scale, mrotations);

}

template <typename T>
void PositionalUncertainty(const T& semimajor, const T& semiminor, const T& azimuth, const T& sdHt,
						   T& hzPosU_Radius, T& vtPosU_Radius)
{
	hzPosU_Radius = vtPosU_Radius = -1.;
	if (semimajor < 0.0)
		return;
	if (semiminor < 0.0)
		return;

	// Horizontal
	T c(semiminor / semimajor);
	T K(HPOS_UNCERT_Q0 + (HPOS_UNCERT_Q1 * c) + (HPOS_UNCERT_Q2 * (c * c)) + (HPOS_UNCERT_Q3 * (c * c * c)));
	T R(semimajor * K);

	hzPosU_Radius = R;

	// Vertical
	vtPosU_Radius = sdHt * 1.96;
}

template <typename T>
T PedalVariance(const matrix_2d& mvariance, const T& direction)
{
	T cos_theta(cos(direction));
	T sin_theta(sin(direction));
	return mvariance.get(0, 0) * cos_theta * cos_theta +
		mvariance.get(1, 1) * sin_theta * sin_theta +
		2. * mvariance.get(0, 1) * cos_theta * sin_theta;
}

template <typename T>
void ErrorEllipseParameters(const matrix_2d& mvariance, T& semimajor, T& semiminor, T& azimuth)
{
	semimajor = semiminor = azimuth = -1.;

	if (mvariance.rows() < 2)
		return;
	if (mvariance.columns() < 2)
		return;

	T e2(mvariance.get(0, 0));
	T n2(mvariance.get(1, 1));
	T en(mvariance.get(0, 1));
	T e2_plus_n2(e2 + n2);
	T e2_minus_n2(e2 - n2);
	T n2_minus_e2(n2 - e2);

	T W((e2_minus_n2 * e2_minus_n2) + (4. * en * en));

	if (W < 0.0)
	{
		if (fabs(W) > PRECISION_1E15)
			return;								// temp term cannot be negative!!!
		else
			W = 0.0;
	}

	T a_sqd(0.5 * (e2_plus_n2 + sqrt(W)));		// semi-major
	T b_sqd(0.5 * (e2_plus_n2 - sqrt(W)));		// semi-minor

	if (a_sqd < 0.0)
		return;									// lamda2 term cannot be negative!!!
	if (b_sqd < 0.0)
		return;									// lamda2 term cannot be negative!!!

	semimajor = sqrt(a_sqd);
	semiminor = sqrt(b_sqd);

	// Compute the azimuth of the semi-major axis
	if (fabs(e2 - n2) < PRECISION_1E25)
	{
		if (en < PRECISION_1E25)
			azimuth = 0.;			// ellipse is a circle
		else
			azimuth = PI / 4.;		// azimuth = 45
	}
	else
	{
		T x(en + en);
		azimuth = 0.5 * atan_2(x, n2_minus_e2);
	}
}

#endif /* DNATEMPLATEMATRIXFUNCS_H_ */


#include <iostream>
#include <Eigen/Eigenvalues>
#include <Eigen/Dense>
#include <vector>
#include <math.h>
#include <cmath>
#include <fstream>
#include <string>
#include <vector>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/fpfh.h>

#include <Config.hpp>

int main(int argc, char *argv[])
{
    // *Parse Config
    Config config;
    std::string input_pcd = config.input_pcd;
    std::string output_pcd = config.output_pcd;

    // *Read point cloud
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::io::loadPCDFile(input_pcd, *cloud);
    std::cout << "Number of points in the input cloud is:" << cloud->points.size() << std::endl;

    // *Create the filtering object
    // pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
    // sor.setInputCloud(cloud);
    // sor.setLeafSize(0.1f, 0.1f, 0.1f);
    // sor.filter(*cloud);

    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Normals(new pcl::PointCloud<pcl::PointXYZRGBA>);
    Normals->resize(cloud->size());

    // *K nearest neighbor search
    int KNumbersNeighbor = 10; // numbers of neighbors 7 , 120
    std::vector<int> NeighborsKNSearch(KNumbersNeighbor);
    std::vector<float> NeighborsKNSquaredDistance(KNumbersNeighbor);

    int *NumbersNeighbor = new int[cloud->points.size()];
    pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
    kdtree.setInputCloud(cloud);
    pcl::PointXYZRGBA searchPoint;

    double *SmallestEigen = new double[cloud->points.size()];
    double *MiddleEigen = new double[cloud->points.size()];
    double *LargestEigen = new double[cloud->points.size()];

    double *DLS = new double[cloud->points.size()];
    double *DLM = new double[cloud->points.size()];
    double *DMS = new double[cloud->points.size()];
    double *Sigma = new double[cloud->points.size()];

    //  ************ All the Points of the cloud *******************
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        searchPoint.x = cloud->points[i].x;
        searchPoint.y = cloud->points[i].y;
        searchPoint.z = cloud->points[i].z;

        if (kdtree.nearestKSearch(searchPoint, KNumbersNeighbor, NeighborsKNSearch, NeighborsKNSquaredDistance) > 0)
        {
            NumbersNeighbor[i] = NeighborsKNSearch.size();
        }
        else
        {
            NumbersNeighbor[i] = 0;
        }

        float Xmean;
        float Ymean;
        float Zmean;
        float sum = 0.00;
        // *Computing Covariance Matrix
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += cloud->points[NeighborsKNSearch[ii]].x;
        }
        Xmean = sum / NumbersNeighbor[i];
        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += cloud->points[NeighborsKNSearch[ii]].y;
        }
        Ymean = sum / NumbersNeighbor[i];
        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += cloud->points[NeighborsKNSearch[ii]].z;
        }
        Zmean = sum / NumbersNeighbor[i];

        float CovXX;
        float CovXY;
        float CovXZ;
        float CovYX;
        float CovYY;
        float CovYZ;
        float CovZX;
        float CovZY;
        float CovZZ;

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].x - Xmean) * (cloud->points[NeighborsKNSearch[ii]].x - Xmean));
        }
        CovXX = sum / (NumbersNeighbor[i] - 1);

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].x - Xmean) * (cloud->points[NeighborsKNSearch[ii]].y - Ymean));
        }
        CovXY = sum / (NumbersNeighbor[i] - 1);

        CovYX = CovXY;

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].x - Xmean) * (cloud->points[NeighborsKNSearch[ii]].z - Zmean));
        }
        CovXZ = sum / (NumbersNeighbor[i] - 1);

        CovZX = CovXZ;

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].y - Ymean) * (cloud->points[NeighborsKNSearch[ii]].y - Ymean));
        }
        CovYY = sum / (NumbersNeighbor[i] - 1);

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].y - Ymean) * (cloud->points[NeighborsKNSearch[ii]].z - Zmean));
        }
        CovYZ = sum / (NumbersNeighbor[i] - 1);

        CovZY = CovYZ;

        sum = 0.00;
        for (size_t ii = 0; ii < NeighborsKNSearch.size(); ++ii)
        {
            sum += ((cloud->points[NeighborsKNSearch[ii]].z - Zmean) * (cloud->points[NeighborsKNSearch[ii]].z - Zmean));
        }
        CovZZ = sum / (NumbersNeighbor[i] - 1);

        // *Computing Eigenvalue and EigenVector
        Eigen::Matrix3f Cov;
        Cov << CovXX, CovXY, CovXZ, CovYX, CovYY, CovYZ, CovZX, CovZY, CovZZ;

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(Cov);
        if (eigensolver.info() != Eigen::Success)
            abort();

        double EigenValue1 = eigensolver.eigenvalues()[0];
        double EigenValue2 = eigensolver.eigenvalues()[1];
        double EigenValue3 = eigensolver.eigenvalues()[2];

        double Smallest = 0.00;
        double Middle = 0.00;
        double Largest = 0.00;
        if (EigenValue1 < EigenValue2)
        {
            Smallest = EigenValue1;
        }
        else
        {
            Smallest = EigenValue2;
        }
        if (EigenValue3 < Smallest)
        {
            Smallest = EigenValue3;
        }

        if (EigenValue1 <= EigenValue2 && EigenValue1 <= EigenValue3)
        {
            Smallest = EigenValue1;
            if (EigenValue2 <= EigenValue3)
            {
                Middle = EigenValue2;
                Largest = EigenValue3;
            }
            else
            {
                Middle = EigenValue3;
                Largest = EigenValue2;
            }
        }

        if (EigenValue1 >= EigenValue2 && EigenValue1 >= EigenValue3)
        {
            Largest = EigenValue1;
            if (EigenValue2 <= EigenValue3)
            {
                Smallest = EigenValue2;
                Middle = EigenValue3;
            }
            else
            {
                Smallest = EigenValue3;
                Middle = EigenValue2;
            }
        }

        if ((EigenValue1 >= EigenValue2 && EigenValue1 <= EigenValue3) || (EigenValue1 <= EigenValue2 && EigenValue1 >= EigenValue3))
        {
            Middle = EigenValue1;
            if (EigenValue2 >= EigenValue3)
            {
                Largest = EigenValue2;
                Smallest = EigenValue3;
            }
            else
            {
                Largest = EigenValue3;
                Smallest = EigenValue2;
            }
        }

        SmallestEigen[i] = Smallest;
        MiddleEigen[i] = Middle;
        LargestEigen[i] = Largest;

        DLS[i] = std::abs(SmallestEigen[i] / LargestEigen[i]); // std::abs ( LargestEigen[i] -  SmallestEigen[i] ) ;
        DLM[i] = std::abs(MiddleEigen[i] / LargestEigen[i]);   // std::abs (  LargestEigen[i] - MiddleEigen[i] ) ;
        DMS[i] = std::abs(SmallestEigen[i] / MiddleEigen[i]);  // std::abs ( MiddleEigen[i] -  SmallestEigen[i] ) ;
        Sigma[i] = (SmallestEigen[i]) / (SmallestEigen[i] + MiddleEigen[i] + LargestEigen[i]);
    } // For each point of the cloud

    std::cout << "Computing Sigma is Done! " << std::endl;
    // *Color Map For the difference of the eigen values

    double MaxD = 0.00;
    double MinD = cloud->points.size();

    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        if (Sigma[i] < MinD)
            MinD = Sigma[i];
        if (Sigma[i] > MaxD)
            MaxD = Sigma[i];
    }

    std::cout << "Minimum is :" << MinD << std::endl;
    std::cout << "Maximum  is :" << MaxD << std::endl;

    //   *****************************************
    // *Red and white (khaki)

    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        cloud->points[i].r = 240;
        cloud->points[i].g = 230;
        cloud->points[i].b = 140;
    }

    float step = ((MaxD - MinD) / 100);
    std::vector<Eigen::Vector3f> Edge;
    int EdgeNum = 0;
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        if (Sigma[i] > (MinD + (10 * step)))
        {
            // *Original: 6 * step
            cloud->points[i].r = 255;
            cloud->points[i].g = 0;
            cloud->points[i].b = 0;
            Eigen::Vector3f temp(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
            Edge.push_back(temp);
            EdgeNum++;
        }
    }

    Eigen::Vector3f mu(0.0, 0.0, 0.0);
    float mu_distance = 0.0;
    float mean_distance = 0.0;
    const int N = Edge.size();
    for(size_t i = 0; i < N; i++) {
        mu = mu + Edge[i];
        mean_distance = mean_distance + sqrt(Edge[i].dot(Edge[i]));
    }
    mu = mu / N;
    mu_distance = sqrt(mu.dot(mu));
    mean_distance = mean_distance / N;
    std::cout << "Edge Points Center: " << mu(0) << ' ' << mu(1) << ' ' << mu(2) << std::endl;
    std::cout << "Distance of Edge Points Center: " << mu_distance << std::endl;
    std::cout << "Mean Distance of Edge Points: " << mean_distance << std::endl;

    //   *****************************************

    std::cout << "Number of Edge points  is :" << EdgeNum << std::endl;

    pcl::io::savePCDFileASCII(output_pcd, *cloud);
    std::cerr << "Saved " << cloud->size() << " data points to test_pcd.pcd." << std::endl;

    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    viewer.showCloud(cloud);
    while (!viewer.wasStopped())
    {
    }

    return 0;
}


//
// Created by ziqwang on 04.04.20.
//

#include "InterlockingSolver_Clp.h"
#include "tbb/tbb.h"
#include <Eigen/SparseQR>

#include "ClpInterior.hpp"
#include "ClpSimplex.hpp"
#include "ClpCholeskyDense.hpp"

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::isTranslationalInterlocking(InterlockingSolver_Clp::pInterlockingData &data) {
    vector<EigenTriple> tris;
    Eigen::Vector2i size;

    InterlockingSolver<Scalar>::computeTranslationalInterlockingMatrix(tris, size);

    if (!checkSpecialCase(data, tris, false, size)) {
        return false;
    }

    int num_var = size[1];
    InterlockingSolver<Scalar>::appendAuxiliaryVariables(tris, size);
    InterlockingSolver<Scalar>::appendMergeConstraints(tris, size, false);

    return solve(data, tris, false, size[0], size[1], num_var);
}

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::isRotationalInterlocking(InterlockingSolver_Clp::pInterlockingData &data) {
    vector<EigenTriple> tris;
    Eigen::Vector2i size;

    InterlockingSolver<Scalar>::computeRotationalInterlockingMatrix(tris, size);

//    std::cout << "special case" << std::endl;
    if (!checkSpecialCase(data, tris, true, size)) {
        return false;
    }

    // Ignore this for lpopt - Enlarging the matrix
    int num_var = size[1];
    InterlockingSolver<Scalar>::appendAuxiliaryVariables(tris, size);
    InterlockingSolver<Scalar>::appendMergeConstraints(tris, size, true);

//    std::cout << "solve" << std::endl;
    return solve(data, tris, true, size[0], size[1], num_var);
}

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::checkSpecialCase(pInterlockingData &data,
                                                      vector<EigenTriple> copy_tris,
                                                      bool rotationalInterlockingCheck,
                                                      Eigen::Vector2i copy_size) {

    InterlockingSolver<Scalar>::appendMergeConstraints(copy_tris, copy_size, rotationalInterlockingCheck);

    if (copy_size[0] < copy_size[1]) return true;

    EigenSpMat A(copy_size[0], copy_size[1]);
    A.setFromTriplets(copy_tris.begin(), copy_tris.end());

    Eigen::SparseQR<Eigen::SparseMatrix<double>,
            Eigen::COLAMDOrdering<int> > solver;
    solver.compute(A.transpose());

    if (A.cols() - solver.rank() == 0) {
        return true;
    } else {
        Eigen::VectorXd solution = Eigen::MatrixXd(solver.matrixQ()).rightCols(A.cols() - solver.rank()).col(0);
        std::cout << "||A * x||: " << (A * solution).norm() << ", ||x||: " << solution.norm() << std::endl;
        unpackSolution(data, rotationalInterlockingCheck, solution.data(), copy_size[1]);
        return false;
    }
    return true;
}

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::solve(InterlockingSolver_Clp::pInterlockingData &data, vector<EigenTriple> &tris,
                                           bool rotationalInterlockingCheck,
                                           int num_row,
                                           int num_col,
                                           int num_var) {

    //Problem definition
    //tris is equal to a sparse matrix A, which size is [num_row x num_col]
    //our variables are [x, t], a row vector.
    //x: (size: num_var) is the instant translational and rotational velocity.
    //t: (size: num_col - num_var) is the auxiliary variable.
    //the optimization is formulated as:
    //              min \sum_{i = 0}^{num_row} -t_i
    //  s.t.            A[x, t] >= 0
    //                  1 >= t >= 0
    //                    x \in R
    // Ideally if the structure is interlocking, the objective value should be zero.
    // In practice, due to numerical error, we have to allow a small tolerance for the objective value.

    EigenSpMat spatMat(num_row, num_col);
    spatMat.setFromTriplets(tris.begin(), tris.end());

    CoinPackedMatrix matrix(true, num_row, num_col, spatMat.nonZeros(), spatMat.valuePtr(), spatMat.innerIndexPtr(),
                            spatMat.outerIndexPtr(), spatMat.innerNonZeroPtr());

    // boundaries for rows and colums values

    double *objective = new double[num_col];
    double *rowLower = new double[num_row];
    double *rowUpper = new double[num_row];
    double *colLower = new double[num_col];
    double *colUpper = new double[num_col];

    //objects
    for (size_t id = 0; id < num_col; id++) {
        if (id < num_var)
            objective[id] = 0;
        else
            objective[id] = -1;
    }

    //bound
    for (size_t id = 0; id < num_row; id++)
        rowLower[id] = 0;

    for (size_t id = 0; id < num_row; id++)
        rowUpper[id] = 0;

    for (size_t id = 0; id < num_col; id++) {
        if (id < num_var)
            colLower[id] = -COIN_DBL_MAX;
        else
            colLower[id] = 0;
    }

    for (size_t id = 0; id < num_col; id++) {
        if (id < num_var)
            colUpper[id] = COIN_DBL_MAX;
        else
            colUpper[id] = 1;
    }

    if (type == SIMPLEX) {
        return solveSimplex(data, rotationalInterlockingCheck, num_row, num_col, num_var, matrix, colLower, colUpper, objective, rowLower,
                            rowUpper);
    } else {
        return solveBarrier(data, rotationalInterlockingCheck, num_row, num_col, num_var, matrix, colLower, colUpper, objective, rowLower,
                            rowUpper);
    }
}

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::solveSimplex(pInterlockingData &data,
                                                  bool rotationalInterlockingCheck,
                                                  int num_row,
                                                  int num_col,
                                                  int num_var,
                                                  const CoinPackedMatrix &matrix,
                                                  const double *colLower,
                                                  const double *colUpper,
                                                  const double *objective,
                                                  const double *rowLower,
                                                  const double *rowUpper) {

    ClpSimplex model(true);
    CoinMessageHandler handler;
    handler.setLogLevel(0); //set loglevel to zero will silence the solver
    model.passInMessageHandler(&handler);
    model.newLanguage(CoinMessages::us_en);

    // load problem
    model.loadProblem(matrix, colLower, colUpper, objective, rowLower, rowUpper);

    // set tolerance
    // a experiment discovery: if the structure is interlocking,
    // the maximum "t" (the auxiliary variables) is around tolerance * 10
    // the average of the "t" is around tolerance * 5.
    // it is very useful to use these number to check whether structure is interlocking or not.
    model.setPrimalTolerance(1e-9);
    // Solve
    model.primal();

    // Solution
    const double target_obj_value = model.rawObjectiveValue();
    double *solution = model.primalColumnSolution();
    double *row_solution = model.primalRowSolution();

    double max_sol = 0;
    for (int id = num_var; id < num_col; id++) {
        max_sol = std::max(solution[id], max_sol);
    }

    unpackSolution(data, rotationalInterlockingCheck, solution, num_var);

    double min_row_sol = MAX_FLOAT;
    for (int id = 0; id < num_row; id++) {
        min_row_sol = std::min(row_solution[id], min_row_sol);
    }

    std::cout << "min_row:\t" << min_row_sol; //should be around zero
    std::cout << ",\tmax_t:\t" << std::abs(max_sol); //interlocking if max_t is around zero
    std::cout << ",\taverage_t:\t" << std::abs(target_obj_value) / num_row << std::endl;//interlocking if average_t is around zero
    if (max_sol < 5e-6) {
        return true;
    } else {
        return false;
    }
}

template<typename Scalar>
bool InterlockingSolver_Clp<Scalar>::solveBarrier(pInterlockingData &data,
                                                  bool rotationalInterlockingCheck,
                                                  int num_row,
                                                  int num_col,
                                                  int num_var,
                                                  const CoinPackedMatrix &matrix,
                                                  const double *colLower,
                                                  const double *colUpper,
                                                  const double *objective,
                                                  const double *rowLower,
                                                  const double *rowUpper) {

    ClpInterior int_model;
    ClpCholeskyDense *cholesky = new ClpCholeskyDense();

    CoinMessageHandler handler;
    handler.setLogLevel(0); //set loglevel to zero will silence the solver
    int_model.passInMessageHandler(&handler);
    int_model.newLanguage(CoinMessages::us_en);

    int_model.loadProblem(matrix, colLower, colUpper, objective, rowLower, rowUpper);
    int_model.setCholesky(cholesky);

    int_model.setPrimalTolerance(1e-8);

    int_model.primalDual();

    // Solution
    const double target_obj_value = int_model.rawObjectiveValue();
    double *solution = int_model.primalColumnSolution();
    double max_sol = 0;
    for (int id = num_var; id < num_col; id++) {
        max_sol = std::max(solution[id], max_sol);
    }

    unpackSolution(data, rotationalInterlockingCheck, solution, num_var);

    //verify solution

    double *row_solution = int_model.primalRowSolution();

    double min_row_sol = MAX_FLOAT;
    for (int id = 0; id < num_row; id++) {
        min_row_sol = std::min(row_solution[id], min_row_sol);
    }

    std::cout << "min_row:\t" << min_row_sol; //should be around zero
    std::cout << ",\tmax_t:\t" << std::abs(max_sol); //interlocking if max_t is around zero
    std::cout << ",\taverage_t:\t" << std::abs(target_obj_value) / num_row << std::endl;//interlocking if average_t is around zero
    if (max_sol < 5e-6) {
        return true;
    } else {
        return false;
    }
}

template<typename Scalar>
void InterlockingSolver_Clp<Scalar>::unpackSolution(InterlockingSolver_Clp::pInterlockingData &data,
                                                    bool rotationalInterlockingCheck,
                                                    const double *solution,
                                                    int num_var) {
    data = make_shared<typename InterlockingSolver<Scalar>::InterlockingData>();
    for (pContactGraphNode node: graph->nodes) {
        Vector3 trans(0, 0, 0);
        Vector3 rotate(0, 0, 0);
        Vector3 center = node->centroid;
        if (node->dynamicID != -1) {
            if (rotationalInterlockingCheck) {
                trans = Vector3(solution[node->dynamicID * 6],
                                solution[node->dynamicID * 6 + 1],
                                solution[node->dynamicID * 6 + 2]);
                rotate = -Vector3(solution[node->dynamicID * 6 + 3],
                                  solution[node->dynamicID * 6 + 4],
                                  solution[node->dynamicID * 6 + 5]);
            } else {
                trans = Vector3(solution[node->dynamicID * 3],
                                solution[node->dynamicID * 3 + 1],
                                solution[node->dynamicID * 3 + 2]);
            }
        }

        data->traslation.push_back(trans);
        data->rotation.push_back(rotate);
        data->center.push_back(center);

//        std::cout << node->staticID << ":" << trans.transpose() << ", " << rotate.transpose() << std::endl;
    }
}

void TemporaryFunction_InterlockingSolver_Clp ()
{
    InterlockingSolver_Clp<double> solver(nullptr, nullptr);
}


/* p_integrand.cpp */
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <boost/math/special_functions/gamma.hpp>
#include <algorithm>

extern "C"
{
/* Survival function for R^phi*W */
int RW_marginal_C(double *xval, double phi, double gamma, int n_xval, double *result){
    double tmp2 = pow(gamma/2, phi)/boost::math::tgamma(0.5);
    double tmp1, tmp0, a;
    a = 0.5-phi;

    for(int i=0; i<n_xval; i++){
        tmp1 = gamma/(2*pow(xval[i],1/phi));
        tmp0 = tmp2/(a*xval[i]);
        result[i] = boost::math::gamma_p(0.5L,tmp1) + boost::math::tgamma((long double)(a+1),tmp1)*tmp0-pow(tmp1,a)*exp(-tmp1)*tmp0;
    }
    return 1;
}

/* Marginal distribution function for R^phi*W + epsilon */
int pRW_me_interp_C(double *xval, double *xp, double *surv_p, double tau_sqd, double phi, double gamma, int n_xval, int n_grid, double *result){
    bool tau_bool = (tau_sqd > 0.05);
    double tp[n_grid];
    double integrand_p[n_grid];
    double tmp, tmp_res; /* temporary constant */
    double tmp_sum = 0; /* temporary trapesoid sum */
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double sd_const_pi =sqrt(2*M_PI)*sd;
    int i,j, tmp_int; /* iterative constants */

    for (i = 0; i < n_xval; i++) {
        if(tau_bool & (xval[i]<820)){
            /* Calculate integrand on a grid */
            for(j=0; j<n_grid;j++){
                tmp = xval[i]-xp[j];
                tp[j] = tmp;
                integrand_p[j] = exp(-tmp*tmp/(2*tau_sqd)) * surv_p[j];
            }

            /* Numerical integral using the trapesoid method */
            for(j=0; j<(n_grid-1);j++){
                tmp_sum+= (tp[j+1]-tp[j])*(integrand_p[j] + integrand_p[j+1])/2;
            }
            tmp_res = 0.5*erfc(-xval[i]/sd_const)-tmp_sum/sd_const_pi;
            tmp_sum = 0;

            /* CDF value must be greater than 0 */
            if(tmp_res < 0){
                tmp_res = 0;
            }
            result[i] = tmp_res;
        }
        else{
            tmp_int = RW_marginal_C(&xval[i], phi, gamma, 1, &tmp_res);
            result[i] = 1-tmp_res;
        }
    }

    return 1;
}

/* Transform to uniform scales from RW mixtures */
int RW_me_2_unifs(double *X, double *xp, double *Surv, double tau_sqd, double *phi, double gamma,
                    int n_s, int n_grid, int n_t, double *unifs){
    int tmp_int, X_lookup, Surv_lookup;
    for (int i = 0; i<n_s;i++){
        X_lookup = i*n_t;
        Surv_lookup = i*n_grid;
        tmp_int = pRW_me_interp_C(X+X_lookup, xp, Surv+Surv_lookup, tau_sqd, phi[i], gamma, n_t, n_grid, unifs+X_lookup);
    }

    return 1;
}

/* Get the quantile range for certain probability levels */
int find_xrange_pRW_C(double min_p, double max_p, double min_x, double max_x, double phi, double gamma, double *x_range){
    if (min_x >= max_x){
        printf("Initial value of mix_x must be smaller than max_x.\n");
        exit(EXIT_FAILURE);
    }

    /* First the min */
    double p_min_x;
    int tmp_int;
    tmp_int = 1-RW_marginal_C(&min_x, phi, gamma, 1, &p_min_x);
    while (1-p_min_x > min_p){
        min_x = min_x/2; /* R^phi*W is always positive */
        tmp_int = RW_marginal_C(&min_x, phi, gamma, 1, &p_min_x);
    }

    x_range[0] = min_x;

    /* Now the max */
    double p_max_x;
    tmp_int = RW_marginal_C(&max_x,  phi, gamma, 1, &p_max_x); /* Survival value */
    while (1 - p_max_x < max_p){
        max_x = max_x*2; /* Upper will set to 20 initially */
        tmp_int = RW_marginal_C(&max_x, phi, gamma, 1, &p_max_x);
    }

    x_range[1] = max_x;
    return 1;
}

/* Get the quantile using the bisection method */
double qRW_bisection_C(double p, double phi, double gamma, int n_x){
    double x_range[2];
    int tmp_res = 0;
    tmp_res = find_xrange_pRW_C(p, p, 1.0, 5.0, phi, gamma, x_range);
    double m = (x_range[0]+x_range[1])/2;
    int iter=0;
    double new_F;
    tmp_res = RW_marginal_C(&m, phi, gamma, 1, &new_F);
    double diff = 1-new_F-p;
    while (iter<100 & abs(diff)> 1e-04){
        if (diff>0){
            x_range[1] = m;}
        else{
            x_range[0]=m;}
        m = (x_range[0]+x_range[1])/2;
        tmp_res = RW_marginal_C(&m, phi, gamma, 1, &new_F);
        diff = 1-new_F-p;
        iter += 1;
    }
    return m;
}

/* Get the quantile range for certain probability levels */
int find_xrange_pRW_me_C(double min_p, double max_p, double min_x, double max_x, double *xp, double *surv_p, double tau_sqd, double phi, double gamma, int n_grid, double *x_range){
    if (min_x >= max_x){
        printf("Initial value of mix_x must be smaller than max_x.\n");
        exit(EXIT_FAILURE);
    }

    /* First the min */
    double p_min_x;
    int tmp_int;
    tmp_int = pRW_me_interp_C(&min_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_min_x);
    while (p_min_x > min_p){
        min_x = min_x-40/phi;
        tmp_int = pRW_me_interp_C(&min_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_min_x);
    }

    x_range[0] = min_x;

    /* Now the max */
    double p_max_x;
    tmp_int = pRW_me_interp_C(&max_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_max_x);
    while (p_max_x < max_p){
        max_x = max_x*2; /* Upper will set to 20 initially */
        tmp_int = pRW_me_interp_C(&max_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_max_x);
    }

    x_range[1] = max_x;
    return 1;
}

/* Density function for R^phi*W */
int RW_density_C(double *xval, double phi, double gamma, int n_xval, double *result){
    double tmp2 = pow(gamma/2, phi)/boost::math::tgamma(0.5);
    double tmp1, tmp0, a;
    a = 0.5-phi;

    for(int i=0; i<n_xval; i++){
        tmp1 = gamma/(2*pow(xval[i],1/phi));
        tmp0 = tmp2/(a*pow(xval[i],2));
        result[i] = (boost::math::tgamma((long double)(a+1),tmp1)-pow(tmp1,a)*exp(-tmp1))*tmp0;
    }
    return 1;
}

/* Get the quantile using Newton-Raphson method */
double qRW_newton_C(double p, double phi, double gamma, int n_x){
    double x_range[2];
    int tmp_res = 0;
    tmp_res = find_xrange_pRW_C(p, p, 1.0, 5.0, phi, gamma, x_range);
    double new_x, current_x = x_range[0];
    int iter=0;
    double error=1;
    double Surv_value, f_value;

    while (iter<400 & error> 1e-08){
        tmp_res = RW_marginal_C(&current_x , phi, gamma, 1, &Surv_value);
        tmp_res = RW_density_C(&current_x , phi, gamma, 1, &f_value);
        new_x = current_x - (1-Surv_value-p)/f_value;
        error = abs(new_x-current_x);
        iter += 1;
        current_x = fmax(x_range[0], new_x);
        if(current_x == x_range[0]){current_x = qRW_bisection_C(p, phi, gamma, 100);}
    }

    return current_x;
}

/* Marginal density function for R^phi*W + epsilon */
int dRW_me_interp_C(double *xval, double *xp, double *den_p, double tau_sqd, double phi, double gamma, int n_xval, int n_grid, double *result){
    double thresh_large = 820;
    if(tau_sqd < 1) {
        thresh_large = 50;
    }
    bool tau_bool = (tau_sqd > 0.05);

    double tp[n_grid];
    double integrand_p[n_grid];
    double tmp, tmp_res; /* temporary constant */
    double tmp_sum = 0; /* temporary trapesoid sum */
    double sd = sqrt(tau_sqd);
    double sd_const_pi =sqrt(2*M_PI)*sd;
    int i,j, tmp_int; /* iterative constants */

    for (i = 0; i < n_xval; i++) {
        if(tau_bool & (xval[i]<thresh_large)){
            /* Calculate integrand on a grid */
            for(j=0; j<n_grid;j++){
                tmp = xval[i]-xp[j];
                tp[j] = tmp;
                integrand_p[j] = exp(-tmp*tmp/(2*tau_sqd)) * den_p[j];
            }

            /* Numerical integral using the trapesoid method */
            for(j=0; j<(n_grid-1);j++){
                tmp_sum+= (tp[j+1]-tp[j])*(integrand_p[j] + integrand_p[j+1])/2;
            }
            tmp_res = tmp_sum/sd_const_pi;
            tmp_sum = 0;
            result[i] = tmp_res;
        }else if((tau_bool & (xval[i]>=thresh_large))|(!tau_bool & (xval[i]>0))){
            tmp_int = RW_density_C(&xval[i], phi, gamma, 1, &tmp_res);
            result[i] = tmp_res;
        }else{
            result[i] = 0;
        }
    }

    return 1;
}

int density_interp_grid(double *xp, double *phi, double gamma, int n_phi, int n_grid, double *Den, double *Surv){
    int counter = 0;
    int i,j, tmp_int;
    double tmp_surv, tmp_den;
    double tmp2, tmp1, tmp0, a, tmp_incomp, tmp_phi, tmp_phi_inv, tmp_xp;
    double gamma_half = gamma/2;

    for(i=0; i<n_phi; i++){
        tmp_phi = phi[i];
        a = 0.5-tmp_phi;
        tmp2 = std::pow(gamma_half, tmp_phi)/(a*sqrt(M_PI));
        tmp_phi_inv = 1/tmp_phi;
        for(j=0; j<n_grid; j++){
            tmp_xp = xp[j];
            tmp1 = gamma_half/std::pow(tmp_xp,tmp_phi_inv);
            tmp0 = tmp2/tmp_xp;
            tmp_incomp = (boost::math::tgamma((long double)(a+1),tmp1)-std::pow(tmp1,a)*exp(-tmp1))*tmp0;
            Surv[counter] = boost::math::gamma_p(0.5L,tmp1) + tmp_incomp;
            Den[counter++] = tmp_incomp/tmp_xp;
        }
    }
    return 1;
}

double dgev_C(double y, double loc, double scale, double shape, bool log_out){
    double t = std::pow(1+shape*((y-loc)/scale), -1/shape);
    double result;
    if(log_out){
        result = -log(scale)+(shape+1)*log(t)-t;
    }else{
        result = std::pow(t, shape+1)*exp(-t)/scale;
    }
    return result;
}

double dnorm_C(double y, double mean, double sd, bool log_out){
    double t=(y-mean)/sd;
    double result;
    if(log_out){
        result = -0.5*log(2*M_PI)-log(sd)-0.5*t*t;
    }else{
        result = exp(-0.5*t*t)/(sqrt(2*M_PI)*sd);
    }
    return result;
}

/* Thresh_X and Thresh_X_above are required */
/* xp, den_p and surv_p are required */
/* Calculate column_wise in C order OR one time  */
double marg_transform_data_mixture_me_likelihood_C(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double *phi, double gamma,
                                                double *xp, double *Den, int n_s, int n_grid){
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double ll=0;
    double RW_den;
    int i, tmp_int, Den_lookup;

    for (i=0; i<n_s; i++){
        if(cen[i]){
            ll += log(0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else if(cen_above[i]){
            ll += log(1-0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else{
            Den_lookup = i*n_grid;
            tmp_int = dRW_me_interp_C(&X[i], xp, &Den[Den_lookup], tau_sqd, phi[i], gamma, 1, n_grid, &RW_den);
            ll += dnorm_C(X[i], X_s[i], sd, true)+dgev_C(Y[i], Loc[i], Scale[i], Shape[i], true)-log(RW_den);
        }
    }
    return ll;
}

/* Calculate row_wise in F order OR one location */
double marg_transform_data_mixture_me_likelihood_F(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double phi, double gamma,
                                                double *xp, double *den_p, int n_t, int n_grid){
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double ll=0;
    double RW_den;
    int i, tmp_int;

    for (i=0; i<n_t; i++){
        if(cen[i]){
            ll += log(0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else if(cen_above[i]){
            ll += log(1-0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else{
            tmp_int = dRW_me_interp_C(&X[i], xp, den_p, tau_sqd, phi, gamma, 1, n_grid, &RW_den);
            ll += dnorm_C(X[i], X_s[i], sd, true)+dgev_C(Y[i], Loc[i], Scale[i], Shape[i], true)-log(RW_den);
        }
    }
    return ll;
}

/* Calculate all locations and all times */
double marg_transform_data_mixture_me_likelihood_global(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double *phi, double gamma,
                                                double *xp, double *Den, int n_s, int n_t, int n_grid){

    double ll=0;
    int site, tmp_int, Den_lookup, X_lookup;

    for (site=0; site<n_s; site++){
        X_lookup = site*n_t;
        Den_lookup = site*n_grid;
        ll += marg_transform_data_mixture_me_likelihood_F(&Y[X_lookup], &X[X_lookup], &X_s[X_lookup], &cen[X_lookup], &cen_above[X_lookup], &Loc[X_lookup], &Scale[X_lookup], &Shape[X_lookup], tau_sqd, phi[site], gamma, xp, &Den[Den_lookup], n_t, n_grid);
    }
    return ll;
}

void print_c(double *Y, int n_grid){
    printf("%4.2f %4.2f\n",*Y,*(Y+n_grid-1));
}

double print_Vec(double *Y, int n_grid, int n_s){
    int Den_lookup;
    for(int i=0; i<n_s; i++){
        Den_lookup = i*n_grid;
        print_c(&Y[Den_lookup], n_grid);
    }

    return Y[0];
}

}


//
// Created by lynn on 2020/7/19.
//
//#include <acado/acado_optimal_control.hpp>
#include <acado/acado_toolkit.hpp>
#include "ros/ros.h"
#include <chrono>
#include "iomanip"
#include <acado/acado_gnuplot.hpp>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include "offb_posctl/controlstate.h"
//#include <Eigen/Eigen>// This step will generate error about eigen ,the reason is not clear by far
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <thread>
#include <math.h>
#include <stdlib.h>
#include <FILTER.h>

using namespace Eigen;
using namespace std;

geometry_msgs::PoseStamped pos_ref;         //无人机参考位置

mavros_msgs::State current_state;           //无人机当前状态(mode arm)
sensor_msgs::Imu   imu_drone;               //读入的无人机的IMU信息 包括姿态角和线加速度

geometry_msgs::PoseStamped  pos_drone;      //读入的无人机当前位置
geometry_msgs::PoseStamped  pos_drone_last; //读入的无人机上一次位置
geometry_msgs::PoseStamped  fused_drone; //融合了vicon数据的无人机位姿，注意是NED坐标下的，需要翻转

geometry_msgs::TwistStamped vel_drone;      //读入的无人机当前速度

geometry_msgs::Vector3 acc_receive;         //读入的无人机线加速度
geometry_msgs::Vector3 angle_receive;       //读入的无人机姿态（欧拉角）
geometry_msgs::Vector3 angle_fromvicon;       //读入的vicon计算的sgement姿态（欧拉角）
geometry_msgs::Vector3 angle_fromvicon_qua;       //读入的vicon计算的sgement姿态（欧拉角）
geometry_msgs::Vector3 angle_fromviconinit;  //读入的vicon计算的sgement initial 姿态（欧拉角）
geometry_msgs::Vector3 fusedangle_receive;

geometry_msgs::Quaternion orientation_target;   //发给无人机的姿态指令

geometry_msgs::Vector3 angle_des;            //线性模型输出的理想值
//geometry_msgs::Vector3 angle_dis;            //DOB控制器估计的扰动值
geometry_msgs::Vector3 angle_target;            //经DOB控制器作用后的实际系统输入值
geometry_msgs::Vector3 vel_target, vel_read, vel_read2;
geometry_msgs::Vector3 currentRPYangle;   //欧拉角
geometry_msgs::Vector3 filteredPlaneVelmsg;   //欧拉角
// debug data
geometry_msgs::Vector3 vel_vicon;
const float MAX_POSITION_MEASURE_ERROR = 1;
Eigen::Quaterniond current_quaternion(0,0,0,0);

bool pose_initialized = false;
bool vel_initialized = false;
int posvel_updatesynflag= false;// the variable to make sure the pos and vel used in ocp is synchronized, In reality, we can put pos and vel in one topic to avoid this problem
USING_NAMESPACE_ACADO
VariablesGrid state, parameter, control;// used for output result
returnValue resultofsolve;
int timeconsumption=0;
float t_end=2;
int pointnumber=20;// the number is almost always 20. It less, the accuracy won't be enough, if more, the time consumpiton will be too large.
int controlfreq=50;
int discretizedpointpersecond=(int)pointnumber/t_end;
float px_ini = -3.0;
float pz_ini = 0.0;
float vx_ini = -0.1;
float vz_ini = 0.0;
float theta_ini = 0.0;
float rate_ini = 0.0;
float vxPlane_ini=0.0,vyPlane_ini=0.0,vzPlane_ini=0.0;
float aircoeffx=1.5,aircoeffz=0.35;
float carPlaneDistance=3.0;
offb_posctl::controlstate controlstate_msg;
bool currentupdateflag= false;
FILTER filterDroneVelx(150);
sensor_msgs::Imu drone_imu;
void do_process()
{
    chrono::time_point<chrono::steady_clock> begin_time = chrono::steady_clock::now();
    DifferentialState        px,pz,vx,vz,vxPlane,theta,rate;     // px=xdrone-xcar, pz=zdrone-zcar vx=dot(px),vz=dot(pz)
    Control                  u1,u2          ;     // u1 is thrust, u2 is theta
    DifferentialEquation     f( 0.0, t_end );     // the differential equation
    f << dot(px) == vx;                         // an implementation
    f << dot(pz) == vz;             // of the model equations
    f << dot(vx) == u1*sin(theta)-aircoeffx*vxPlane;                 // for the drone.
    f << dot(vz) == u1*cos(theta)-9.8-aircoeffz*vxPlane;
    f << dot(vxPlane) == u1*sin(theta)-aircoeffx*vxPlane;
    f << dot(theta)==rate;
    f << dot(rate)==u2;
    OCP ocp_(0.0,t_end,pointnumber);
    ocp_.minimizeLagrangeTerm(1*(pz+px*theta)*(pz+px*theta)+3*(px+carPlaneDistance)*(px+carPlaneDistance)+0.5*(u1-9.8)*(u1-9.8)+0.5*u2*u2); // the 1.5 is for the target high(in this gazebo the height of car is 0)
//        ocp_.minimizeLagrangeTerm(1*(pz+px*u2)*(pz+px*u2)+3*(px+carPlaneDistance)*(px+carPlaneDistance)+0.5*(u1-9.8)*(u1-9.8)+0.5*u2*u2); // the 1.5 is for the target high(in this gazebo the height of car is 0)
//    ocp_.minimizeLagrangeTerm((pz-0.61-(0.1*t-px)*u2)*(pz-0.61-(0.1*t-px)*u2)+(0.1*t-px-3)*(0.1*t-px-3)+0.5*u1*u1+0.5*u2*u2);

    ocp_.subjectTo( f                   );     // minimize T s.t. the model,
    ocp_.subjectTo( AT_START, px == px_ini );     // the initial values for s,
    ocp_.subjectTo( AT_START, pz == pz_ini);     // v,
    ocp_.subjectTo( AT_START, vx == vx_ini);     // and m,
    ocp_.subjectTo( AT_START, vz == vz_ini);     // and m,
    ocp_.subjectTo( AT_START, vxPlane == vxPlane_ini);     // and m,
    ocp_.subjectTo( AT_START, theta == theta_ini);     // and m,
    ocp_.subjectTo( AT_START, rate == rate_ini);     // and m,

    ocp_.subjectTo( 0 <= u1 <=  9.8*2   );     // the crol input u,
    ocp_.subjectTo(  -0.628<= theta <= 0.628  );     // and the time horizon T.
    ocp_.subjectTo(  -1<= vxPlane <= 10  );     // and the time horizon T.
    ocp_.subjectTo(  -3<= rate <= 3  );
    ocp_.subjectTo(  -20<= u2 <= 20  );

    OptimizationAlgorithm algorithm(ocp_);     // the optimization algorithm
    algorithm.set(MAX_NUM_ITERATIONS,50);// default value is 1000
    algorithm.set(MAX_NUM_QP_ITERATIONS,100);//default value is 10000
    algorithm.set(KKT_TOLERANCE,1e-3);// default value is 1e-6
    algorithm.set(PRINTLEVEL,NONE);// do not print solution process.
    try {
        resultofsolve=algorithm.solve();                        // solves the problem.
        algorithm.getControls(control);
        algorithm.getDifferentialStates(state);
//        cout << "controls:  time   |  controls" <<endl;
//        control.print();
    }catch (exception e)
    {
        resultofsolve=-1;
        std::cout<<"ocp throw exception"<<std::endl;
    }
    px.clearStaticCounters();
    pz.clearStaticCounters();
    vx.clearStaticCounters();
    vz.clearStaticCounters();
    vxPlane.clearStaticCounters();
    theta.clearStaticCounters();
    rate.clearStaticCounters();
    u1.clearStaticCounters();
    u2.clearStaticCounters();
    chrono::time_point<chrono::steady_clock> end_time = chrono::steady_clock::now();
    timeconsumption=chrono::duration_cast<chrono::milliseconds>(end_time - begin_time).count();
    cout <<"time acado consume~~...........~~~~~~~~mmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmmm: "<<timeconsumption<< endl;
//    controlcounter=floor(timeconsumption/1000.0*controlfreq)
}

/**match the current state with the newest states solved by OCP, and the time of the nearest state will be the first used point
// in the newest solved input. the return value is the matched index
*/
double tempStateX=0.0,tempStateZ=0.0,tempStateVX=0.0,tempStateVZ=0.0;
int StateMatch()
{
    int k1=1,k2=1,k3=1,k4=1,lefnodeindex=0,rightnodeindex=0,controlcounter=0,mincounter=0;
    float minStateDistance=0.0,currentStateDistance=0.0,stateX=0.0,stateZ=0.0,stateVX=0.0,stateVZ=0.0;
    for(;rightnodeindex<controlstate_msg.stateXarray.size();)
    {
        lefnodeindex=floor(controlcounter*discretizedpointpersecond/controlfreq);
        rightnodeindex=lefnodeindex+1;
        if(rightnodeindex<controlstate_msg.stateXarray.size())
        {
            //interpolition
            stateX= (rightnodeindex-(double)controlcounter*discretizedpointpersecond/controlfreq)*controlstate_msg.stateXarray[lefnodeindex]
                                +((double)controlcounter*discretizedpointpersecond/controlfreq-lefnodeindex)*controlstate_msg.stateXarray[rightnodeindex];

            stateZ =(rightnodeindex-(double)controlcounter*discretizedpointpersecond/controlfreq)*controlstate_msg.stateZarray[lefnodeindex]
                               +((double)controlcounter*discretizedpointpersecond/controlfreq-lefnodeindex)*controlstate_msg.stateZarray[rightnodeindex];

            stateVX= (rightnodeindex-(double)controlcounter*discretizedpointpersecond/controlfreq)*controlstate_msg.stateVXarray[lefnodeindex]
                    +((double)controlcounter*discretizedpointpersecond/controlfreq-lefnodeindex)*controlstate_msg.stateVXarray[rightnodeindex];

            stateVZ =(rightnodeindex-(double)controlcounter*discretizedpointpersecond/controlfreq)*controlstate_msg.stateVZarray[lefnodeindex]
                    +((double)controlcounter*discretizedpointpersecond/controlfreq-lefnodeindex)*controlstate_msg.stateVZarray[rightnodeindex];
            currentStateDistance=k1*pow(stateX-px_ini,2)+k2*pow(stateZ-pz_ini,2)+k3*pow(stateVX-vx_ini,2)+k4*pow(stateVZ-vx_ini,2);
            if(minStateDistance==0)
            {
                minStateDistance=currentStateDistance;
                controlcounter=0;
            } else{
                if(minStateDistance>currentStateDistance)
                {
                    minStateDistance=currentStateDistance;
                    mincounter=controlcounter;
                    tempStateX=stateX;
                    tempStateZ=stateZ;
                    tempStateVX=stateVX;
                    tempStateVZ=stateVZ;
                }
            }
        }
        controlcounter++;
    }
//    std::cout<<"matchedcoutner:---"<<mincounter<<" X:"<<tempStateX<<" Z:"<<tempStateZ<<" VX:"<<tempStateVX<<" VZ:"<<tempStateVZ<<endl;
    return mincounter;
}

void state_cb(const mavros_msgs::State::ConstPtr &msg){
    current_state = *msg;
}

bool hasGotImu = false;
void droneImu_cb(const sensor_msgs::Imu::ConstPtr &msg)
{
    drone_imu=*msg;
    rate_ini=drone_imu.angular_velocity.y;
}

void dronerpy_cb(const geometry_msgs::Vector3::ConstPtr &msg)
{
    currentRPYangle =*msg;
    theta_ini=currentRPYangle.y;
//    std::cout <<"currentRPYangle-----------theta_ini:"<<theta_ini<<endl;
}

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){ //由ConstPtr可以看到msg是指针的引用，因此*msg相当于对被引用的指针对象取值
    if (pose_initialized== false)
    {
        pos_drone_last=*msg;
        pose_initialized=true;
        return;
    }
    if(fabs((*msg).pose.position.x - pos_drone_last.pose.position.x) < MAX_POSITION_MEASURE_ERROR &&
       fabs((*msg).pose.position.y - pos_drone_last.pose.position.y) < MAX_POSITION_MEASURE_ERROR &&
       fabs((*msg).pose.position.z - pos_drone_last.pose.position.z) < MAX_POSITION_MEASURE_ERROR)
    {
        pos_drone_last = pos_drone;
        pos_drone = *msg;
        current_quaternion=Eigen::Quaterniond(pos_drone.pose.orientation.w,pos_drone.pose.orientation.x,pos_drone.pose.orientation.y,pos_drone.pose.orientation.z);
       if(pose_initialized&&vel_initialized)
       {
           if (fabs(pos_drone.header.stamp.nsec-vel_drone.header.stamp.nsec)<30e6)
           {
               posvel_updatesynflag=true;
           }
       }
    } else
    {
        pos_drone_last=*msg;
    }
}

void plane_vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg){
    vel_drone = *msg;
//    vxPlane_ini=filterDroneVelx.filter(vel_drone.twist.linear.x);
    vxPlane_ini=(vel_drone.twist.linear.x);
    filteredPlaneVelmsg.x=vxPlane_ini;
    vyPlane_ini=vel_drone.twist.linear.y;
    vzPlane_ini=vel_drone.twist.linear.z;
}

void relative_postwist_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    px_ini=(*msg).pose.pose.position.x;
    pz_ini=(*msg).pose.pose.position.z;
    vx_ini=(*msg).twist.twist.linear.x;
    vz_ini=(*msg).twist.twist.linear.z;
    currentupdateflag=true;
//    std::cout <<"ocpsolveriniconditons-----------px_ini:  " << px_ini <<"   vx_ini:  " << vx_ini<<"  pz_ini:  " << pz_ini <<"   vz_ini:  " << vz_ini<<
//    "   theta_ini:  " << theta_ini<< "   rate_ini:  " << rate_ini<<std::endl;

}
int main( int argc, char ** argv)
{
    ros::init(argc, argv, "acado_lag_control");
    ros::NodeHandle nh;
    ros::Rate rate(100);// it is the rate to check and process callback functions. the high rate is to avoid the latency to accept the pos and vel
//    //Gazebo 仿真数据
    ros::Subscriber drone_imu_sub=nh.subscribe<sensor_msgs::Imu>("/mavros/imu/data",10,droneImu_cb);
    ros::Subscriber plane_rpy_sub = nh.subscribe<geometry_msgs::Vector3>("drone/current_rpy",10,dronerpy_cb);
    ros::Subscriber car_position_sub = nh.subscribe<nav_msgs::Odometry>("current_relative_postwist",10,relative_postwist_cb); //车的pos+twist
    ros::Subscriber plane_velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>("mavros/local_position/velocity_local", 10, plane_vel_cb); //twist

    // 【发布】飞机姿态/拉力信息 坐标系:NED系
    ros::Publisher controlstate_pub=nh.advertise<offb_posctl::controlstate>("ocp/control_state",10);
    ros::Publisher planeVel_pub=nh.advertise<geometry_msgs::Vector3>("filteredPlaneVel",10);
//  -------------------------------------
    while (ros::ok())
    {
        ros::spinOnce();// to examine the queues of the callback functions once
        if(currentupdateflag)
        {
            do_process();
            if(resultofsolve==SUCCESSFUL_RETURN)
            {
                controlstate_msg.discrepointpersecond=discretizedpointpersecond;
                controlstate_msg.inicounter=0;
                controlstate_msg.arraylength=control.getNumPoints();
                controlstate_msg.thrustarray.clear();
                controlstate_msg.thetaarray.clear();
                controlstate_msg.stateXarray.clear();
                controlstate_msg.stateZarray.clear();
                controlstate_msg.stateVXarray.clear();
                controlstate_msg.stateVZarray.clear();
                for(int i=0;i<control.getNumPoints();i++)
                {
//                    cout<<"control.getNumPoints()-----!!!!:"<<control.getNumPoints()<<"  control.getMatrix(i)(0,0):"<<control.getMatrix(i)(0,0)<<endl;
                    controlstate_msg.thrustarray.push_back((float)control.getMatrix(i)(0,0));
//                    controlstate_msg.thetaarray.push_back((float)control.getMatrix(i)(1,0));
                    controlstate_msg.stateXarray.push_back((float)state.getMatrix(i)(0,0));
                    controlstate_msg.stateZarray.push_back((float)state.getMatrix(i)(1,0));
                    controlstate_msg.stateVXarray.push_back((float)state.getMatrix(i)(2,0));
                    controlstate_msg.stateVZarray.push_back((float)state.getMatrix(i)(3,0));
                    controlstate_msg.thetaarray.push_back((float)state.getMatrix(i)(5,0));

                }
//                ros::spinOnce();// update to newest state to match the state.
//                controlstate_msg.inicounter=StateMatch();//match the state
                controlstate_pub.publish(controlstate_msg);
//                cout<<"controlstate_msg.stateXarray[0]-----------fffffff:"<<controlstate_msg.stateXarray[0]<<"  state.getMatrix(i)(0,0):"<<state.getMatrix(0)(0,0)<<endl;
            }
            currentupdateflag= false;
            planeVel_pub.publish(filteredPlaneVelmsg);
        }
        rate.sleep();
    }

    return 0;
}


#include "pd/edge_length_constraint.h"

#include <Eigen/SparseCore>
#include <array>

namespace pd {

void edge_length_constraint_t::project_wi_SiT_AiT_Bi_pi(q_type const& q, Eigen::VectorXd& b) const
{
    using index_type         = decltype(indices().front());
    index_type const vi      = indices().at(0);
    index_type const vj      = indices().at(1);
    Eigen::Vector3d const p1 = q.block(std::size_t{3u} * vi, 0, 3, 1);
    Eigen::Vector3d const p2 = q.block(std::size_t{3u} * vj, 0, 3, 1);
    auto const N             = q.rows() / 3;

    Eigen::Vector3d const spring = p2 - p1;
    auto const length            = spring.norm();
    Eigen::Vector3d const n      = spring / length;
    auto const delta             = scalar_type{0.5} * (length - d_);

    // find the position p1 which results in ||p2 - p1|| = rest length
    Eigen::Vector3d const pi1 = p1 + delta * n;
    Eigen::Vector3d const pi2 = p2 - delta * n;

    constexpr scalar_type half{0.5};
    constexpr std::size_t three{3};
    // the product wi * (Ai*Si)^T * (Ai*Si) only yields non-zero
    // entries at coordinates [3vi, 3vi+3[ and [3vj, 3vj+3[.
    // The matrices Ai,Bi are differential coordinate matrices
    // which result in mean subtraction in every dimension.
    // Thus, we subtract the mean in every dimension directly
    // instead of performing the matrix multiplication.
    b(three * vi + 0) += wi() * half * (pi1.x() - pi2.x());
    b(three * vi + 1) += wi() * half * (pi1.y() - pi2.y());
    b(three * vi + 2) += wi() * half * (pi1.z() - pi2.z());

    b(three * vj + 0) += wi() * half * (pi2.x() - pi1.x());
    b(three * vj + 1) += wi() * half * (pi2.y() - pi1.y());
    b(three * vj + 2) += wi() * half * (pi2.z() - pi1.z());
}

std::vector<Eigen::Triplet<edge_length_constraint_t::scalar_type>>
edge_length_constraint_t::get_wi_SiT_AiT_Ai_Si(positions_type const& p, masses_type const& M) const
{
    int const vi = static_cast<int>(indices().at(0));
    int const vj = static_cast<int>(indices().at(1));
    auto const N = p.rows();

    // We precompute the product (Ai*Si)^T * (Ai*Si) and find that
    // there are only nonzero elements in the blocks
    // [3vi:3vi+3, 3vi:3vi+3], [3vj:3vj+3, 3vi:3vi+3],
    // [3vj:3vj+3, 3vi:3vi+3], [3vj:3vj+3, 3vj:3vj+3]
    // Those blocks contain the differential coordinates of
    // the mean subtraction differential coordinates Ai.
    // We then multiply by wi as in wi * (Ai*Si)^T * (Ai*Si)
    constexpr scalar_type half{0.5};
    constexpr int three{3};
    std::array<Eigen::Triplet<scalar_type>, 12u> triplets;
    triplets[0] = {three * vi + 0, three * vi + 0, wi() * half};
    triplets[2] = {three * vi + 1, three * vi + 1, wi() * half};
    triplets[4] = {three * vi + 2, three * vi + 2, wi() * half};

    triplets[1] = {three * vj + 0, three * vi + 0, -wi() * half};
    triplets[3] = {three * vj + 1, three * vi + 1, -wi() * half};
    triplets[5] = {three * vj + 2, three * vi + 2, -wi() * half};

    triplets[6]  = {three * vi + 0, three * vj + 0, -wi() * half};
    triplets[8]  = {three * vi + 1, three * vj + 1, -wi() * half};
    triplets[10] = {three * vi + 2, three * vj + 2, -wi() * half};

    triplets[7]  = {three * vj + 0, three * vj + 0, wi() * half};
    triplets[9]  = {three * vj + 1, three * vj + 1, wi() * half};
    triplets[11] = {three * vj + 2, three * vj + 2, wi() * half};

    return std::vector<Eigen::Triplet<scalar_type>>{triplets.begin(), triplets.end()};
}

} // namespace pd


#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
//include the bie header files
#include "material.hh"
#include "precomputed_kernel.hh"
#include "bimat_interface.hh"
#include "infinite_boundary.hh"
//include the fem header files
#include "mesh_Generated.hpp"
#include "bcdof.hpp"
#include "cal_ke.hpp"
#include "cal_M.hpp"
#include "cal_M_global_vec.hpp"
#include "cal_fe_global_const_ke.hpp"
#include "mapglobal.hpp"
#include "Slip_Weakening.hpp"
#include "cal_slip_sliprate.hpp"
#include "time_advance.hpp"
#include "BIE_correct.hpp"

using namespace Eigen;
using namespace std;

int main() {
    // Domain Size
    double x_min = -5e3;
    double x_max = 5e3;
    double y_min = -1.0e3;
    double y_max = 1.0e3;
    int dim = 2.0;
    double dx = 100;
    double dy = 100;
    int nx = (x_max-x_min)/dx;
    int ny = (y_max-y_min)/dy;
    MatrixXd Node = MatrixXd::Zero((nx+1)*(ny+1),2);
    MatrixXd Element = MatrixXd::Zero(nx*ny,4);
    VectorXd fault_surf_nodes = VectorXd::Zero((nx+1),1);
    VectorXd fault_surf_nodes_new = VectorXd::Zero((nx+1),1);
    VectorXd BIE_top_surf_nodes = VectorXd::Zero((nx+1),1);
    VectorXd BIE_bot_surf_nodes = VectorXd::Zero((nx+1),1);
    // Mesh
    mesh_Generated(x_min,x_max,y_min,y_max,dx,dy,nx,ny,Node, Element, fault_surf_nodes, fault_surf_nodes_new, BIE_top_surf_nodes, BIE_bot_surf_nodes);
    int n_nodes = Node.rows();
    int n_el = Element.rows();
    int Ndofn = 2;
    int Nnel = Element.cols();
    nx = fault_surf_nodes.size()-1;
    // Material
    double density = 2670.0;
    double v_s =3.464e3;
    double v_p = 6.0e3;
    double G= pow(v_s,2)*density;
    double Lambda = pow(v_p,2)*density-2.0*G;
    double E  = G*(3.0*Lambda+2.0*G)/(Lambda+G);
    double nu = Lambda/(2.0*(Lambda+G));
    // Time
    double alpha = 0.4;
    double dt = alpha*dx/v_p;
    // Reyleigh Damping
    double beta =0.1;
    double q = beta*dt;
    double time_run = 6.0;
    int numt = time_run/dt;
    //numt = 3;
    VectorXd time = dt*VectorXd::LinSpaced(numt,1,numt);
    // Slip weakening friction parameters
    double Dc = 0.2;
    double mu_d= 0.5;
    double mu_s = 0.6;
    // Intialization
    // disp velocity current and next time step (new)
    VectorXd u_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_n = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd u_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd v_new = VectorXd::Zero(n_nodes*Ndofn,1);
    VectorXd a_n = VectorXd::Zero(n_nodes*Ndofn,1);
    // slip and slip-rate
    VectorXd delt_u_n = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd delt_v_n = VectorXd::Zero(Ndofn*(nx+1),1);
    // Stress on the fault T_0 = intial stress , T = sticking force, T_c= stress critical goes into F_global
    VectorXd T_0 = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd T = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd T_c = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd tau_s = VectorXd::Zero(nx+1,1);
    VectorXd F_ext_global = VectorXd::Zero(n_nodes*Ndofn,1);
    // Setting intial stress on the fault
    for (int i=0; i<T_0.size()/2; i++)
    {
        T_0(2*i+1) = -50.0e6;
    }
    VectorXd x = VectorXd::LinSpaced(nx+1,x_min,x_max);
    for (int i=0 ; i<nx+1; i++)
    {
        if ((x(i)<=(x_max+x_min)/2+0.8e3)&&(x(i)>=(x_max+x_min)/2-0.8e3))
        {
            T_0(2*i) = 31.0e6;
        }
        else
        {
            T_0(2*i) = 27.5e6;

        }
    }
    // Get the index degree of freedome for each element
    VectorXd index_el = VectorXd::Zero(Ndofn*Nnel,1);
    MatrixXd index_store = MatrixXd::Zero(Nnel*Ndofn,n_el);
    for (int i=0;i<n_el;i++)
    {
        bcdof(Element.row(i),dim,index_el);
        index_store.col(i) = index_el;
    }
    VectorXd top_surf_index = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd bot_surf_index = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd BIE_top_surf_index = VectorXd::Zero(Ndofn*(nx+1),1);
    VectorXd BIE_bot_surf_index = VectorXd::Zero(Ndofn*(nx+1),1);
    bcdof(fault_surf_nodes,dim,top_surf_index);
    bcdof(fault_surf_nodes_new,dim,bot_surf_index);
    bcdof(BIE_top_surf_nodes,dim,BIE_top_surf_index);
    bcdof(BIE_bot_surf_nodes,dim,BIE_bot_surf_index);
    // Calculating the Global Mass Vector (lumped mass)
    // Element mass
    double M=density*dx*dy*1.0;
    VectorXd M_el_vec = M/4*VectorXd::Ones(Nnel*Ndofn,1);
    VectorXd M_global_vec=VectorXd::Zero(n_nodes*Ndofn,1);
    cal_M_global_vec(Node, Element, density, index_store, Ndofn, M_global_vec);
//    for (int i=0 ; i<n_el;i++)
//    {
//        index_el = index_store.col(i);
//        mapglobal(index_el,M_global_vec,M_el_vec);
//    }
    // Element matrix
    MatrixXd ke = MatrixXd::Zero(8,8);
    MatrixXd coord = MatrixXd::Zero(4,2);
    VectorXd Element_0= Element.row(0);
    coord.row(0) = Node.row(Element_0(0));
    coord.row(1) = Node.row(Element_0(1));
    coord.row(2) = Node.row(Element_0(2));
    coord.row(3) = Node.row(Element_0(3));
    cal_ke (coord,E,nu,ke);
    // BIE part initiation
    // Setting up the material property for the BIE code
    Material BIE_top_mat = Material(E,nu,density);
    Material BIE_bot_mat = Material(E,nu,density);
    double length = x_max-x_min;
    // infinte bc BIE call infinite_boundary.cc
    PrecomputedKernel h11("kernels/nu_.25_h11.dat");
    PrecomputedKernel h12("kernels/nu_.25_k12.dat");
    PrecomputedKernel h22("kernels/nu_.25_h22.dat");
    InfiniteBoundary BIE_inf_top(length,nx+1,1.0,&BIE_top_mat,&h11,&h12,&h22);
    InfiniteBoundary BIE_inf_bot(length,nx+1,-1.0,&BIE_bot_mat,&h11,&h12,&h22);
    // BIE setting time step
    BIE_inf_top.setTimeStep(dt);
    BIE_inf_bot.setTimeStep(dt);
    // BIE initialization
    BIE_inf_top.init();
    BIE_inf_bot.init();

    //BIE_initiation(E, nu, density, x_max, x_min, nx, dt);
    printf("ready to start\n");
    // Output
    ofstream slip("slip.bin",ios::binary);
    slip.close();
    ofstream slip_rate("slip_rate.bin",ios::binary);
    slip_rate.close();
    ofstream shear("shear.bin",ios::binary);
    shear.close();
    std::ofstream fe("results/fe.txt");

    // Main time loop
    for (int j=0;j<numt;j++)
    {
        // Compute the global internal force
        VectorXd fe_global= VectorXd::Zero(n_nodes*Ndofn,1);
        cal_fe_global_const_ke(n_nodes, n_el, index_store, q, u_n, v_n, Ndofn, ke, fe_global);
        // Friction subroutine
        VectorXd F_fault = VectorXd::Zero(Ndofn*(nx+1),1);
        Slip_Weakening(M_global_vec, top_surf_index, bot_surf_index, fe_global, dt, dx, dy, nx, delt_v_n, delt_u_n, T_0, tau_s, mu_s, mu_d, Dc, Ndofn, M, F_fault, T_c);
        // Calculate the global force vector
        // Adding contribution of the fault force to the global force vector F_total
        VectorXd F_total = F_ext_global-fe_global;
        mapglobal(top_surf_index,F_total,-F_fault);
        mapglobal(bot_surf_index,F_total,F_fault);
        // Central Difference Time integration
        time_advance(u_n, v_n, F_total, M_global_vec, dt);
        // Get the slip and slip rate
        cal_slip_slip_rate(u_n, v_n, top_surf_index, bot_surf_index, Ndofn, nx, delt_u_n, delt_v_n);
        // Correct the BIE surf nodes solutions from FEM with the BIE solution
        BIE_correct(BIE_top_surf_index, BIE_bot_surf_index, fe_global, Ndofn, nx, dx, BIE_inf_top, BIE_inf_bot, u_n, v_n);
        //ofstream file;
        slip.open("slip.bin",ios::binary | ios::app);
        slip.write((char*)(delt_u_n.data()),delt_u_n.size()*sizeof(double));
        slip.close();
        slip_rate.open("slip_rate.bin",ios::binary | ios::app);
        slip_rate.write((char*)(delt_v_n.data()),delt_v_n.size()*sizeof(double));
        slip_rate.close();
        shear.open("shear.bin",ios::binary | ios::app);
        shear.write((char*)(T_c.data()),T_c.size()*sizeof(double));
        shear.close();
        printf("Simulation time = %f\n",time(j));
    }
    return 0;
}


// Copyright (c) 2020 Chris Richardson
// FEniCS Project
// SPDX-License-Identifier:    MIT

#include "crouzeix-raviart.h"
#include "core/element-families.h"
#include "core/polyset.h"
#include "core/quadrature.h"
#include <Eigen/Dense>
#include <numeric>
#include <vector>

using namespace basix;

//-----------------------------------------------------------------------------
FiniteElement basix::create_cr(cell::type celltype, int degree)
{
  if (degree != 1)
    throw std::runtime_error("Degree must be 1 for Crouzeix-Raviart");

  const int tdim = cell::topological_dimension(celltype);
  if (tdim < 2)
    throw std::runtime_error("Tdim must be 2 or 3 for Crouzeix-Raviart");

  const std::vector<std::vector<std::vector<int>>> topology
      = cell::topology(celltype);
  const std::vector<std::vector<int>> facet_topology = topology[tdim - 1];
  const Eigen::ArrayXXd geometry = cell::geometry(celltype);

  const int ndofs = facet_topology.size();
  Eigen::ArrayXXd pts = Eigen::ArrayXXd::Zero(ndofs, tdim);

  // Compute facet midpoints
  int c = 0;
  for (const std::vector<int>& f : facet_topology)
  {
    for (int i : f)
      pts.row(c) += geometry.row(i);
    pts.row(c) /= static_cast<double>(f.size());
    ++c;
  }

  Eigen::MatrixXd dual = polyset::tabulate(celltype, 1, 0, pts)[0];
  int perm_count = tdim == 2 ? 3 : 14;
  std::vector<Eigen::MatrixXd> base_permutations(
      perm_count, Eigen::MatrixXd::Identity(ndofs, ndofs));

  const Eigen::MatrixXd coeffs = compute_expansion_coefficients(
      Eigen::MatrixXd::Identity(ndofs, ndofs), dual);

  // Crouzeix-Raviart has one dof on each entity of tdim-1.
  std::vector<std::vector<int>> entity_dofs(topology.size());
  entity_dofs[0].resize(topology[0].size(), 0);
  entity_dofs[1].resize(topology[1].size(), (tdim == 2) ? 1 : 0);
  entity_dofs[2].resize(topology[2].size(), (tdim == 3) ? 1 : 0);
  if (tdim == 3)
    entity_dofs[3] = {0};

  return FiniteElement(element::family::CR, celltype, 1, {1}, coeffs,
                       entity_dofs, base_permutations, {});
}
//-----------------------------------------------------------------------------