/*
 *check M z + q =0
 *
 *
 */
double LinearSystem_computeError(LinearSystemProblem* problem, double *z)
{
  double * pM = problem->M->matrix0;
  double * pQ = problem->q;
  double error = 10;
  int n = problem->size;
  double * res = (double*)malloc(n * sizeof(double));
  memcpy(res, pQ, n * sizeof(double));
  cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, 1.0, pM, n, z, 1, 1.0, res, 1);
  error = cblas_dnrm2(n, res, 1);
  free(res);
  return error;

}

///////////////Problem///////////////
Problem::Problem(long n,std::string structure="diagonal", std::string type = "nice") {

    m_A = Tools::initArray(n*n);
    m_x = Tools::initArray(n);
    m_b = Tools::initArray(n);
    m_n = n;

    double (*f_pntr)(long);

    if ( type.compare("nice") ) {

        f_pntr = randomNumber;

    } else if (type.compare("random")) {

        f_pntr = niceNumber;

    }

    if ( !structure.compare("diagonal") ) {

        for (long ii = 0; ii < m_n; ii++ ) {

            m_A[ii + m_n*ii] = f_pntr(ii);
            m_x[ii] = f_pntr(ii);

        }

    } else if (!structure.compare("dense")) {
    
        for( long ii = 0; ii < m_n*m_n; ii++) {

            m_A[ii] = f_pntr(ii); 

        }

        for( long ii = 0; ii < m_n; ii++) {

            m_x[ii] = f_pntr(ii); 

        }

    } 

    int inc = 1;
    cblas_dgemv(CblasColMajor,CblasNoTrans,m_n,m_n,1.0,m_A,m_n,m_x,inc,0.0,m_b, inc);


}

JNIEXPORT void JNICALL Java_uncomplicate_neanderthal_CBLAS_dgemv
(JNIEnv * env, jclass clazz,
 jint Order, jint TransA,
 jint M, jint N,
 jdouble alpha,
 jobject A, jint lda,
 jobject X, jint incX,
 jdouble beta,
 jobject Y, jint incY) {

  double *cA = (double *) (*env)->GetDirectBufferAddress(env, A);
  double *cX = (double *) (*env)->GetDirectBufferAddress(env, X);
  double *cY = (double *) (*env)->GetDirectBufferAddress(env, Y);
  cblas_dgemv(Order, TransA, M, N, alpha, cA, lda, cX, incX, beta, cY, incY);
};

void method5(int M,int N,int T,double *Xin,double *Xout,double *Kern) {

    omp_set_num_threads(10);
#pragma omp parallel
  {
    int tid = omp_get_thread_num();
    int nthreads=omp_get_num_threads();

    if (tid==0) printf("Using %d threads\n",omp_get_num_threads());

    int i1=((N-T)*tid)/nthreads;
    int i2=((N-T)*(tid+1))/nthreads;
    for (int n=i1; n<i2; n++) {
        cblas_dgemv(CblasColMajor, CblasNoTrans, M, T, 1.0, &Xin[M*n], M, Kern, 1, 0.0, &Xout[M*n], 1);
    }
  }
}

void VectorView::set_to_product(const MatrixView& m, const VectorView& v,
                                const bool transpose)
{
  CBLAS_TRANSPOSE tr;
  if (transpose){
    tr = CblasTrans;
    assert(m.cols() == length());
    assert(m.rows() == v.length());
  } else {
    tr = CblasNoTrans;
    assert(m.cols() == v.length());
    assert(m.rows() == length());
  }

  cblas_dgemv(CblasColMajor, tr, m.rows(), m.cols(), 1.0, m.data(),
              m.stride(), v.data(), 1, 0.0, data_, 1);
}

    /*
     * Class:     com_intel_analytics_bigdl_mkl_MKL
     * Method:    vdgemv
     * Signature: (SSIIID[DII[DIID[DII)V
     */
