/*
引数:deg_x
*/
static bool Mat33RotX(const TArgInfo &info){
	const TString1D &tmp = info.m_arg;
	if(! (1 <= tmp.size())){
		return false;
	}
	TMatrix		m;
	m.RotX(deg2rad(atof(tmp[0].c_str())));
	Print(m);
	return true;
}

TEST(TMatrix, can_assign_matrices_of_equal_size)
{
    TMatrix<int> m (5);
	TMatrix<int> m1(5);
	for(int i=0; i < m.GetSize(); i++)
		m1[i]=1;

	ASSERT_NO_THROW(m = m1);
	m=m1;
	EXPECT_EQ(m, m1);
}

//---------------------------------------------------------------------------
TMatrix TMatrix::operator~() const //транспонирование матрицы
{                                    // возвращает транспонированную матрицу
                                     // не изменяя собственного объекта
   TMatrix Result (sizeH, sizeV);    //
   for (int i = 0; i < sizeV; i++)
     for (int j = 0; j < sizeH; j++)
     {
        Result.WriteElem (j, i, ReadElem(i,j));
     }
   return Result;
}

void CCharShape::ScaleNode (size_t node_name, const TVector3d& vec) {
	TCharNode *node = GetNode(node_name);
	if (node == NULL) return;

	TMatrix<4, 4> matrix;

	matrix.SetScalingMatrix(vec.x, vec.y, vec.z);
	node->trans = node->trans * matrix;
	matrix.SetScalingMatrix(1.0 / vec.x, 1.0 / vec.y, 1.0 / vec.z);
	node->invtrans = matrix * node->invtrans;

	if (newActions && useActions) AddAction (node_name, 4, vec, 0);
}

// Random biologically meaningful K81 transition matrix with given length.
// Here biologically meaningful means that diagonal entries are maximal in the column they belong to (and the row in this case)
// ( Chang's DLC condition).
void K81_random_edge_bio_length(double len, TMatrix &tm) {
  TMatrix tmaux;
  long i0, i;

  tmaux.resize(4);
  for(i=0; i < 4; i++) {
    tmaux[i].resize(4);
  }

  K81_random_edge_length(len, tmaux);

  // Permute with the row that starts with the largest entry in column.
  // All 4 possible permutations have det = 1
  i0 = max_in_col(tmaux, 0);
  K81_matrix(tmaux[i0][0], tmaux[i0][1], tmaux[i0][2], tmaux[i0][3], tm);
}

QList<SStep::SCandidate> CTSPSolver::findCandidate(const TMatrix &matrix, int &nRow, int &nCol) const
{
    nRow = -1;
    nCol = -1;
QList<SStep::SCandidate> alts;
SStep::SCandidate cand;
double h = -1;
double sum;
    for (int r = 0; r < nCities; r++)
        for (int c = 0; c < nCities; c++)
            if (matrix.at(r).at(c) == 0) {
                sum = findMinInRow(r,matrix,c) + findMinInCol(c,matrix,r);
                if (sum > h) {
                    h = sum;
                    nRow = r;
                    nCol = c;
                    alts.clear();
                } else if ((sum == h) && !hasSubCycles(r,c)) {
                    cand.nRow = r;
                    cand.nCol = c;
                    alts.append(cand);
                }
            }
    return alts;
}

TVector CCharacterCamera::GetThirdPersonCameraPosition()
{
	CCharacter* pCharacter = m_hCharacter;
	if (!pCharacter)
		return TVector(10, 0, 0);

	TVector vecEyeHeight = pCharacter->GetUpVector() * pCharacter->EyeHeight();

	TMatrix mView = TMatrix(pCharacter->GetThirdPersonCameraAngles(), TVector());
	TVector vecThird = pCharacter->GetGlobalTransform().GetTranslation() + vecEyeHeight;
	vecThird -= Vector(mView.GetForwardVector()) * m_flBack;
	vecThird += Vector(mView.GetUpVector()) * m_flUp;
	vecThird += Vector(mView.GetLeftVector()) * m_flSide;

	return vecThird;
}

void CTSPSolver::normalize(TMatrix &matrix) const
{
    for (int r = 0; r < nCities; r++)
        for (int c = 0; c < nCities; c++)
            if ((r != c) && (matrix.at(r).at(c) == INFINITY))
                matrix[r][c] = MAX_DOUBLE;
}

