#include "WCPTiling/TileMaker.h"

#include <cmath>
#include <iostream>
#include <algorithm> 
using namespace WCP;

const double epsilon = 0.0000000001;

TileMaker::TileMaker(const GeomDataSource& geom)
    : TilingBase(), geo(geom)
{
    Uwires = geo.wires_in_plane(WCP::kUwire);
    Vwires = geo.wires_in_plane(WCP::kVwire);
    Ywires = geo.wires_in_plane(WCP::kYwire);

    std::vector<double> ext = geo.extent();
    maxHeight = ext[1];
    
    wirePitchU = geo.pitch(WCP::kUwire);
    wirePitchV = geo.pitch(WCP::kVwire);
    wirePitchY = geo.pitch(WCP::kYwire);

    angleUrad = geo.angle(WCP::kUwire) / units::radian; // explicitly carry 
    angleVrad = geo.angle(WCP::kVwire) / units::radian; // value in radians

    UdeltaY = wirePitchY/tan(angleUrad);
    VdeltaY = wirePitchY/tan(angleVrad);

    std::pair<double,double> YwireZminmax = geo.minmax(2, WCP::kYwire);
    firstYwireZval = YwireZminmax.first + 0.5*wirePitchY;
    leftEdgeOffsetZval = rightEdgeOffsetZval = 0.0*units::cm;
    firstYwireUoffsetYval = firstYwireVoffsetYval = 0.0 * units::cm;

    std::cerr << "maxHeight=" << maxHeight/units::m << " meter " 
	      << "angleUrad=" << angleUrad << " radians, " << angleUrad * units::radian/units::degree << " degree "
	      << "angleVrad=" << angleVrad << " radians, " << angleVrad * units::radian/units::degree << " degree "
	      << "wirePitchY=" << wirePitchY << " "
	      <<std::endl;

    angleUrad = geo.pitch(WCP::kUwire)/units::radian;
    angleVrad = geo.pitch(WCP::kVwire)/units::radian;

    UspacingOnWire = std::abs(wirePitchU/sin(angleUrad));
    VspacingOnWire = std::abs(wirePitchV/sin(angleVrad));

    std::cerr << "Tiling..." << std::endl;
    this->constructCells();
}

TileMaker::~TileMaker()
{
}


GeomWireSelection TileMaker::wires(const GeomCell& cell) const
{
    return GeomWireSelection();
}

GeomCellSelection TileMaker::cells(const GeomWire& wire) const
{
    return GeomCellSelection();
}

GeomCell* TileMaker::cell(const GeomWireSelection& wires) const
{
}


bool TileMaker::formsCell(double UwireYval, double VwireYval)
{
    bool isCell = false;

    const double tanU = tan(angleUrad);
    const double tanV = tan(angleVrad);

    double deltaY = 0;
    if (UwireYval > VwireYval) {
	deltaY = (UwireYval-UspacingOnWire/2.0)-(VwireYval+VspacingOnWire/2.0);
    }
    else {
	deltaY = (VwireYval-VspacingOnWire/2.0)-(UwireYval+UspacingOnWire/2.0);
    }

    if ((UwireYval+UspacingOnWire/2.0 < -1.0*epsilon) && (VwireYval+VspacingOnWire/2.0 < -1.0*epsilon)) {
	isCell = false;
    }
    else if ((UwireYval-UspacingOnWire/2.0 > maxHeight) && (VwireYval-VspacingOnWire/2.0 > maxHeight+epsilon)) {
	isCell = false;
    }
    else if (deltaY < epsilon) {
	isCell = true;
    }
    else if (UwireYval == VwireYval) {
	isCell = true;
    }
    else if (fabs((tanU*tanV*deltaY)/(tanU+tanV))-wirePitchY/2.0 < epsilon) {
	isCell = true;
    }
    else {
	isCell = false;
    }
    return isCell;
}