JNIEXPORT void JNICALL Java_com_intel_analytics_bigdl_mkl_MKL_vdgemv
   (JNIEnv * env, jclass cls, jchar trans, jint m, jint n,
   jdouble alpha, jdoubleArray a, jint aOffset, jint lda, jdoubleArray x,
   jint xOffset, jint incx, jdouble beta, jdoubleArray y, jint yOffset, jint incy) {

   jdouble * jni_a = (*env)->GetPrimitiveArrayCritical(env, a, JNI_FALSE);
   jdouble * jni_x = (*env)->GetPrimitiveArrayCritical(env, x, JNI_FALSE);
   jdouble * jni_y = (*env)->GetPrimitiveArrayCritical(env, y, JNI_FALSE);

   int jni_trans;
   if(trans == 't' || trans == 'T') jni_trans = CblasTrans; else jni_trans = CblasNoTrans;
   cblas_dgemv(CblasColMajor, jni_trans, m, n, alpha, jni_a + aOffset, lda, jni_x + xOffset, incx,
      beta, jni_y + yOffset, incy);

   (*env)->ReleasePrimitiveArrayCritical(env, a, jni_a, 0);
   (*env)->ReleasePrimitiveArrayCritical(env, x, jni_x, 0);
   (*env)->ReleasePrimitiveArrayCritical(env, y, jni_y, 0);
}

double * solve(size_t nfield, double * perm, double h, double lb, double ub)
{
    double * op = buildOP(nfield,perm,h);
    double * rhs = buildRHS(nfield-1,lb+h,ub);
    
    double * inv = calloc_double((nfield-1) * (nfield-1));
    pinv(nfield-1,nfield-1,nfield-1,op,inv,0.0);
    
    double * sol = calloc_double(nfield);
    cblas_dgemv(CblasColMajor,CblasNoTrans,nfield-1,nfield-1,1.0,inv,nfield-1,
                rhs,1,0.0,sol+1,1);

    free(op); op = NULL;
    free(rhs); rhs = NULL;
    free(inv); inv = NULL;

    return sol;
}

void test_dgemv_trans2()
{
    const size_t m=35, n=45;
    double a[m*n];
    double x[m];
    double y[n];
    double z[n];
    size_t i;
    for(i=0; i<m*n; i++) a[i]=i;
    for(i=0; i<n; i++) x[i]=i+m*n;
    for(i=0; i<m; i++) y[i]=z[i]=i*i;

    my_dgemv(CblasRowMajor,CblasTrans,m,n,2.0,a,n,x,1,2.0,y,1);
    cblas_dgemv(CblasRowMajor,CblasTrans,m,n,2.0,a,n,x,1,2.0,z,1);
    for(i=0; i<m; i++){
	assert(y[i]==z[i]);
    }
}

int
gsl_blas_dgemv(CBLAS_TRANSPOSE_t TransA, double alpha,
	       const gsl_matrix * A, const gsl_vector * X, double beta,
	       gsl_vector * Y)
{
    const size_t M = A->size1;
    const size_t N = A->size2;

    if ((TransA == CblasNoTrans && N == X->size && M == Y->size)
	|| (TransA == CblasTrans && M == X->size && N == Y->size)) {
	cblas_dgemv(CblasRowMajor, TransA, INT(M), INT(N), alpha, A->data,
		    INT(A->tda), X->data, INT(X->stride), beta, Y->data,
		    INT(Y->stride));
	return GSL_SUCCESS;
    } else {
	GSL_ERROR("invalid length", GSL_EBADLEN);
    }
}

void test_dgemv()
{
    const size_t m=3, n=4;
    double a[3*4]={
	1,2,3,4,
	5,6,7,8,
	9,10,11,12
    };
    double x[4]={2,1,4,3};
    double y[3]={6,5,7};
    double z[3]={6,5,7};
    size_t i;
    my_dgemv(CblasRowMajor,CblasNoTrans,m,n,2.0,a,n,x,1,2.0,y,1);
    cblas_dgemv(CblasRowMajor,CblasNoTrans,m,n,2.0,a,n,x,1,2.0,z,1);
    for(i=0; i<m; i++){
	assert(y[i]==z[i]);
    }
}

void ReducedLinearForceModel::GetInternalForce(double * q, double * internalForces)
{
  CBLAS_ORDER     order= CblasColMajor;
  CBLAS_TRANSPOSE trans= CblasNoTrans;
  int m = r;
  int n = r;
  double alpha = 1;
  double * a = stiffnessMatrix;
  int lda = r;
  double * x = q;
  int incx = 1;
  double beta = 0;
  double * y = internalForces; 
  int incy = 1;

  cblas_dgemv(order, trans, m, n, alpha, a, lda, x, incx,
                  beta, y, incy);
}