void CCharacter::Jump()
{
	if (!GetGroundEntity())
		return;

	SetGroundEntity(NULL);

	Vector vecLocalUp = GetUpVector();
	if (HasMoveParent())
	{
		TMatrix mGlobalToLocal = GetMoveParent()->GetGlobalToLocalTransform();
		vecLocalUp = mGlobalToLocal.TransformNoTranslate(vecLocalUp);
	}

	SetLocalVelocity(GetLocalVelocity() + vecLocalUp * JumpStrength());
}

void CStructure::PostSetLocalTransform(const TMatrix& m)
{
	BaseClass::PostSetLocalTransform(m);

	CPlanet* pPlanet = GameData().GetPlanet();
	if (!pPlanet)
		return;

	if (!pPlanet->GetChunkManager()->HasGroupCenter())
		return;

	TMatrix mLocalTransform = m;

	CBaseEntity* pMoveParent = GetMoveParent();
	if (pMoveParent)
	{
		while (pMoveParent != pPlanet)
		{
			mLocalTransform = pMoveParent->GetLocalTransform() * mLocalTransform;
			pMoveParent = pMoveParent->GetMoveParent();
		}
	}
	else
		mLocalTransform = pPlanet->GetGlobalToLocalTransform() * m;

	GameData().SetGroupTransform(pPlanet->GetChunkManager()->GetPlanetToGroupCenterTransform() * mLocalTransform.GetMeters());
}

    void minRectTest(unsigned int N, const TMatrix & v) {
        using namespace PointFunctions;
        using namespace TestFunctions;

        dumpPointsMatrixBinary("./MinAreaRectangleTest" + std::to_string(N) +".bin",v);
        dumpPointsMatrix("./MinAreaRectangleTest"+ std::to_string(N) +".txt",v);

        std::cout << "\n\nStart MinAreaRectangle Test "+ std::to_string(N) +"" << std::endl;
        START_TIMER(start)

        MinAreaRectangle c(v);
        c.compute();

        STOP_TIMER_SEC(count, start)
        std::cout << "Timings: " << count << " sec for " <<v.cols() << " points" << std::endl;
        std::cout << "End MinAreaRectangle Test "+ std::to_string(N) +"" << std::endl;
        auto rect = c.getMinRectangle();

        Matrix2Dyn p(2,7);
        p.col(0) =  rect.m_p;
        p.col(1) =  rect.m_p + rect.m_u*rect.m_uL ;
        p.col(2) =  rect.m_p + rect.m_u*rect.m_uL + rect.m_v*rect.m_vL ;
        p.col(3) =  rect.m_p + rect.m_v*rect.m_vL ;
        p.col(4) =  rect.m_p;
        p.col(5) =  rect.m_u;
        p.col(6) =  rect.m_v;

        dumpPointsMatrixBinary("./MinAreaRectangleTest"+ std::to_string(N) +"Out.bin",p);
        dumpPointsMatrix("./MinAreaRectangleTest"+ std::to_string(N) +"Out.txt",p);
    }

    void k_extract(const typename EigenType<T, D>::EigenvalueType& eigen_values, 
        const typename EigenType<T, D>::EigenvectorsType& eigen_vectors, 
        TMatrix& Q, TMatrix& S, int K)
    {
        size_t eigen_num = eigen_values.rows();

        typename std::vector<typename EigenValue<T> > ev;

        for (size_t i = 0; i < eigen_num; i ++)
        {
            typename std::complex<T> cv = eigen_values(i);
            typename EigenValue<T> i_ev(cv.real(), i);
            ev.push_back(i_ev);
        }

        std::sort(ev.begin(), ev.end(), EigenValue<T>());

        int gm = eigen_vectors.rows();
        Q.resize(gm, K);
        TVector s(K);
        
        for (size_t i = 0; i < K; i ++)
        {
            s(i) = ev[i]._value;
            typename MatrixType<std::complex<T>, D>::Matrix q_ci = eigen_vectors.col(ev[i]._idx);
            for (size_t j = 0, j_end = q_ci.rows(); j < j_end; j ++)
            {
                Q(j, i) = q_ci(j).real();
            }
        }

        S = s.asDiagonal();
    }