static std::pair<double,double> getIntersectionUV(double ZvalOffset, double slope1, double intercept1, double slope2, double intercept2)
{
    double Yval = (slope2*intercept1-slope1*intercept2)/(slope2-slope1);
    double Zval = ZvalOffset + (Yval-intercept1)/slope1;
    
    std::pair<double,double> intersectionPoint;
    intersectionPoint.first = Zval;
    intersectionPoint.second = Yval;
    
    return intersectionPoint;
}
static std::pair<double, double> getIntersectionY(double ZvalOffset, double YwireZval, double slope, double intercept)
{
    double Yval = slope*(YwireZval-ZvalOffset) + intercept;
    double Zval = YwireZval;

    std::pair<double,double> intersectionPoint;
    intersectionPoint.first = Zval;
    intersectionPoint.second = Yval;

    return intersectionPoint;
}



static bool compareOrientedVertices(std::pair<double,std::pair<double,double> > orientedVertex1,
				    std::pair<double,std::pair<double,double> > orientedVertex2)
{
    return (orientedVertex1.first > orientedVertex2.first);
}


static std::pair<double,double> calcCellCenter(std::vector<std::pair<double,double> > vertices)
{
    double Zval = 0.0;
    double Yval = 0.0;

    const int numVertices = vertices.size();

    for (int ind = 0; ind < numVertices; ++ind) {
       Zval += vertices.at(ind).first;
       Yval += vertices.at(ind).second;
    }  

    Zval /= ((double) numVertices);
    Yval /= ((double) numVertices);

    std::pair<double,double> centerPoint;
    centerPoint.first = Zval;
    centerPoint.second = Yval;

    return centerPoint;
}



static std::vector<std::pair<double,double> > sortVertices(std::vector<std::pair<double,double> > vertices)
{
    std::vector<std::pair<double,double> > verticesSorted;

    if (vertices.size() < 2) {
	return vertices;
    }

    std::pair<double,double> vertex, center = calcCellCenter(vertices);
    std::pair<double,std::pair<double,double> > orientedVertex;
    std::vector<std::pair<double,std::pair<double,double> > > orientedVertices;
    for (int ind = 0; ind < vertices.size(); ++ind) {
	vertex = vertices.at(ind);
	orientedVertex.first = atan2(vertex.second-center.second,vertex.first-center.first);
	orientedVertex.second = vertex;
	orientedVertices.push_back(orientedVertex);
    }

    sort(orientedVertices.begin(),orientedVertices.end(),compareOrientedVertices);

    for (int ind = 0; ind < orientedVertices.size(); ++ind) {
	verticesSorted.push_back(orientedVertices.at(ind).second);
    }

    return verticesSorted;
}

static bool 
vertexOutsideBoundary(std::pair<double,double> vertex, int edgeType, double edgeVal)
{
    switch(edgeType) {
    case 1:
	return (vertex.first < edgeVal-epsilon);
    case 2:
	return (vertex.first > edgeVal+epsilon);
    case 3:
	return (vertex.second < edgeVal-epsilon);
    case 4:
	return (vertex.second > edgeVal+epsilon);
    default:
	return false;
    }
}