int main(int iArgCnt, char* sArrArgs[])
{
	int iIterationNo = 0;
	double dNormOfResult = 0;
	double dTime0 = 0, dTime1 = 0, dTimeDiff = 0, dMinTimeDiff = DBL_MAX, dMaxTimeDiff = 0;

	parseInputs(iArgCnt, sArrArgs);

	MPI_Init(&iArgCnt, &sArrArgs);
	MPI_Comm_size(MPI_COMM_WORLD, &GiProcessCnt);
	MPI_Comm_rank(MPI_COMM_WORLD, &GiProcessRank);

	initData();

	for(iIterationNo = 0; iIterationNo < GiIterationCnt; iIterationNo++)
	{
		MPI_Barrier(MPI_COMM_WORLD);
		dTime0 = MPI_Wtime();

		cblas_dgemv(CblasRowMajor, CblasNoTrans, GiRowCntForOneProc, GiVectorLength, 1.0, GdArrSubMatrix, GiVectorLength, GdArrVector, 1, 0.0, GdArrSubResult, 1);

		MPI_Barrier(MPI_COMM_WORLD);
		MPI_Gather(GdArrSubResult, GiRowCntForOneProc, MPI_DOUBLE, GdArrTotalResult, GiRowCntForOneProc, MPI_DOUBLE, 0, MPI_COMM_WORLD);

		dTime1 = MPI_Wtime();
		dTimeDiff = (dTime1 - dTime0);

		if(dTimeDiff > dMaxTimeDiff)
			dMaxTimeDiff = dTimeDiff;
		if(dTimeDiff < dMinTimeDiff)
			dMinTimeDiff = dTimeDiff;
	}

	if(GiProcessRank == 0)
	{
		dNormOfResult = cblas_dnrm2(GiVectorLength, GdArrTotalResult, 1);
		printf("Result=%f\nMin Time=%f uSec\nMax Time=%f uSec\n", dNormOfResult, (1.e6 * dMinTimeDiff), (1.e6 * dMaxTimeDiff));
	}

	MPI_Finalize();

	return 0;
}

/** In place prediction of the next state mean and covariances
 */
void predict_forward(kf_t *kf)
{
  //TODO take advantage of sparsity in this function

  double x[kf->state_dim];
  memcpy(x, kf->state_mean, kf->state_dim * sizeof(double));

  //TODO make more efficient via the structure of the transition matrix
  cblas_dgemv(CblasRowMajor, CblasNoTrans, // CBLAS_ORDER, CBLAS_TRANSPOSE
              kf->state_dim, kf->state_dim, // int M, int N,
              1, (double *) kf->transition_mtx, kf->state_dim, // double 1, double *A, int lda
              x, 1, // double *X, int incX
              0, kf->state_mean, 1); // double beta, double *Y, int incY
  // VEC_PRINTF((double *) state_mean, kf->state_dim);

  double state_cov[kf->state_dim * kf->state_dim];
  reconstruct_udu(kf->state_dim, kf->state_cov_U, kf->state_cov_D, state_cov);
  // MAT_PRINTF((double *) state_cov, kf->state_dim, kf->state_dim);

  //TODO make more efficient via the structure of the transition matrix
  double FC[kf->state_dim * kf->state_dim];
  cblas_dsymm(CblasRowMajor, CblasRight, CblasUpper, //CBLAS_ORDER, CBLAS_SIDE, CBLAS_UPLO
              kf->state_dim, kf->state_dim, // int M, int N
              1, state_cov, kf->state_dim, // double alpha, double *A, int lda
              kf->transition_mtx, kf->state_dim, // double *B, int ldb
              0, FC, kf->state_dim); // double beta, double *C, int ldc
  // MAT_PRINTF((double *) FC, kf->state_dim, kf->state_dim);

  //TODO make more efficient via the structure of the transition matrix
  double FCF[kf->state_dim * kf->state_dim];
  memcpy(FCF, kf->transition_cov, kf->state_dim * kf->state_dim * sizeof(double));
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, // CBLAS_ORDER, CBLAS_TRANSPOSE transA, cBLAS_TRANSPOSE transB
              kf->state_dim, kf->state_dim, kf->state_dim, // int M, int N, int K
              1, FC, kf->state_dim, // double alpha, double *A, int lda
              kf->transition_mtx, kf->state_dim, //double *B, int ldb
              1, FCF, kf->state_dim); //beta, double *C, int ldc
  // MAT_PRINTF((double *) FCF, kf->state_dim, kf->state_dim);

  udu(kf->state_dim, FCF, kf->state_cov_U, kf->state_cov_D);
  // MAT_PRINTF((double *) state_cov_U, kf->state_dim, kf->state_dim);
  // VEC_PRINTF((double *) state_cov_D, kf->state_dim);
}

