#include "MvdCalcStrip.h"
#include "TRandom3.h"

std::vector<MvdPixel> MvdCalcStrip::GetStrips(double inx, double iny, double inz,
					      double outx, double outy, double outz,
					      double energy)
{

//   _energy=energy;

// // one side
// 	_in.setXYZ(inx, iny, 0);
// 	_out.setXYZ(outx, outy, 0);
// 	_dir = _out - _in;
// 	_pos = _in;
// 	if (_dir.length() < 0.001){	 	//1 m
// 		CalcStartStrip();
// 		_activeStrip.SetCharge(energy/3.61);
// 		_strips.push_back(_activeStrip);
// 		return _strips;
// 	}
//   CalcOneSideOfStrips();
// // one side
// 
// // other side
//   _in.setXYZ(inx, iny, 0);
//   _out.setXYZ(outx, outy, 0);
//   _dir = _out - _in;
//   _pos = _in;
//   if (_dir.length() < 0.001){   //1 m
//     CalcStartStrip();
//     _activeStrip.SetCharge(energy/3.61);
//     _strips.push_back(_activeStrip);
//     return _strips;
//   }
//   CalcOneSideOfStrips();
// // other side
// 




/// New Method for Strip part:
///
/// Do this for both parts:
///   1: Calculate Angle between Strip and Hit direction
///   2: Transform by (90 deg - theta)
///      --> New coordinate across strips
///   3: Calculate Strip numbers and energy fractions
///   4: add strip objects to the list

  for (double skew = _skew; skew > -_skew; skew-=2*_skew)
  {

    _energy=energy;
    _in.setXYZ(inx, iny, 0);
    _out.setXYZ(outx, outy, 0);
    _dir = _out - _in;
    _pos = _in;

    // avoid too short tracklets
    if (_dir.length() < 0.001){   //1 m
      /// TODO Transform by skew
      CalcStartStrip();
      _activeStrip.SetCharge(energy/3.61);
      _allstrips.push_back(_activeStrip);
      /// TODO Transform by -skew
      CalcStartStrip();
      _activeStrip.SetCharge(energy/3.61);
      _allstrips.push_back(_activeStrip);
      return _allstrips;
    }

/// 1: Calculate Angle between Strip and Hit direction
    double phi = atan(_dir.getY()/_dir.getX());
    double theta = skew - phi;
/// ACHTUNG Sind alle Vorzeichn richtig?
/// Kommt noch eine Trafo auf den skew?
///   --> Sensordesign mit Thomas absprechen.
  
/// TODO  2: Transform by (90 deg - theta)
    // noetig? 
    _in.setX(_in.getX()*cos(theta));
    _in.setY(_in.getY()*sin(theta));
    _out.setX(_out.getX()*cos(theta));
    _out.setY(_out.getY()*sin(theta));

    // reicht das? Stimmt cos und sin so?
    _dir.setX(_dir.getX()*cos(theta));
    _dir.setY(_dir.getY()*sin(theta));
    _pos.setX(_pos.getX()*cos(theta));
    _pos.setY(_pos.getY()*sin(theta));
/// TODO  3: Calculate Strip numbers and energy fractions

/// TODO  4: add strip objects to the list
  }// end for skew


















  return _allstrips;
}


void MvdCalcStrip::CalcOneSideOfStrips()
{
	CalcConMatrix();
	ApplyConMatrix();
	if (_verboseLevel > 1){
		std::cout << "Converted Vectors: " << std::endl;
		std::cout << _in << _out << _dir << _pos << std::endl;
	}
	CalcQuadrant();
  CalcCperL(_energy);
	CalcStartStrip();

	_stop = false;
	while (_stop != true){
		CalcStrip();
		int col = _activeStrip.GetCol();
		int row = _activeStrip.GetRow();
		switch (_nextStrip) {
			case U : _activeStrip.SetRow(++row); break;
			case D : _activeStrip.SetRow(--row); break;
			case L : _activeStrip.SetCol(--col); break;
			case R : _activeStrip.SetCol(++col); break;
			case StripUNDEF : std::cout << "MvdCalcStrip::GetStrips no next strip."<<std::endl; break;
		}
		_activeStrip.SetCharge(-1.0);
	}

	ConvertStrips();
  _allstrips.insert(_allstrips.end(), _strips.begin(), _strips.end());
}




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

void MvdCalcStrip::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 MvdCalcStrip::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 MvdCalcStrip::CalcCperL(double Energy)
{
	double Charge = Energy / 3.61;
	_CperL = Charge / _dir.length();	
}

void MvdCalcStrip::CalcStartStrip ()
{
	int col = int(_in.getX() / _stripLength);
	int row = int(_in.getY() / _stripWidth);
	_activeStrip.SetCol(col);
	_activeStrip.SetRow(row);
}

void MvdCalcStrip::CalcStrip()
{
	double borderX = 0;
	double borderY = 0;
	CbmGeoVector OutPoint;
	bool XbeforeY = false;
	
	switch (_quad){
		case UR : borderX = _stripLength * (_activeStrip.GetCol()+1);
			  borderY = _stripWidth  * (_activeStrip.GetRow()+1);
			  if ( ( (borderX - _pos.getX()) * _dir.getY() / _dir.getX() )+_pos.getY() < borderY)
			  	XbeforeY = true;
			  else XbeforeY = false;
			  break;
		case UL : borderX = _stripLength * (_activeStrip.GetCol());
			  borderY = _stripWidth  * (_activeStrip.GetRow()+1);
			  if ( ( (borderX - _pos.getX()) * _dir.getY() / _dir.getX() )+_pos.getY() < borderY)
			  	XbeforeY = true;
			  else XbeforeY = false;
			  break;
		case DL : borderX = _stripLength * (_activeStrip.GetCol());
			  borderY = _stripWidth  * (_activeStrip.GetRow());
			  if ( ( (borderX - _pos.getX()) * _dir.getY() / _dir.getX() )+_pos.getY() > borderY)
			  	XbeforeY = true;
			  else XbeforeY = false;
			  break;
		case DR : borderX = _stripLength * (_activeStrip.GetCol()+1);
			  borderY = _stripWidth  * (_activeStrip.GetRow());
			  if ( ( (borderX - _pos.getX()) * _dir.getY() / _dir.getX() )+_pos.getY() > borderY)
			  	XbeforeY = true;
			  else XbeforeY = false;
			  break;
		case QuadUNDEF : std::cout<<"MvdCalcStrip::CalcStrip : 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)
			_nextStrip = R;
		else _nextStrip = L;
	}
	else {
		OutPoint.setXYZ(((borderY-_pos.getY())*_dir.getX()/_dir.getY())+_pos.getX(), borderY,0);
		
		if (_quad == UR || _quad == UL)
			_nextStrip = U;
		else _nextStrip = 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){
		_activeStrip.SetCharge(DepCharge);
	}
	else
		_activeStrip.SetCharge(0);

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

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

std::ostream& MvdCalcStrip::operator<<(std::ostream& out)
{
	out << "StripWidth: " << _stripWidth << " StripLength: " <<
		_stripLength << " Threshold: " << _threshold << std::endl;
	
	return out;
}