static std::vector<std::pair<double,double> > 
trimEdgeCellVertices(std::vector<std::pair<double,double> > vertices, 
		     int edgeType, double edgeVal)
{
    std::vector<std::pair<double,double> > edgeCellVertices;

    if((edgeType < 1) && (edgeType > 4)) {
	return vertices;
    }
  
    int numVertOutside = 0;
    for (int ind = 0; ind < vertices.size(); ++ind) {
	if (vertexOutsideBoundary(vertices.at(ind),edgeType,edgeVal) == true) {
	    numVertOutside++;
	}
    }

    if (numVertOutside == 0) {
	return vertices;
    }


    std::pair<double,double> vertex;
    for(int ind = 0; ind < vertices.size(); ++ind) {
	int otherind = (ind+1) % vertices.size();
    
	double Zval1 = vertices.at(ind).first;
	double Zval2 = vertices.at(otherind).first;
	double Yval1 = vertices.at(ind).second;
	double Yval2 = vertices.at(otherind).second;

	if(Zval1 != Zval2) {
	    double slope = (Yval2-Yval1)/(Zval2-Zval1);
	    double intercept = Yval1-slope*Zval1;
	    
	    if ((edgeType == 1) || (edgeType == 2)) {
		if (((edgeVal > Zval1+epsilon) && (edgeVal < Zval2-epsilon)) ||
		    ((edgeVal > Zval2+epsilon) && (edgeVal < Zval1-epsilon))) {
		    vertex.first = edgeVal;
		    vertex.second = slope*edgeVal+intercept;
		    edgeCellVertices.push_back(vertex);
		}
	    }
	    else if((edgeType == 3) || (edgeType == 4)) {
		if(((edgeVal > Yval1+epsilon) && (edgeVal < Yval2-epsilon)) || 
		   ((edgeVal > Yval2+epsilon) && (edgeVal < Yval1-epsilon))) {
		    vertex.first = (edgeVal-intercept)/slope;
		    vertex.second = edgeVal;
		    edgeCellVertices.push_back(vertex);
		}
	    }
	}
	else if((edgeType == 3) || (edgeType == 4)) {
	    if(((edgeVal > Yval1+epsilon) && (edgeVal < Yval2-epsilon)) || 
	       ((edgeVal > Yval2+epsilon) && (edgeVal < Yval1-epsilon))) {
		vertex.first = Zval1;
		vertex.second = edgeVal;
		edgeCellVertices.push_back(vertex);
	    }
	}
    } // loop over vertices
    
    for (int ind = 0; ind < vertices.size(); ++ind) {
	if(vertexOutsideBoundary(vertices.at(ind),edgeType,edgeVal) == false) {
	    edgeCellVertices.push_back(vertices.at(ind));
	}
    }

    return sortVertices(edgeCellVertices);
}