static void FB_compute_H_mlcp(void* data_opaque, double* z, double* w, double* workV1, double* workV2, NumericsMatrix* H)
{
  printf("MLCP FB_compute_H_mlcp not implemented yet");
  exit(1);
#if 0
  MixedLinearComplementarityProblem* data = (MixedLinearComplementarityProblem *)data_opaque;
  unsigned int n = data->size;
  assert(data->M);
  assert(data->M->matrix0);
  double* M = data->M->matrix0;
  double normi;

  // workV1 = "z" in Facchibei--Pang p. 808
  // "z_i" = 1 if z_i = w_i = 0.0
  // M^T.workV1 --> workV2
  cblas_dgemv(CblasColMajor, CblasTrans, n, n, 1.0, M, n , workV1, 1, 0.0, workV2, 1);
  for (unsigned int i = 0; i < n; ++i)
  {
    if (workV1[i] != 0.0) // i in beta
    {
      normi = sqrt(workV1[i] * workV1[i] + workV2[i] * workV2[i]);
      for (unsigned int j = 0; j < n; j++)
      {
        H[j * n + i] = (workV2[i] / normi - 1.0) * M[j * n + i];
      }
      H[i * n + i] += (workV1[i] / normi - 1.0);

    }
    else // i not in beta
    {
      normi = sqrt(z[i] * z[i] + w[i] * w[i]);
      for (unsigned int j = 0; j < n; j++)
      {
        H[j * n + i] = (w[i] / normi - 1.0) * M[j * n + i];
      }
      H[i * n + i] += (z[i] / normi - 1.0);
    }

  }
#endif
}

 void HMM::smoother(int iseq, double* c, double* beta, double* Z) {
     int si = o->sind[iseq];           // start index of the current sequence
     int sl = o->slen[iseq];           // length of the current sequence
     uint32_t* y_ = o->y + si;         // pointer to the current sequence
     
     double one = 1;
     
     // initialize: fill last frame of beta with ones
     vfill(&one, beta + (sl-1)*hs, 1, hs);
     
     for(int t = sl - 2; t > -1; t--) {
         double* out = beta + t*hs;
         vmul(beta + (t+1)*hs, 1, g + y_[t+1], os, Z, 1, hs);
         
         // using out as a temp vector because in cblas_dgemv, x cannot be y
         cblas_dgemv(CblasRowMajor, CblasNoTrans, hs, hs, 1, Q, hs,
                     Z, 1, 0, out, 1);
         
         vsdiv(out, 1, c + t+1, out, 1, hs);
     }
 }

void StVKReducedHessianTensor::ContractWithVector(int r, double * Hq, double * q, double * A)
{
  // computes A = Hq : q

  int quadraticSize = StVKReducedInternalForces::GetQuadraticSize(r);

  // multiply Hq and q
  cblas_dgemv(CblasColMajor, CblasTrans, 
       r, quadraticSize, 
       1.0,
       Hq, r,
       q, 1,
       0.0,
       A, 1);

  for(int j=r-1; j>=0; j--)
    for(int i=r-1; i>=j; i--)
    {
      int lowerTrianglePos = j * r - (j-1) * j / 2 + (i-j);
      A[ELT(r,i,j)] = A[lowerTrianglePos];
      A[ELT(r,j,i)] = A[lowerTrianglePos];
    }
}

/* Numerics Matrix wrapper  for y <- alpha trans(A) x + beta y */
void NM_tgemv(const double alpha, NumericsMatrix* A, const double *x,
              const double beta, double *y)
{
    switch (A->storageType)
    {
    case NM_DENSE:
    {
        cblas_dgemv(CblasColMajor, CblasTrans, A->size0, A->size1,
                    alpha, A->matrix0, A->size0, x, 1, beta, y, 1);
        break;
    }
    case NM_SPARSE_BLOCK:
    case NM_SPARSE:
    {
        CHECK_RETURN(cs_aaxpy(alpha, NM_csc_trans(A), x, beta, y));
        break;
    }
    default:
    {
        assert(0 && "NM_tgemv unknown storageType");
    }
    }
}