void TLogReg::IRLS(const TMatrix& Matrix, TFltV& y, TFltV& bb, 
                   const double& ChangeEps, const int& MaxStep, const int& Verb) {
    IAssert(Matrix.GetCols() == y.Len());

    int M = Matrix.GetRows(), R = Matrix.GetCols(), i;
    if (bb.Len() != M+1) { bb.Gen(M+1); bb.PutAll(0.0); }

    TFltV mu(R), w(R), z(R), delta;

    // adjust y
    for (i = 0; i < R; i++) {
        if (y[i] >= 1.0)
            y[i] = 0.999;
        else if (y[i] <= 0.0)
            y[i] = 0.001;
    }

    //const double eps = 0.01;
    double NewDEV = 0.0, OldDEV = -100.0;
    forever {
        Matrix.MultiplyT(bb, z);
        for (i = 0; i < R; i++) {
            z[i] += bb[M];
            // evaluate current model
            mu[i] = 1/(1 + exp(-z[i]));
            // calculate weights
            w[i] = mu[i] * (1 - mu[i]);
            // calculate adjusted dependent variables
            z[i] += (y[i] - mu[i]) / w[i];
        }
        // get new aproximation for bb
        CG(Matrix, w, z, bb, MaxStep, Verb);
        // calculate deviance (error measurement)
        NewDEV = 0.0;
        for (i = 0; i < R; i++) {
            double yi = y[i], mui = mu[i];
            NewDEV += yi*log(yi / mui) + (1 - yi)*log((1 - yi)/(1 - mui));
        }
        
        if (Verb == 1) printf(" -> %.5f\n", NewDEV);
        else if (Verb > 1) printf("NewDEV = %.5f\n", NewDEV);

        // do we stop?
        if (fabs(NewDEV - OldDEV) < ChangeEps) break;
        OldDEV = NewDEV;
    }
}

static void ConjugGrad(const TMatrix& Matrix, const TFltV& b, TFltV& x, 
        const int& CGMxIter, const double& RelErr, const TFltV& x0) {

    // prepare start vector
    x.Gen(Matrix.GetCols());
    if (x0.Empty()) { x.PutAll(0.0); }
    else { x = x0; }
    // do the magic
}

///////////////////////////////////////////////////////////////////////
// Fast-Robust-Logistic-Regression
void TLogReg::CG(const TMatrix& Matrix, const TFltV& w, const TFltV& b, 
                 TFltV& x, const int& MaxStep, const int& Verb) { // x == bb, b == z
    int M = x.Len(), R = b.Len(), i;
    TFltV r(M), p(M), q(M), tmp(R);

    x.PutAll(0.0);
    // calculate right side of system
    for (i = 0; i < R; i++) tmp[i] = w[i] * b[i];
    Matrix.Multiply(tmp, r); r[M-1] = TLAMisc::SumVec(tmp);

    double nro, ro, alpha, beta;
    const double eps = 0.000001;

    // conjugate gradient method - CG
    // from "Templates for the soltuion of linear systems" (M == eye)
    ro = nro = TLinAlg::Norm2(r); int StepN=0;
    for (int k = 1; k <= MaxStep && nro > eps && k <= M; k++) {
        if ((Verb > 1) && (k%10 == 0)) printf(".");
        if (k == 1) {
            p = r;
        } else {
            beta = nro / ro;
            for (i = 0; i < M; i++)
                p[i] = r[i] + beta*p[i];
        }

        // q = A*p = (X'*W*X)*p = (Matrix*W*Matrix')*p
        Matrix.MultiplyT(p, tmp);
        for (i = 0; i < R; i++) tmp[i] = (tmp[i] + p[M-1]) * w[i];
        Matrix.Multiply(tmp, q); q[M-1] = TLAMisc::SumVec(tmp);

        // calcualte new x and residual
        alpha = nro / TLinAlg::DotProduct(p, q);
        for (i = 0; i < M; i++) {
            x[i] = x[i] + alpha * p[i];
            r[i] = r[i] - alpha * q[i];
        }

        ro = nro;
        nro = TLinAlg::Norm2(r);
        StepN=k;
    }
    if (Verb > 1) printf("\nnorm(r) = %.5f at k = %d\n", nro, StepN-1);
}

    void lr_approximate(const TMatrix& G, TMatrix& Q, TMatrix& S, int K, size_t lr_maxitr)
    {
        typename Eigen::EigenSolver<TMatrix> es;
        es.setMaxIterations(lr_maxitr*G.rows());
        es.compute(G);

        const typename EigenType<T, D>::EigenvalueType& eigen_values = es.eigenvalues();
        const typename EigenType<T, D>::EigenvectorsType& eigen_vectors = es.eigenvectors();	
        k_extract<T, D>(eigen_values, eigen_vectors, Q, S, K);
    }

