#include "MvdCalcPixel.h"
#include "TRandom3.h"

std::vector<MvdPixel> MvdCalcPixel::GetPixels(double inx, double iny, double inz,
					      double outx, double outy, double outz,
					      double energy)
{
	_in.setXYZ(inx, iny, 0);
	_out.setXYZ(outx, outy, 0);

	_dir = _out - _in;
	_pos = _in;
	if (_dir.length() < 0.001){	 	//1 m
		CalcStartPixel();
		_activePixel.SetCharge(energy/3.61);
		_pixels.push_back(_activePixel);
		return _pixels;
	}
	CalcConMatrix();
	ApplyConMatrix();
	if (_verboseLevel > 1){
		std::cout << "Converted Vectors: " << std::endl;
		std::cout << _in << _out << _dir << _pos << std::endl;
	}
	CalcQuadrant();
	CalcCperL(energy);
	
	CalcStartPixel();

	_stop = false;
	while (_stop != true){
		CalcPixel();
		int col = _activePixel.GetCol();
		int row = _activePixel.GetRow();
		switch (_nextPixel) {
			case U : _activePixel.SetRow(++row); break;
			case D : _activePixel.SetRow(--row); break;
			case L : _activePixel.SetCol(--col); break;
			case R : _activePixel.SetCol(++col); break;
			case PixelUNDEF : std::cout << "MvdCalcPixel::GetPixels no next pixel."<<std::endl; break;
		}
		_activePixel.SetCharge(-1.0);
	}

	ConvertPixels();
	return _pixels;
}
void MvdCalcPixel::CalcConMatrix()
{
	if (_in.getX() < 0)
		_con.setX(-1);
	else _con.setX(1);
	if (_in.getY() < 0)
		_con.setY(-1);
	else _con.setY(1);
}

void MvdCalcPixel::ApplyConMatrix()
{
	_in.setX(_con.getX() * _in.getX());
	_in.setY(_con.getY() * _in.getY());
	
	_out.setX(_con.getX() * _out.getX());
	_out.setY(_con.getY() * _out.getY());
	
	_pos.setX(_con.getX() * _pos.getX());
	_pos.setY(_con.getY() * _pos.getY());
	
	_dir.setX(_con.getX() * _dir.getX());
	_dir.setY(_con.getY() * _dir.getY());
		
}

void MvdCalcPixel::CalcQuadrant()
{
	if (_dir.getX() > 0)
		if (_dir.getY() > 0)
			_quad = UR;
		else
			_quad = DR;
	else if (_dir.getY() > 0)
		_quad = UL;
	else
		_quad = DL;

	if (_verboseLevel > 1){
		std::cout << "CalcQuadrant: " << _quad << std::endl;
	}
}

void MvdCalcPixel::CalcCperL(double Energy)
{
	double Charge = Energy / 3.61;
	_CperL = Charge / _dir.length();	
}

void MvdCalcPixel::CalcStartPixel ()
{
	int col = int(_in.getX() / _pixelLength);
	int row = int(_in.getY() / _pixelWidth);
	_activePixel.SetCol(col);
	_activePixel.SetRow(row);
}

void MvdCalcPixel::CalcPixel()
{
	double borderX = 0;
	double borderY = 0;
	CbmGeoVector OutPoint;
	bool XbeforeY = false;
	
	switch (_quad){
		case UR : borderX = _pixelLength * (_activePixel.GetCol()+1);
			  borderY = _pixelWidth  * (_activePixel.GetRow()+1);
			  if ( ( (borderX - _pos.getX()) * _dir.getY() / _dir.getX() )+_pos.getY() < borderY)
			  	XbeforeY = true;
			  else XbeforeY = false;
			  break;
		case UL : borderX = _pixelLength * (_activePixel.GetCol());
			  borderY = _pixelWidth  * (_activePixel.GetRow()+1);
			  if ( ( (borderX - _pos.getX()) * _dir.getY() / _dir.getX() )+_pos.getY() < borderY)
			  	XbeforeY = true;
			  else XbeforeY = false;
			  break;
		case DL : borderX = _pixelLength * (_activePixel.GetCol());
			  borderY = _pixelWidth  * (_activePixel.GetRow());
			  if ( ( (borderX - _pos.getX()) * _dir.getY() / _dir.getX() )+_pos.getY() > borderY)
			  	XbeforeY = true;
			  else XbeforeY = false;
			  break;
		case DR : borderX = _pixelLength * (_activePixel.GetCol()+1);
			  borderY = _pixelWidth  * (_activePixel.GetRow());
			  if ( ( (borderX - _pos.getX()) * _dir.getY() / _dir.getX() )+_pos.getY() > borderY)
			  	XbeforeY = true;
			  else XbeforeY = false;
			  break;
		case QuadUNDEF : std::cout<<"MvdCalcPixel::CalcPixel : Quadrant not defined!"<<std::endl;
		//default : ErrMsg(fatal) << " Quadrant not defined! " << endmsg;
	}
	
	if ( XbeforeY ) {	//Is borderX reached berfore borderY
		OutPoint.setXYZ(borderX, ((borderX-_pos.getX())*_dir.getY()/_dir.getX())+_pos.getY(),0);
		
		if (_quad == UR || _quad == DR)
			_nextPixel = R;
		else _nextPixel = L;
	}
	else {
		OutPoint.setXYZ(((borderY-_pos.getY())*_dir.getX()/_dir.getY())+_pos.getX(), borderY,0);
		
		if (_quad == UR || _quad == UL)
			_nextPixel = U;
		else _nextPixel = D;
	}	
//	ErrMsg(warning) << " OutPoint: " << OutPoint << endmsg;
	
			
	if ((OutPoint - _pos).length() > (_out - _pos).length()){
		_stop = true;
		OutPoint = _out;
	}
		
	double DepCharge = (OutPoint - _pos).length() * _CperL;
	if (_verboseLevel > 1){
		std::cout << "DepCharge w/o noise: " << DepCharge << std::endl;
	}
	TRandom* rgen = gRandom;//new TRandom3();
	double addNoise = rgen->Gaus(0,_noise);
	DepCharge += addNoise;
	
	if (_verboseLevel > 1){
		std::cout << "DepCharge w noise " << _noise << ": " << DepCharge << " " << addNoise << std::endl;
	}

	_pos = OutPoint;
	
	if (DepCharge > _threshold){
		_activePixel.SetCharge(DepCharge);
	}
	else
		_activePixel.SetCharge(0);

	_pixels.push_back(_activePixel);
	
	if (_verboseLevel > 1){
		std::cout << _activePixel << std::endl;
	}
			
}

void MvdCalcPixel::ConvertPixels()
{
	int col, row;
	for (uint i = 0; i < _pixels.size(); i++){
		col = _pixels[i].GetCol();
		row = _pixels[i].GetRow();
		_pixels[i].SetCol(int(col * _con.getX()));
		_pixels[i].SetRow(int(row * _con.getY()));
	}
		
}

std::ostream& MvdCalcPixel::operator<<(std::ostream& out)
{
	out << "PixelWidth: " << _pixelWidth << " PixelLength: " <<
		_pixelLength << " Threshold: " << _threshold << std::endl;
	
	return out;
}