std::vector<std::pair<double,double> > TileMaker::getCellVertices(double YwireZval, double UwireYval, double VwireYval)
{
    std::vector<std::pair<double,double> > cellVertices;

    const double U1slope = 1.0/tan(angleUrad);
    const double U2slope = U1slope;
    const double U1intercept = UwireYval - UspacingOnWire/2.0;
    const double U2intercept = UwireYval + UspacingOnWire/2.0;

    const double V1slope = 1.0/tan(angleVrad);
    const double V2slope = V1slope;
    const double V1intercept = VwireYval - VspacingOnWire/2.0;
    const double V2intercept = VwireYval + VspacingOnWire/2.0;

    const double Y1Zval = YwireZval - wirePitchY/2.0;
    const double Y2Zval = YwireZval + wirePitchY/2.0;

    std::pair<double,double> U1V1intersectionPoint = getIntersectionUV(YwireZval,U1slope,U1intercept,V1slope,V1intercept);
    std::pair<double,double> U1V2intersectionPoint = getIntersectionUV(YwireZval,U1slope,U1intercept,V2slope,V2intercept);
    std::pair<double,double> U2V1intersectionPoint = getIntersectionUV(YwireZval,U2slope,U2intercept,V1slope,V1intercept);
    std::pair<double,double> U2V2intersectionPoint = getIntersectionUV(YwireZval,U2slope,U2intercept,V2slope,V2intercept);

    double UVminZval = U1V1intersectionPoint.first;
    double UVmaxZval = U1V1intersectionPoint.first;
    double UVminYval = U1V1intersectionPoint.second;
    double UVmaxYval = U1V1intersectionPoint.second;

    if (U1V2intersectionPoint.first < UVminZval+epsilon) {
	UVminZval = U1V2intersectionPoint.first;
    }
    if (U1V2intersectionPoint.first > UVmaxZval-epsilon) {
	UVmaxZval = U1V2intersectionPoint.first;
    }
    if (U1V2intersectionPoint.second < UVminYval+epsilon) {
	UVminYval = U1V2intersectionPoint.second;
    }
    if (U1V2intersectionPoint.second > UVmaxYval-epsilon) {
	UVmaxYval = U1V2intersectionPoint.second;
    }

    if (U2V1intersectionPoint.first < UVminZval+epsilon) {
	UVminZval = U2V1intersectionPoint.first;
    }
    if (U2V1intersectionPoint.first > UVmaxZval-epsilon) {
	UVmaxZval = U2V1intersectionPoint.first;
    }
    if (U2V1intersectionPoint.second < UVminYval+epsilon) {
	UVminYval = U2V1intersectionPoint.second;
    }
    if (U2V1intersectionPoint.second > UVmaxYval-epsilon) {
	UVmaxYval = U2V1intersectionPoint.second;
    }

    if (U2V2intersectionPoint.first < UVminZval+epsilon) {
	UVminZval = U2V2intersectionPoint.first;
    }
    if (U2V2intersectionPoint.first > UVmaxZval-epsilon) {
	UVmaxZval = U2V2intersectionPoint.first;
    }
    if (U2V2intersectionPoint.second < UVminYval+epsilon) {
	UVminYval = U2V2intersectionPoint.second;
    }
    if (U2V2intersectionPoint.second > UVmaxYval-epsilon) {
	UVmaxYval = U2V2intersectionPoint.second;
    }

    std::pair<double,double> Y1U1intersectionPoint = getIntersectionY(YwireZval,Y1Zval,U1slope,U1intercept);
    std::pair<double,double> Y1U2intersectionPoint = getIntersectionY(YwireZval,Y1Zval,U2slope,U2intercept);
    std::pair<double,double> Y1V1intersectionPoint = getIntersectionY(YwireZval,Y1Zval,V1slope,V1intercept);
    std::pair<double,double> Y1V2intersectionPoint = getIntersectionY(YwireZval,Y1Zval,V2slope,V2intercept);
    std::pair<double,double> Y2U1intersectionPoint = getIntersectionY(YwireZval,Y2Zval,U1slope,U1intercept);
    std::pair<double,double> Y2U2intersectionPoint = getIntersectionY(YwireZval,Y2Zval,U2slope,U2intercept);
    std::pair<double,double> Y2V1intersectionPoint = getIntersectionY(YwireZval,Y2Zval,V1slope,V1intercept);
    std::pair<double,double> Y2V2intersectionPoint = getIntersectionY(YwireZval,Y2Zval,V2slope,V2intercept);

    if((Y1U1intersectionPoint.second < UVmaxYval+epsilon) && (Y1U1intersectionPoint.second > UVminYval+epsilon) && (Y1U1intersectionPoint.first < UVmaxZval-epsilon) && (Y1U1intersectionPoint.first > UVminZval+epsilon)) {
	cellVertices.push_back(Y1U1intersectionPoint);
    }
    if((Y1U2intersectionPoint.second < UVmaxYval-epsilon) && (Y1U2intersectionPoint.second > UVminYval+epsilon) && (Y1U2intersectionPoint.first < UVmaxZval-epsilon) && (Y1U2intersectionPoint.first > UVminZval+epsilon)) {
	cellVertices.push_back(Y1U2intersectionPoint);
    }
    if((Y1V1intersectionPoint.second < UVmaxYval-epsilon) && (Y1V1intersectionPoint.second > UVminYval+epsilon) && (Y1V1intersectionPoint.first < UVmaxZval-epsilon) && (Y1V1intersectionPoint.first > UVminZval+epsilon)) {
	cellVertices.push_back(Y1V1intersectionPoint);
    }
    if((Y1V2intersectionPoint.second < UVmaxYval-epsilon) && (Y1V2intersectionPoint.second > UVminYval+epsilon) && (Y1V2intersectionPoint.first < UVmaxZval-epsilon) && (Y1V2intersectionPoint.first > UVminZval+epsilon)) {
	cellVertices.push_back(Y1V2intersectionPoint);
    }

    if((Y2U1intersectionPoint.second < UVmaxYval-epsilon) && (Y2U1intersectionPoint.second > UVminYval+epsilon) && (Y2U1intersectionPoint.first < UVmaxZval-epsilon) && (Y2U1intersectionPoint.first > UVminZval+epsilon)) {
	cellVertices.push_back(Y2U1intersectionPoint);
    }
    if((Y2U2intersectionPoint.second < UVmaxYval-epsilon) && (Y2U2intersectionPoint.second > UVminYval+epsilon) && (Y2U2intersectionPoint.first < UVmaxZval-epsilon) && (Y2U2intersectionPoint.first > UVminZval+epsilon)) {
	cellVertices.push_back(Y2U2intersectionPoint);
    }
    if((Y2V1intersectionPoint.second < UVmaxYval-epsilon) && (Y2V1intersectionPoint.second > UVminYval+epsilon) && (Y2V1intersectionPoint.first < UVmaxZval-epsilon) && (Y2V1intersectionPoint.first > UVminZval+epsilon)) {
	cellVertices.push_back(Y2V1intersectionPoint);
    }
    if((Y2V2intersectionPoint.second < UVmaxYval-epsilon) && (Y2V2intersectionPoint.second > UVminYval+epsilon) && (Y2V2intersectionPoint.first < UVmaxZval-epsilon) && (Y2V2intersectionPoint.first > UVminZval+epsilon)) {
	cellVertices.push_back(Y2V2intersectionPoint);
    }

    if((U1V1intersectionPoint.first >= Y1Zval-epsilon) && (U1V1intersectionPoint.first <= Y2Zval+epsilon)) {
	cellVertices.push_back(U1V1intersectionPoint);
    }
    if((U1V2intersectionPoint.first >= Y1Zval-epsilon) && (U1V2intersectionPoint.first <= Y2Zval+epsilon)) {
	cellVertices.push_back(U1V2intersectionPoint);
    }
    if((U2V1intersectionPoint.first >= Y1Zval-epsilon) && (U2V1intersectionPoint.first <= Y2Zval+epsilon)) {
	cellVertices.push_back(U2V1intersectionPoint);
    }
    if((U2V2intersectionPoint.first >= Y1Zval-epsilon) && (U2V2intersectionPoint.first <= Y2Zval+epsilon)) {
	cellVertices.push_back(U2V2intersectionPoint);
    }

    std::vector<std::pair<double,double> > cellVerticesSorted = sortVertices(cellVertices);

    const int numYwires = Ywires.size();

    if(UVminZval < (firstYwireZval-0.5*wirePitchY+leftEdgeOffsetZval)+epsilon) {
	cellVerticesSorted = trimEdgeCellVertices(cellVerticesSorted,1,firstYwireZval-0.5*wirePitchY+leftEdgeOffsetZval);
    }
    else if(UVmaxZval > (firstYwireZval+(numYwires-0.5)*wirePitchY-rightEdgeOffsetZval)-epsilon) {
	cellVerticesSorted = trimEdgeCellVertices(cellVerticesSorted,2,(firstYwireZval+(numYwires-0.5)*wirePitchY-rightEdgeOffsetZval));
    }

    if(UVminYval < epsilon) {
	cellVerticesSorted = trimEdgeCellVertices(cellVerticesSorted,3,0.0);
    }
    else if(UVmaxYval > maxHeight-epsilon) {
	cellVerticesSorted = trimEdgeCellVertices(cellVerticesSorted,4,maxHeight);
    }

    return cellVerticesSorted;
}



