#include "GPUErrorCheck.h"
#include <DMatrixSIMD.h>

#ifndef _DBG_
#define _DBG_  cout<<__FILE__<<":"<<__LINE__<<" "
#define _DBG__ cout<<__FILE__<<":"<<__LINE__
#endif


//----------------
// printHostTrajectories
//----------------
void printHostTrajectories(TrajectoryBlock *tb){
	int size = sizeof(trajectory_t) * tb->N;
	cout << "Size " << size << endl;
	cudaMemcpyAsync(tb->h, tb->d, size, cudaMemcpyDeviceToHost,
	tb->stream);

	gpuErrchk( cudaPeekAtLastError() );
	gpuErrchk( cudaDeviceSynchronize() );

   cout << "Nsteps:" << endl;
	for(int i = 0; i < tb->N; i++)
	{
		_DBG_ << "Trajectory: " << i << " " << tb->h[i].Nsteps << endl;;
	}
}

//----------------
// printNearestTrajectoryPoint
//----------------
void printNearestTrajectoryPoint(TrajectoryBlock *tb)
{
	int size = tb->Nhits * tb->N;
	int hit_index_h[size];
	cudaMemcpyAsync(hit_index_h, tb->hit_index_d, size * sizeof(int), cudaMemcpyDeviceToHost,
	tb->stream);

	gpuErrchk( cudaPeekAtLastError() );
	gpuErrchk( cudaDeviceSynchronize() );
	
	int M = 16;

	cout << "Indexes (trajectory=" << M << " Nhits="<<tb->Nhits<<"):" << endl;
	for(int i = 0; i < tb->Nhits; i++)
	{
		_DBG_ << hit_index_h[i + tb->Nhits*M] << endl;
	}
}

//----------------
// printResiduals
//----------------
void printResiduals(TrajectoryBlock *tb)
{
	int size = tb->Nhits * tb->N;
	float hit_resi_h[size];
	cudaMemcpyAsync(hit_resi_h, tb->hit_resi_d, size * sizeof(float), cudaMemcpyDeviceToHost,
	tb->stream);

	gpuErrchk( cudaPeekAtLastError() );
	gpuErrchk( cudaDeviceSynchronize() );
	
	/*
	int M = 1;
	float err = 0.1443375673; // FDC sigma = 0.5/sqrt(12)
	cout << "Residuals (trajectory=" << M << " Nhits="<<tb->Nhits<<"):" << endl;
	cout << "resiPy+  resiPy-  diff_resiPy  -- (diff_resiPy/sigma)^2" << endl;
	for(int i = 0; i < tb->Nhits; i++)
	{
		float resiPy = hit_resi_h[i + tb->Nhits*M], resiPyN = hit_resi_h[i + tb->Nhits*(M+1)];
		_DBG_ << resiPy << " " << resiPyN << " Difference: " << resiPy - resiPyN;
		cout << " -- " << pow((resiPy - resiPyN)/state_ds[0]/err,2) << endl;
	}*/
	for(int i = 0; i < tb->Nhits; i++)
	{
		for(int m = 0; m < 11; m++)
		{
			cout << " " << hit_resi_h[m + tb->Nhits*i];
		}
		cout << endl;
	}
}

//----------------
// printMatrix5x5
//----------------
void printMatrix5x5(DMatrix5x5 matrix)
{
	cout << "Matrix elements in 5x5 format" << endl;
	for(int i=0; i<5; i++){
			for(int j=0; j<5; j++){
				char str[256];
				sprintf(str, "%+3.5f",  matrix(i,j));
				int len = 10 - strlen(str);
				if(len<1) len = 1;
				cout << str << string(len, ' ');
			}
			cout << endl;
	}
}

//----------------
// printdSdRMatrix
//----------------
void printdSdRMatrix(TrajectoryBlock *tb)
{
	int size = tb->N *5;
	float dSdR_matrix_h[size];
	cudaMemcpyAsync(dSdR_matrix_h, tb->dSdR_matrix_d, size * sizeof(float), cudaMemcpyDeviceToHost,
	tb->stream);

	gpuErrchk( cudaPeekAtLastError() );
	gpuErrchk( cudaDeviceSynchronize() );
	
	cout << "dSdR Matrix" << endl;
	for(int i=0; i<5; i++){
			for(int j=0; j<tb->Nhits; j++){
				char str[256];
				sprintf(str, "%+3.5f",dSdR_matrix_h[i + 5*j]);
				int len = 10 - strlen(str);
				if(len<1) len = 1;
				cout << str << string(len, ' ');
			}
			cout << endl;
	}
}

//----------------
// printChangeInStateParams
//----------------
void printChangeInStateParams(TrajectoryBlock *tb)
{
	cout << "Printing change in state params (dS_matrix): " << endl;
	for(int i = 0; i < 5; i++)
	{
		cout << tb->dS_matrix_h[i] << endl;
	}
}

//----------------
// printStateParams
//----------------
void printStateParams(state_t &S, char *mess="")
{
	cout << "State Parameters: " << mess << endl;
	cout << "      x : " << S.pos.x << endl;
	cout << "      y : " << S.pos.y << endl;
	cout << "      z : " << S.pos.z << endl;
	cout << "     px : " << S.mom.x << endl;
	cout << "     py : " << S.mom.y << endl;
	cout << "     pz : " << S.mom.z << endl;
	cout << "    q/p : " << S.q_over_p << endl;
	cout << "   mass : " << S.mass << endl;
}