// nRow: The row number
// matrix: The cost matrix
// exc: column to exclude from the min calculation
double CTSPSolver::findMinInRow(int nRow, const TMatrix &matrix, int exc) const {
    double total_min = INFINITY;

    #pragma omp parallel num_threads(numThreads)
    {
        double min = INFINITY;
        #pragma omp for
        for (int k = 0; k < nCities; k++) {
            if (((k != exc)) && (min > matrix.at(nRow).at(k))) {
                min = matrix.at(nRow).at(k);
            }
        }

        #pragma omp critical
        total_min = fmin(min, total_min);
    }

    return (total_min == INFINITY) ? 0 : total_min;
}

//---------------------------------------------------------------------------
TMatrix TMatrix::operator+(TMatrix& B)
{
	int N0,M0;
	B.Dim(&N0,&M0);


	TMatrix C(N,M);
	if (N0!=N && M0!=M)
		for (int i=0;i<N;i++){
			for (int j=0;j<M;j++)
				C.SetElement(i,j,A[i][j]);
	}else{
		for (int i=0;i<N;i++)
			for (int j=0;j<M;j++)
				C.SetElement(i,j,A[i][j]+B.GetElement(i,j));
	}

	return C;

}

/*
 * dorgqr: genarates (M x N) orthogonal matrix Q: A = Q x R
 *
 * @param[in] A tile matrix
 * @param[in] T tile matrix
 * @param[in] Q tile matirx
 *
 */
void dorgqr( const TMatrix A, const TMatrix T, TMatrix& Q )
{
	assert( A.M() == Q.M() );

	const int aMT = A.mt();
	const int aNT = A.nt();
	const int qMT = Q.mt();
	const int qNT = Q.nt();

	for (int tk = min(aMT, aNT)-1; tk+1 >= 1; tk--)
	{
		for (int ti = qMT - 1; ti > tk; ti--)
		{
			#pragma omp parallel for
			for (int tj = tk; tj < qNT; tj++)
			{
				SSRFB( PlasmaLeft, PlasmaNoTrans,
						A(ti,tk), T(ti,tk), Q(tk,tj), Q(ti,tj) );
			}
		}
		#pragma omp parallel for
		for (int tj = tk; tj < qNT; tj++)
		{
			LARFB( PlasmaLeft, PlasmaNoTrans,
					A(tk,tk), T(tk,tk), Q(tk,tj) );
		}
	}
}

//---------------------------------------------------------------------------
void MAT_SMOOTH::StepTo(TMatrix &newX, TMatrix &newQ,
                        TMatrix &curX, TMatrix &curQ,
                        TMatrix &oldX, TMatrix &oldQ)
{
    long double d;
    QT1.madd(oldQ, U_1);
    d = QT1.Inverse();
    while(d==0)
    {
        printf("1 \n");
        QT1.addDiag(1e-5);
        QT1.Inverse();
    }

    QT2.mmul(AT, QT1);
    QT3.mmul(QT2, A);
    QT1 = curQ;
    d = QT1.Inverse();
    while(d==0)
    {
        printf("2 \n");
        QT1.addDiag(1e-5);
        d = QT1.Inverse();
    }

    newQ.madd(QT1, QT3);

    XT1.mmul(QT1, curX);
    XT2.mmul(QT2, oldX);
    XT3.madd(XT1, XT2);
    d = newQ.Inverse();
    while(d==0)
    {
        printf("3 \n");
        newQ.addDiag(1e-5);
        newQ.Inverse();
    }

    newX.mmul(newQ, XT3);
};