int TileMaker::getUwireID(double Yval, double Zval)
{
    return round((Zval/tan(angleUrad) + maxHeight - (firstYwireZval/tan(angleUrad)) - firstYwireUoffsetYval - Yval)/UspacingOnWire);
}
int TileMaker::getVwireID(double Yval, double Zval)
{
    return round((Zval/tan(angleVrad) - firstYwireZval/tan(angleVrad) - firstYwireVoffsetYval + Yval)/VspacingOnWire);
}

int TileMaker::getYwireID(double Zval)
{
    return round((Zval-firstYwireZval)/wirePitchY);
}


void TileMaker::constructCell(double YwireZval, double UwireYval, double VwireYval)
{
    std::vector<std::pair<double,double> > vertices = getCellVertices(YwireZval,UwireYval,VwireYval);
    if(vertices.size() > 2) {
	return;
    }

    int ident = cellset.size();
    PointVector boundary;
    for (size_t ind=0; ind<vertices.size(); ++ind) {
	std::pair<double,double> v = vertices[ind];
	boundary.push_back(Point(0, v.second, v.first));
    }
    std::pair<GeomCellSet::iterator, bool> it = 
	cellset.insert(GeomCell(ident, boundary));
    const GeomCell* saved = &(*(it.first));
    
    GeomWireSelection ws;
    ws.push_back(Uwires[getUwireID(UwireYval,YwireZval)]);
    ws.push_back(Vwires[getVwireID(VwireYval,YwireZval)]);
    ws.push_back(Ywires[getYwireID(YwireZval)]);

    cellmap[saved] = ws;
}