// computes means of the rows of A, subtracts them from A, and returns them in meanVec on the root process
// assumes memory has already been allocated for meanVec
void computeAndSubtractRowMeans(double *localRowChunk, double *meanVec, distMatrixInfo *matInfo) {
    int mpi_rank = matInfo->mpi_rank;
    int numcols = matInfo->numcols;
    int localrows = matInfo->localrows;
    int * rowcounts = matInfo->rowcounts;
    int * rowoffsets = matInfo->rowoffsets;
    MPI_Comm *comm = matInfo->comm;

    double *onesVec = (double *) malloc( numcols * sizeof(double));
    double *localMeanVec = (double *) malloc( localrows * sizeof(double));

    for(int idx = 0; idx < numcols; idx = idx + 1) {
        onesVec[idx]=1;
    }
    cblas_dgemv(CblasRowMajor, CblasNoTrans, localrows, numcols, 1.0/((double)numcols), localRowChunk, numcols, onesVec, 1, 0, localMeanVec, 1);
    cblas_dger(CblasRowMajor, localrows, numcols, -1.0, localMeanVec, 1, onesVec, 1, localRowChunk, numcols);
    if (mpi_rank != 0) {
        MPI_Gatherv(localMeanVec, localrows, MPI_DOUBLE, NULL, NULL, NULL, MPI_DOUBLE, 0, *comm);
    } else {
        MPI_Gatherv(localMeanVec, localrows, MPI_DOUBLE, meanVec, rowcounts, rowoffsets, MPI_DOUBLE, 0, *comm);
    }
    free(onesVec);
    free(localMeanVec);
}

AA_API void aa_la_xlsnp( size_t m, size_t n,
                         const double *A, const double *A_star, const double *x,
                         const double *yp, double *y ) {
    aa_la_mvmul(n,m,A_star,x,y);

    double *B = (double*)aa_mem_region_local_alloc( sizeof(double) * n*n );

    // B = A^* A
    cblas_dgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
                 (int)n, (int)n, (int)m,
                 1, A_star, (int)n, A, (int)m, 0, B, (int)n );

    // B =  A^* A - I
    for( size_t i = 0; i < n; i ++ )
        AA_MATREF(B,n,i,i) -= 1;

    // y = y + -B yp
    cblas_dgemv( CblasColMajor, CblasNoTrans, (int)n, (int)n,
                 -1.0, B, (int)n,
                 yp, 1,
                 1, y, 1 );

    aa_mem_region_local_pop( B );
}

void plotMerit(double *z, double psi_k, double descentCondition)
{
  int incx = 1, incy = 1;
  double q_0, q_tk, qp_tk, merit_k;
  /* double tmin = 1e-12; */
  double tk = 1, aux;
  double m1 = 1e-4;
  double Nstep = 0;
  int i = 0;

  FILE *fp;

  (*sFphi)(sN, z, sphi_z, 0);
  aux = cblas_dnrm2(sN, sphi_z, 1);
  /* Computes merit function */
  aux = 0.5 * aux * aux;
  printf("plot psi_z %e\n", aux);


  if (!sPlotMerit)
    return;

  if (sPlotMerit)
  {
    /*    sPlotMerit=0;*/
    strcpy(fileName, "outputLS");


    (*sFphi)(sN, z, sphi_z, 0);
    q_0 =  cblas_dnrm2(sN, sphi_z , incx);
    q_0 = 0.5 * q_0 * q_0;

    fp = fopen(fileName, "w");

    /*    sPlotMerit=0;*/
    tk = 5e-7;
    aux = -tk;
    Nstep = 1e4;
    for (i = 0; i < 2 * Nstep; i++)
    {
      cblas_dcopy(sN, z, incx, sz2, incx);
      cblas_daxpy(sN , aux , sdir_descent , incx , sz2 , incy);
      (*sFphi)(sN, sz2, sphi_z, 0);
      q_tk =  cblas_dnrm2(sN, sphi_z , incx);
      q_tk = 0.5 * q_tk * q_tk;


      (*sFjacobianPhi)(sN, sz2, sjacobianPhi_z, 1);
      /* Computes the jacobian of the merit function, jacobian_psi = transpose(jacobianPhiMatrix).phiVector */
      cblas_dgemv(CblasColMajor,CblasTrans, sN, sN, 1.0, sjacobianPhi_z, sN, sphi_z, incx, 0.0, sgrad_psi_z, incx);
      qp_tk = cblas_ddot(sN, sgrad_psi_z, 1, sdir_descent, 1);

      merit_k = psi_k + m1 * aux * descentCondition;


      fprintf(fp, "%e %.16e %.16e %e\n", aux, q_tk, merit_k, qp_tk);
      if (i == Nstep - 1)
        aux = 0;
      else
        aux += tk / Nstep;
    }

    fclose(fp);
  }
}