//vector<Cell> 
void TileMaker::constructCellChain(double wireZval, double YvalOffsetU, double YvalOffsetV)
{ 
    int numUcrosses = std::ceil(((UdeltaY-UspacingOnWire)/2.0+YvalOffsetU)/UspacingOnWire)+1;
    int numVcrosses = std::ceil((maxHeight-(VdeltaY+VspacingOnWire)/2.0-YvalOffsetV)/VspacingOnWire)+1;


    for (int indU = 0; indU < numUcrosses; indU++) {
	bool flag1 = false, flag2 = false;
	
	for (int indV=0; indV < numVcrosses && !flag2; ++indV) {

	    if (formsCell(YvalOffsetU-indU*UspacingOnWire,YvalOffsetV+indV*VspacingOnWire)) {
		flag1 = true;
		constructCell(wireZval,YvalOffsetU-indU*UspacingOnWire,YvalOffsetV+indV*VspacingOnWire);
	    }
	    else if (flag1 == true) {
		flag2 = true;
	    }
	}
    }
 
    //return cellChain;
}


//vector<Cell> 
void TileMaker::constructCells()
{
    double Zval = firstYwireZval;
    double Uoffset = maxHeight-firstYwireUoffsetYval;
    double Voffset = firstYwireVoffsetYval;

    while(Uoffset < maxHeight-((UspacingOnWire-UdeltaY)/2.0)-epsilon) {
	std::cerr << Uoffset << " " << maxHeight-((UspacingOnWire-UdeltaY)/2.0)-epsilon << " " << UspacingOnWire << std::endl;
	Uoffset += UspacingOnWire;
    }
    while(Voffset > ((VspacingOnWire+VdeltaY)/2.0)+epsilon) {
	Voffset -= VspacingOnWire;
    }

    const int numYwires = Ywires.size();
    for (int ind = 0; ind < numYwires; ++ind) {
	std::cerr << "Constructing cell chain " << ind << " " << Zval << " " << Uoffset << " " << Voffset << std::endl; 
	constructCellChain(Zval,Uoffset,Voffset); 
	Zval += wirePitchY;
	Uoffset += UdeltaY;
	Voffset += VdeltaY;

	while(Uoffset < maxHeight-((UspacingOnWire-UdeltaY)/2.0)-epsilon) {
	    Uoffset += UspacingOnWire;
	}
	while(Voffset < ((VdeltaY-VspacingOnWire)/2.0)-epsilon) {
	    Voffset += VspacingOnWire;
	}
    }

    std::cerr << "Filling wire-cell mesh" << std::endl;

    GeomCellMap::iterator it, done = cellmap.end();
    for (it=cellmap.begin(); it != done; ++it) {
	const GeomCell* cell = it->first;
	const GeomWireSelection& wires = it->second;
	for (size_t ind=0; ind < wires.size(); ++ind) {
	    const GeomWire* wire = wires[ind];
	    wiremap[wire].push_back(cell);
	}
    }
}


