/*
    Hyperbolic Tessellations
    Copyright (C) 2007 Dmitry Brant
    http://dmitrybrant.com

    Based largely on code from
    http://www.plunk.org/~hatch/HyperbolicApplet/
*/
//---------------------------------------------------------------------------
#include <vcl.h>
#pragma hdrstop
//---------------------------------------------------------------------------

#include "hyperbolicutils.h"


double HYPOTSQRD(double a, double b){
    return ((a*a)+(b*b));
}
//--------------------------------------------------

// hyperbolic to euclidean norm (distance from 0,0) in Poincare disk.
double h2eNorm(double hNorm){
    //if (Double.isInfinite(hNorm))
    //    return 1.;
    return __tanh(.5*hNorm);
}
//--------------------------------------------------

// euclidean to hyperbolic norm (distance from 0,0) in Poincare disk
double e2hNorm(double eNorm){
    return 2*atanh(eNorm);
}
//--------------------------------------------------


// Given a point p and nVerts verts,
// calculate the barycentric coordinates
void unHBary2(Vector<double>& result, complex& p, Vector<complex>& verts){
    // v = verts translated by isometry that takes p to origin...
    Isometry translatePToOrigin = Isometry::pureTranslation(-(p[0]),-(p[1]));
    int nVerts = verts.length;
    Vector<complex> v(nVerts);
    int i;
    for(i=0; i<nVerts; ++i){
        translatePToOrigin.apply(verts[i], v[i]);
    }
    for(i=0; i<3; ++i){
                                             //null?
        result[i] = calcSmallestPointOnLine2(complex(), v[(i+1)%3], v[(i+2)%3]);

        if(vxv2(v[(i+1)%3],v[(i+2)%3]) < 0.0)
            result[i] *= -1.0;
    }
    for(; i<nVerts; ++i)
        result[i] = 0;
}
//--------------------------------------------------




bool hBary2(complex& result,
            Vector<double>& b, // barycentric weights
            Vector<complex>& p)
{
    int nPoints = p.length;
    Vector<complex> q(nPoints);
    Vector<complex> otherPoints(nPoints-1);
    Vector<double> otherBs(nPoints-1);

    int i,j;
    for(i=0; i<nPoints; ++i){
        for(j=0; j<nPoints-1; ++j){
            otherPoints[j] = p[(i+1+j)%nPoints]; // point into p
            otherBs[j] = b[(i+1+j)%nPoints];
        }
        _calcPointOnWeightedSector2(q[i], p[i], otherPoints, otherBs);
    }

    // Figure out which two weights are furthest
    // from summing to 1.
    // (If we pick two which sum to 1,
    // then those two lines will be colinear and therefore
    // no good for calculating the intersection.)
    double bestSum = -1;
    int bestI = -1, bestJ = -1;
    for(i=0; i<nPoints; ++i){
        for (j=0; j<i; ++j){
            double sum = ((b[i] + b[j] - 1.) < 0 ? -(b[i] + b[j] - 1.) : (b[i] + b[j] - 1.));
            if (sum > bestSum){
                bestI = i;
                bestJ = j;
                bestSum = sum;
            }
        }
    }
    return calcLineLineIntersection2(result, p[bestI], q[bestI], p[bestJ], q[bestJ]);;
} // hBary2














// This isn't really barycentric anything,
// but it just happens sometimes we know the distances
// to all the verts.
bool hBary2FromDistances(complex& result,
                    Vector<double>& dists,      // barycentric weights
                    Vector<complex>& p) // points
{
    // For now, just consider the first 3 points.
    complex p0 = p[0];
    complex p1 = p[1];
    double A = dists[1];
    double B = dists[0];
    double C = hdist2_poincare(p0, p1);
    double alpha = solveTriangleAlphaFromABC(A, B, C);
    double beta = solveTriangleAlphaFromABC(B, C, A);

    if (pointIsToRightSideOfLinePoincare(p[2], p[0], p[1], 0.)){
        //assert(false); // XXX coverage test-- probably works, but never tested; remove this when hit.
        alpha *= -1.;
        beta *= -1.;
    }

    complex foo0;
    {
        Isometry take_p0_to_origin = Isometry::pureTranslation(-p0[0], -p0[1]);
        Isometry take_origin_to_p0 = Isometry::pureTranslation(p0[0], p0[1]);
        complex q1;
        take_p0_to_origin.apply(p1, q1);
        double dir = atan2(q1[1], q1[0]) + alpha;
        foo0[0] = .5*cos(dir); foo0[1] = .5*sin(dir);
        take_origin_to_p0.apply(foo0, foo0);
    }
    complex foo1;
    {
        Isometry take_p1_to_origin = Isometry::pureTranslation(-p1[0], -p1[1]);
        Isometry take_origin_to_p1 = Isometry::pureTranslation(p1[0], p1[1]);
        complex q0;
        take_p1_to_origin.apply(p0, q0);
        double dir = atan2(q0[1], q0[0]) - beta;
        foo1[0] = .5*cos(dir); foo1[1] = .5*sin(dir);
        take_origin_to_p1.apply(foo1, foo1);
    }

    return calcLineLineIntersection2(result, p0, foo0, p1, foo1);
} // hBary2FromDistances


// Calculate the intersection of p0p1 with q0q1.
bool calcLineLineIntersection2(complex& result, complex& p0, complex& p1, complex& q0, complex& q1){
    // translate so the q line passes throught the origin...
    complex q;
    calcSmallestPointOnLine2(q,q0,q1);
    Isometry F = Isometry::pureTranslation(q[0],q[1]);
    Isometry inverseF = F.inverse();

    complex p0_; inverseF.apply(p0, p0_);
    complex p1_; inverseF.apply(p1, p1_);
    complex q0_; inverseF.apply(q0, q0_);
    complex q1_; inverseF.apply(q1, q1_);

    // use the infinite boundary points of the lines...
    _calcEndOfGeodesic2(p0_, p1_,p0_);
    _calcEndOfGeodesic2(p1_, p0_,p1_);
    _calcEndOfGeodesic2(q0_, q1_,q0_);

    double x0 = dot(q0_,p0_);
    double x1 = dot(q0_,p1_);
    double temp = 2*x0*x1 - dot(p0_,p1_) + 1;
    if (temp*temp-(x0+x1)*(x0+x1) < 0.0)
        return false;
    double x = (x0+x1)/(temp+sqrt(temp*temp-(x0+x1)*(x0+x1)));
    result = q0_ * x;

    // transform back
    F.apply(result, result);
    return true;
}
//----------------------------------------------------------


// Calculate the point on the given hyperbolic line that is closest to the origin.
// The function return value is the hyperbolic distance from the origin to that point.
double calcSmallestPointOnLine2(complex& result, complex& p0, complex& p1){
    complex q0;
    complex q1;
    _calcEndOfGeodesic2(q0, p1, p0);
    _calcEndOfGeodesic2(q1, p0, p1);

    complex q;
    q = q0*0.5 + q1*0.5;

    double scale = 1. / (1 + .5 * sqrt(distsqrd(q0, q1)));
    result = q * scale;
    return e2hNorm(scale * sqrt(dot(q, q)));
}
//----------------------------------------------------------


double calcClosestPointOnLine2(complex& result, complex& p, complex& l0, complex& l1){
    // F = isometry that takes origin to p
    Isometry F = Isometry::pureTranslation(p[0],p[1]);
    Isometry inverseF = F.inverse();
    complex _l0; inverseF.apply(l0, _l0);
    complex _l1; inverseF.apply(l1, _l1);
    double dist = calcSmallestPointOnLine2(result, _l0, _l1);
    F.apply(result, result);
    return dist;
}




// Calculate the point on the boundary of the disk
// obtained by following the geodesic from p0 towards p1.
// p0 and p1 must be distinct.
void _calcEndOfGeodesic2(complex& result, complex& p0, complex& p1){
    Isometry F = Isometry::pureTranslation(p1[0],p1[1]);
    Isometry inverseF = F.inverse();
    complex p0_; inverseF.apply(p0, p0_);
    double len = sqrt(dot(p0_,p0_));
    result[0] = p0_[0] * (-1.0/len);
    result[1] = p0_[1] * (-1.0/len);
    F.apply(result, result);
}
//--------------------------------------------------------------



// Calculate a line originating from the vertex p0
// in the direction of the other vertices p[0..nPoints-1],
// whose perpendicular distance to the face opposite p[i]
// has sinh proportional to b[i].
// This is equivalent to saying that close to p0,
// the distances to the respective faces are proportional to the b[i]'s.
void _calcPointOnWeightedSector2(complex& result, complex& p0, Vector<complex>& p, Vector<double>& b){
    double d00 = dot(p0,p0);
    if (d00 >= (1-1e-6)*(1-1e-6)){
        complex v[2];
        int i;
        for (i = 0; i < 2; ++i) _calcEndOfGeodesic2(v[i], p0, p[i]);

        double dots[2];
        for (i = 0; i < 2; ++i) dots[i] = dot(p0,v[i]);

        complex u[2];
        for (i = 0; i < 2; ++i){
            u[i] = v[i] + p0*(-dots[i]/d00);
            double len = sqrt(dot(u[i],u[i]));
            if (len != 0.0) u[i] = u[i] * (1.0/len);
        }

        Vector<complex> w(2);
        for (i = 0; i < 2; ++i){
            double H = .5 * sqrt( 2. / (1.-dots[i]) - 1. );
            w[i] = u[i] * H;
        }

        // Get affine coefficients for the resulting infinitesimal point
        // in terms of the infinitessimal vectors w1,w2,w3.
        Vector<double> c(2);
        for (i = 0; i < 2; ++i) c[i] = b[i];
        // normalize...
        double sum = 0.;
        for (i = 0; i < 2; ++i) sum += c[i];
        for (i = 0; i < 2; ++i) c[i] *= 1./sum;

        // Calculate the infinitessimal point.
        complex w0;
        vxm(w0, c, w);
        double H = sqrt(dot(w0,w0));
        complex u0;
        if (H == 0.) u0 = complex(0.0, 0.0); // doesn't matter since its coeff will be zero later // XXX stuff like this should cancel out and we shouldn't have to check
        else u0 = w0 * (1.0/H);

        // Project back out to a finite (actually boundary) point
        // (this is the result of more calculations on paper).
        double x = 2/(4*H*H+1)-1;
        result = p0*(-x) + u0*sqrt(1.0-x*x);
        return;
    }

    // Translate so p0 is at the origin...
    Isometry F, inverseF;
    F = Isometry::pureTranslation(p0[0],p0[1]);
    inverseF = Isometry::pureTranslation(-p0[0],-p0[1]);
    Vector<complex> p_(2);
    int i;
    for(i = 0; i < 2; ++i) inverseF.apply(p[i], p_[i]);

    // Calculate coefficients of answer in terms of p_[0..nPoints-1].
    Vector<double> c(2);
    for(i = 0; i < 2; ++i){
        complex otherPoint;
        otherPoint = p_[(i+1)%2];
        double oppositeSideContent = calcContentWithOrigin(otherPoint);
        c[i] = b[i] * oppositeSideContent;
    }
    vxm(result, c, p_);

    double d = dot(result, result);
    if (d == 0.) d = 1e-20;
    // put it on the boundary...
    result = result * (1.0/sqrt(d));
    // Translate so origin goes back to p0...
    F.apply(result, result);
}
//--------------------------------------------------------



double calcContentWithOrigin(complex& v){
    double prod = 1.0;
    double dotii = dot(v,v);
    if (dotii == 0.) return 0.;
    prod *= dotii;
    return sqrt(prod);
}
//--------------------------------------------------------

// calculate unsigned k-dimensional content
//  spanned by k n-dimensional vectors
// together with the origin.
// If n == k then this is the absolute value of the determinant.
double calcContentWithOrigin(int n, int k, Vector<complex>& _v){
    // Make a copy we can modify...
    Vector<complex> v(_v);

    // Use modified-gram-schmidt,
    // since gaussian-elimination-with-partial-pivoting always confuses me.
    double prod = 1.;
    int i;
    for(int i=0; i<k; i++){
        double dotii = dot(v[i], v[i]);
        if (dotii == 0.)
            return 0.;
        for (int j=i+1; j<k; ++j){
            // make v[j] orthogonal to v[i]
            // by subtracting off its component in the v[i] direction
            v[j] -= (v[i] * (dot(v[j],v[i]) * (1./dotii)));
        }
        prod *= dotii;
    }
    return sqrt(prod);
}

// calculate content of k-1-dimensional simplex with given k n-dimensional
// vertices. (actually (k-1)! times it, but we only care about proportions)
double calcSimplexContent(int n, int k, Vector<complex>& p){
    Vector<complex> v(k-1);
    int i;
    for(int i=0; i<k-1; i++)
        v[i] = p[i+1] - p[0];
    return calcContentWithOrigin(n, k-1, v);
}



bool pointIsToRightSideOfLinePoincare(complex p, complex l0, complex l1, double eps){
    // Translate everything so that l0 is at origin;
    // then we can ask the question in euclidean space.
    Isometry I = Isometry::pureTranslation(-l0[0], -l0[1]);
    I.apply(p, p);
    I.apply(l1, l1);
    double twiceTriangleArea = p[0]*l1[1] - p[1]*l1[0];
    return twiceTriangleArea > eps*eps;
}
//---------------------------------------------------


void calcFaceCenter(double p, int q, complex& result){
    // based on that weird law of cosines on the hyperbolic
    // triangle with angles pi/q,pi/p,pi/2...
    double s = sin(PI/q);
    double c = cos(PI/q);
    double r = (p==0 ? 1. : h2eNorm(acosh((c/s)/tan(PI/p))));
    result[0] = r*c;
    result[1] = r*s;
}
//---------------------------------------------------

void calcFaceCenterFromHalfEdgeLength(double p, double halfEdgeLength, complex& result){
    double polyHalfAngle = polygonHalfAngle(p, halfEdgeLength);
    double circumRadius = polygonCircumRadius(p, halfEdgeLength);
    double r = h2eNorm(circumRadius);
    result[0] = r * cos(polyHalfAngle);
    result[1] = r * sin(polyHalfAngle);
}
//---------------------------------------------------

double polygonHalfAngle(double p, double halfEdgeLength){
    return asin((p == 0. ? 1. : cos(PI/p)) / __cosh(halfEdgeLength));
}
//---------------------------------------------------

double polygonCircumRadius(double p, double halfEdgeLength){
    if (p == 0.) // means infinity
        //return Double.POSITIVE_INFINITY;
        return 1e+300;
    else
        return asinh(__sinh(halfEdgeLength) / sin(PI/p));
}
//---------------------------------------------------

double polygonInRadius(double p, double halfEdgeLength){
    if (p == 0.) // means infinity
        //return Double.POSITIVE_INFINITY;
        return 1e+300;
    else
        return asinh(__tanh(halfEdgeLength) / tan(PI/p));
}
//---------------------------------------------------



double calcUniformTilingHalfEdgeLength(Vector<int>& p, int q){
    // We want edge length e such that
    //         SUM(i==0..nps-1) polygonAngle(p[i],e) = 2*PI
    // where polygonAngle(p,e) = 2*asin(cos(pi/p)/cosh(e/2)).
    // Find cosh(e/2) by binary search (it's an increasing function of e).
    int maxp = -1, nps=p.length;
    int i;
    for (i = 0; i < nps; ++i){
        if (p[i] == 0){
            maxp = 0;
            break;
        }
        else if (p[i] > maxp)
            maxp = p[i];
    }
    double lo = 1.0; /* corresponds to e = 0. */
    double hi = (maxp==0?1.0:cos(PI/maxp))/sin(PI/(nps*q)); /* corresponds to e = edgeLength(maxp, nps*q) */
    double mid;
    while ((mid = lo + (hi-lo)*0.5) != lo && mid != hi){
        double sum = 0.0;
        for (i = 0; i < nps; ++i)
            sum += q*2.0*asin((p[i]==0?1.0:cos(PI/p[i]))/mid);
        if (sum > 2.0*PI)  /* then e, and therefore mid, is too small */
            lo = mid;
        else
            hi = mid;
    }
    double result = acosh(mid);
    return result;
}
//---------------------------------------------------------------


//
// Calculate half-edge-length of semiregular tesselation with each
// vertex surrounded by polygons p[0]..p[nps-1] q times,
// with given covering density.
// XXX The paper "Uniform Solution for Uniform Polyhedra" by Zvi Har'el
//      http://www.math.technion.ac.il/~rl/uniform.pdf
// implies this is not the most stable way to do this,
// but I think it's okay for the configurations we are interested in,
// namely the ones that make pretty pictures
//
// XXX I don't think zero density works yet, even though
// I think it's well-defined.
//
// Returns 0 if it's euclidean or hyperbolic.
//
double calcUniformTilingHalfEdgeLength(Vector<double>& p, double q, double density){
    int nps = p.length;
    if (nps == 0 || q == 0.)
        return 0.;
    // We want edge length e such that
    //         SUM(i==0..nps-1) polygonAngle(p[i],e) = 2*PI
    // where polygonAngle(p,e) = 2*asin(cos(pi/p)/cosh(e/2)).
    // Find cosh(e/2) by binary search (it's an increasing function of e).

    double max_cos_pi_over_p = -1e+300;
    for(int i = 0; i < nps; ++i){
        if(p[i] == 0.){
            max_cos_pi_over_p = 1.;
            break;
        }
        double cos_pi_over_p = cos(PI/p[i]);
        if(cos_pi_over_p > max_cos_pi_over_p)
            max_cos_pi_over_p = cos_pi_over_p;
    }

    double lo = 1.; /* corresponds to e = 0. */
    double hi = max_cos_pi_over_p/sin(density*PI/(nps*q)); /* corresponds to e = edgeLength(maxp, nps*q) */

    //if (Double.isInfinite(hi))
    //    hi = 1e9; // XXX HACK! this sucks worse and worse, and I don't think it even fixes the problem which is when density == 0

    double mid = (lo + (hi-lo)*.5);
    while (mid != lo && mid != hi)
    {
        double sum = 0.;
        for (int i = 0; i < nps; ++i)
        {
            // XXX do the following more elegantly?
            if (1./(p[i] == 0. ? p[i] : sin(PI/p[i])) < 0.) // invert so we test the sign even when 0
                sum -= q*2.*asin((p[i]==0.?1.:cos(PI/p[i]))/mid);
            else
                sum += q*2.*asin((p[i]==0.?1.:cos(PI/p[i]))/mid);
        }
        if (sum > density*2*PI)  /* then e, and therefore mid, is too small */
            lo = mid;
        else
            hi = mid;
        mid = lo + (hi-lo)*.5;
    }
    double result = acosh(mid);
    //assert(!Double.isNaN(result));
    //assert(!Double.isInfinite(result));
    return result;
}
//----------------------------------------------------------------------------

double calcUniformTilingHalfEdgeLength(Vector<int>& p, double q, double density){
    int nps = p.length;
    Vector<double> pdouble(nps);
    for(int i=0; i<nps; i++)
        pdouble[i] = p[i];
    return calcUniformTilingHalfEdgeLength(pdouble, q, density);
}
//----------------------------------------------------------------------------


// Calculate the edge length for the hyperbolic
// tiling with schlafli symbol {p,q},
// i.e. q p-gons surrounding each vertex.


double calcRegularTilingHalfEdgeLength(int p, int q){
    // treat 0 as infinity...
    double cos_pi_p = (p==0 ? 1.0 : cos(PI/p));
    double sin_pi_q = (q==0 ? 0.0 : sin(PI/q));
    double result = acosh( cos_pi_p / sin_pi_q);
    return result;
}
//---------------------------------------------------------------


//
// Label a hyperbolic triangle's sides A,B,C
// and the opposite angles alpha,beta,gamma.
// Given alpha,C,beta, find A.
//
double solveTriangleAFromAlphaCBeta(double alpha, double C, double beta){
    double sinAlpha = sin(alpha);
    double temp = __cosh(C)*sinAlpha*sin(beta) - cos(alpha)*cos(beta);
    double A = asinh(__sinh(C)*sinAlpha / sqrt(1. - temp*temp));
    return A;
}

//
// Label a hyperbolic triangle's sides A,B,C
// and the opposite angles alpha,beta,gamma.
// Given A,gamma,B, find C.
//
double solveTriangleCFromAGammaB(double A, double gamma, double B){
    return acosh(__cosh(A)*__cosh(B) - cos(gamma)*__sinh(A)*__sinh(B));
}

//
// Label a hyperbolic triangle's sides A,B,C
// and the opposite angles alpha,beta,gamma.
// Given A,B,C, find alpha.
// From http://home.hetnet.nl/~vanadovv/Gonio.html
double solveTriangleAlphaFromABC(double A, double B, double C){
    return acos((__cosh(B)*__cosh(C) - cosh(A)) / (__sinh(B)*__sinh(C)));
}

double hangle(complex& p0, complex& p1, complex& p2){
    Isometry take_p1_to_origin = Isometry::pureTranslation(-p1[0], -p1[1]);
    complex q0; take_p1_to_origin.apply(p0, q0);
    complex q2; take_p1_to_origin.apply(p2, q2);
    normalize(q0,q0);
    normalize(q2,q2);
    return angleBetweenUnitVectors(q0,q2);
}

// Very special-case and ad-hoc...
bool findBaryCoordsThatMakeSnubUniform(
         Vector<double>& result,   // barycentric coefficients
         Vector<complex>& verts)
{
    // Figure out the schwarz numbers from the angles...
    // (so I don't have to remember the correspondence between
    // the original face list and these schwarz polygon vertices)
    int nPoints = verts.length;
    Vector<double> schwarzNumbers(nPoints);
    Vector<double> snubFaceList(2*nPoints);
    Vector<double> distsToVerts(nPoints);
    int i;
    for(i=nPoints-1; i>=0; i--)
        schwarzNumbers[i] = PI/hangle(verts[(i-1+nPoints)%nPoints], verts[i], verts[(i+1)%nPoints]);
    for(i=nPoints-1; i>=0; i--){
        snubFaceList[2*i+0] = nPoints;
        snubFaceList[2*i+1] = schwarzNumbers[i];
    }
    double snubHalfEdgeLength = calcUniformTilingHalfEdgeLength(snubFaceList, 1., 1.);

    for(i=nPoints-1; i>=0; i--){
        distsToVerts[i] = polygonCircumRadius(schwarzNumbers[i], snubHalfEdgeLength);
    }

    complex resultPoint;
    if(!hBary2FromDistances(resultPoint, distsToVerts, verts))
        return false;

    unHBary2(result, resultPoint, verts);
    return true;
} // findBaryCoordsThatMakeSnubUniform



int UnCachedEnumerateIsometry2GroupForUniformTiling(
                        Isometry& F0,
                        int nGens, // equal to valence of a vertex, including rotationalMultiplicity
                        Vector<Isometry>& gens,
                        Vector<int>& pp, // face list, not expanded by rotationalMultiplicity
                        int rotationalMultiplicity,
                        Vector<int>& _backEdgeInds, // not expanded by rotationalMultiplicity
                        int maxIsometries,
                        Vector<Isometry>& return_isometries,
                        Vector<int> return_isometry_composition_lengths,
                        Isometry& return_smallestIsometry,
                        int maxLevels,
                        double tooFar,
                        double tooSmall)
{
    //assert(rotationalMultiplicity >= 1);
    double smallestTranslateSqrd = HYPOTSQRD(F0.P[0],F0.P[1]);
    return_smallestIsometry = F0;
    if (maxLevels == 0) return 0;
    if (maxIsometries == 0) return 0;
    int ppLength = pp.length;


    Vector<int> backEdgeInds(_backEdgeInds); //so we can modify it


    // For simplicity, extract the signs out of backEdgeInds
    // and let backEdgeInds be simple indices.
    Vector<int> backEdgeSigns(ppLength);
    {
        //Vector<int> backEdgeInds(_backEdgeInds); //so we can modify it
        for(int i=0; i<ppLength; i++){
            backEdgeSigns[i] = (backEdgeInds[i] < 0 ? -1 : 1);
            if (backEdgeSigns[i] == -1)
                backEdgeInds[i] ^= ~0;

            // XXX bleah, now I'm being wishy-washy...
            // XXX for this function, I'd like sign to be 1
            // XXX for a rotation and -1 for a reflection,
            // XXX which is different from the convention currently used
            // XXX in the rest of the program.
            // XXX It might be a good idea to change that convention,
            // XXX since it is sort of an artifact of the old input symbol.
            backEdgeSigns[i] *= -1;
        }
    }

    // So we don't have to think about rotationalMultiplicity,
    // expand pp and backEdgeInds out, and set rotationalMultiplicity to 1.
    {
        Vector<int> expandedPP(ppLength * rotationalMultiplicity);
        Vector<int> expandedBackEdgeInds(ppLength * rotationalMultiplicity);
        Vector<int> expandedBackEdgeSigns(ppLength * rotationalMultiplicity);
        for(int i=0; i<expandedPP.length; i++){
            expandedPP[i] = pp[i%ppLength];
            expandedBackEdgeSigns[i] = backEdgeSigns[i%ppLength];

                                                                     //yes?
            expandedBackEdgeInds[i] = backEdgeInds[i%ppLength] + i/ppLength*ppLength;
        }

        pp = expandedPP;
        backEdgeInds = expandedBackEdgeInds;
        backEdgeSigns = expandedBackEdgeSigns;
        rotationalMultiplicity = 1;
    }

    //assert(pp.length == nGens);

    Node** nodes = new Node*[maxIsometries];


    DWORD ticks = GetTickCount();


    for(int i=maxIsometries-1; i>=0; i--) nodes[i] = NULL;
    
    try{
        int n = 0;
        if (n >= maxIsometries) return n;
        return_isometries[n] = F0;
        nodes[n] = new Node(nGens, 1, 0);

        //if (return_isometry_composition_lengths != NULL)
            return_isometry_composition_lengths[0] = 0;

        n++;
        if (n >= maxIsometries) return n; // XXX should wait so can statistic

        int iNode = 0;
        int iLevel = 0;

        for(iLevel=0; iLevel<maxLevels; iLevel++){
            //
            // In this pass, we create level iLevel+1 from level i.
            //
            int levelEnd = n;
            for (; iNode < levelEnd; ++iNode){
                //
                // Fill in neighbors of node iNode,
                // creating new nodes as necessary.
                //
                for(int _iGen=0; _iGen<nGens; _iGen++){
                    int iGen = (nodes[iNode]->start + nodes[iNode]->sign*_iGen + nGens) % nGens;
                    if (nodes[iNode]->neighbors[iGen] != -1){
                        continue; // it's already there
                    }

                    // Trace CW around face iNeighbor
                    // to see whether this node has already been
                    // created over there,
                    // then do the same tracing CCW around face iNeighbor-1.
                    {
                        for (int dir = 1; dir >= -1; dir -= 2) // 1 means CW around next face; -1 means CCW around prev face
                        {
                            int jNode = iNode;
                            int jGen = (iGen + dir*nodes[jNode]->sign + nGens) % nGens; // turn to next arm
                            int faceSize = pp[dir*nodes[iNode]->sign==-1 ? ((iGen-1 + nGens) % nGens) : iGen];
                            for(int iAlongFace=0; iAlongFace<faceSize-1; iAlongFace++){
                                // progress and reverse arm...
                                Node node(*nodes[jNode]);
                                //node = *nodes[jNode];
                                jNode = node.neighbors[jGen];
                                jGen = node.backEdgeInds[jGen];
                                if (jNode == -1)
                                    break;
                                jGen = (jGen + dir*nodes[jNode]->sign + nGens) % nGens; // turn to next arm
                            }
                            if (jNode != -1){
                                nodes[iNode]->neighbors[iGen] = jNode;
                                nodes[iNode]->backEdgeInds[iGen] = jGen;
                                nodes[jNode]->neighbors[jGen] = iNode;
                                nodes[jNode]->backEdgeInds[jGen] = iGen;
                                break;
                            }
                        }
                        if (nodes[iNode]->neighbors[iGen] != -1)
                            continue; // found it
                    }

                    // use return_isometries[n] as tentative storage...
                    Isometry I = return_isometries[iNode];

                    Isometry F = I * gens[iGen];
                    return_isometries[n] = F;


                    // If it generates an edge and the edge is too small...
                    if ((gens[iGen].P[0] != 0. || gens[iGen].P[1] != 0.)
                     && HYPOTSQRD(F.P[0]-I.P[0], F.P[1]-I.P[1]) < tooSmall*tooSmall){
                        continue;
                    }

                    // If it's too close to the edge of the circle...
                    double thisTranslateSqrd = HYPOTSQRD(F.P[0], F.P[1]);
                    if (thisTranslateSqrd >= tooFar*tooFar){
                        continue;
                    }


                    //
                    // Adding it for sure...
                    //

                    // return_isometries[n] is already set to F...

                    //
                    // Add the node to the graph...
                    //
                    int jNode = n;
                    int jGen = backEdgeInds[iGen];
                    nodes[jNode] = new Node(nGens,
                                        nodes[iNode]->sign * backEdgeSigns[iGen],
                                        jGen);
                    nodes[iNode]->neighbors[iGen] = jNode;
                    nodes[iNode]->backEdgeInds[iGen] = jGen;
                    nodes[jNode]->neighbors[jGen] = iNode;
                    nodes[jNode]->backEdgeInds[jGen] = iGen;


                    //if (return_isometry_composition_lengths != NULL)
                    //    return_isometry_composition_lengths[jNode] = return_isometry_composition_lengths[iNode]+1;

                    //if (return_isometry_composition_lengths != NULL
                    //    ? return_isometry_composition_lengths[jNode]%2 == 0
                    //    : return_isometries[jNode].R == 1)
                    //{
                    if (return_isometry_composition_lengths[jNode]%2 == 0){
                        if (thisTranslateSqrd < smallestTranslateSqrd){
                            smallestTranslateSqrd = thisTranslateSqrd;
                            return_smallestIsometry = F;
                        }
                    }

                    n++;

                    if (n >= maxIsometries) break; // XXX should wait so can statistic, maybe
                }
                if (n >= maxIsometries) break; // XXX should wait so can statistic, maybe
            }
            if (n >= maxIsometries) break; // XXX should wait so can statistic, maybe

        }

        return n;
        
    }__finally{
        for(int i=0; i<maxIsometries; i++)
            if(nodes[i]) delete nodes[i];
        delete [] nodes;
    }
    return(0);
}
//----------------------------------------------------------------------------










/*

// Much of this algorithm adapted from Don Hatch (www.hadron.org)
//
int UnCachedEnumerateIsometryGroup(Isometry F0,
                                   int nGens,
                                   Isometry gens[],
                                   int rotationalMultiplicity,
                                   int maxIsometries,
                                   Isometry return_isometries[],
                                   Isometry& return_smallestIsometry,
                                   int maxLevels,
                                   double tooFar,
                                   double tooSmall)
{
  TList* hashTable = new TList;
  int n = 0;
  try{

    if (!maxLevels) return 0;
    if (!maxIsometries) return 0;

    double smallestTranslateSqrd = HYPOTSQRD(F0.P[0],F0.P[1]);
    return_smallestIsometry.set(F0);

    if (n >= maxIsometries) return n;
    hashTable->Add((void*)F0.hashCode());

    return_isometries[n++] = F0;
    if (n >= maxIsometries) return n; // XXX should wait so can statistic
    int i;

    int nIsometriesOnPreviousLevels = 1;
    int generatingLevel = 1;
    if (generatingLevel >= maxLevels)
        return n;

    Isometry rotation = Isometry::pureRotation(PI/rotationalMultiplicity);

    Isometry temp();

    rotationalMultiplicity = 1; // XXX doesn't work with other multiplicities??? should it? not sure, just kind of blindly ported.  changing it to 1 isn't wrong, it just produces stuff in different order

    for (i = 0; i < n; ++i){
        if (i == nIsometriesOnPreviousLevels){
            if (++generatingLevel >= maxLevels)
                break;
            nIsometriesOnPreviousLevels = n;
        }
        Isometry I(return_isometries[i]); // XXX maybe allocate once at top
        for (int iRot = 0; iRot < rotationalMultiplicity; ++iRot){
            for (int iGen = 0; iGen < nGens; ++iGen){
                // use return_isometries[n] as tentative storage...
                return_isometries[n] = I * gens[iGen];
                Isometry F = return_isometries[n];

                // If it generates an edge and the edge is too small...
                if ((gens[iGen].P[0] != 0.0 || gens[iGen].P[1] != 0.0)
                 && HYPOTSQRD(F.P[0]-I.P[0],
                              F.P[1]-I.P[1]) < tooSmall*tooSmall)
                {
                    continue;
                }

                // If it's too close to the edge of the circle...
                double thisTranslateSqrd = HYPOTSQRD(F.P[0], F.P[1]);
                if (thisTranslateSqrd >= tooFar*tooFar)
                    continue;

                if(hashTable->IndexOf((void*)F.hashCode()) > -1){
                    continue;  //already exists!
                }else{
                    hashTable->Add((void*)F.hashCode());
                }

                if (thisTranslateSqrd < smallestTranslateSqrd){
                    smallestTranslateSqrd = thisTranslateSqrd;
                    return_smallestIsometry.set(F);
                }
                n++;

                if (n >= maxIsometries) break; // XXX should wait so can statistic
            }

            if (n >= maxIsometries) break;
            if (iRot == rotationalMultiplicity-1)
                break; // don't bother rotating after last one

            I.set(I * rotation);
        }
        if (n >= maxIsometries) break;
    }
  }__finally{
    delete hashTable;    //deallocate
  }
  return n;
}
//-----------------------------------------------------------------
*/


//
// take the geodesic in the poincare disk
// from p0 to p1, and subdivide it
// as a circular arc in euclidean space.
// The arc will be part of a circle that
// intersects the unit circle at right angles.
//
int subdivideArc(complex& p0, complex& p1, double tolerance, int maxIntermediatePoints, int startI, Vector<complex>& result_intermediatePoints)
{
    //double fullDist;
    //{
        // calculate the hyperbolic distance between p0 and p1,
        // sort of guessing here
        //complex _p0(p0);
        //complex _p1(p1);
        //complex _p1_minus_p0 = Isometry::pureTranslation(-_p0[0],-_p0[1]).apply(_p1);
        //fullDist = e2hNorm(sqrt(HYPOTSQRD(_p1_minus_p0[0],_p1_minus_p0[1])));
    //}

    //int N = p0.length;
    //assert(N == 2);

    // XXX for now, try to do hlerp, I just want to see how it looks
    int nIntermediatePoints = maxIntermediatePoints;
    int i;
    //double sinh_fullDist = sinh(fullDist);
    for(i=0; i<nIntermediatePoints; i++){
        double t = (double)(i+1)/(nIntermediatePoints+1);
        //if (false) // XXX this didn't work, but it seems something like it might...?
        //    VecMath.sxvpsxv(result_intermediatePoints[startI+i],
        //                    MyMath.sinh((1-t)*fullDist)/sinh_fullDist, p0,
        //                    MyMath.sinh(    t*fullDist)/sinh_fullDist, p1);
        //else
            hlerp2_poincare(p0, p1, t, result_intermediatePoints[startI+i]);
    }
    
    return nIntermediatePoints;
} // subdivideArc





double hlerp2_poincare(complex& p0, complex& p1, double t, complex& result){
    Isometry take_p0_to_origin = Isometry::pureTranslation(-p0[0],-p0[1]);
    Isometry take_origin_to_p0 = Isometry::pureTranslation(p0[0],p0[1]);
    complex p1_; take_p0_to_origin.apply(p1, p1_);
    double normSqrd = HYPOTSQRD(p1_[0],p1_[1]);
    if (normSqrd < 1e-12){
        result = complex(p0[0] + t*(p1[0]-p0[0]), p0[1] + t*(p1[1]-p0[1]));
        return 2*sqrt(normSqrd); // success
    }
    if (normSqrd > 1.0){
        result = zero;
        return -1.0; // failure; points are on opposite sides of the wall
    }
    double norm = sqrt(normSqrd);
    double atanNorm = atanh(norm);
    double desiredNorm = __tanh(t * atanNorm);

    complex p_(p1_); // use same space
    p_ = p1_ * (desiredNorm/norm);
    take_origin_to_p0.apply(p_, result);
    return 2*atanNorm; // success
}
//-----------------------------------------------------------


// the version with aspect
double hlerp2_poincare(complex& p0, complex& p1, double t, double aspect, complex& result){
    if (aspect == 1.0)
        return hlerp2_poincare(p0,p1,t,result);
    complex _p0;
    complex _p1;
    _p0 = p0 * pow(sqrt(dot(p0,p0)), 1.0/aspect-1.0);
    _p1 = p1 * pow(sqrt(dot(p1,p1)), 1.0/aspect-1.0);
    double dist = hlerp2_poincare(_p0,_p1,t,result);
    result = result * pow(sqrt(dot(result,result)), aspect-1.0);
    return dist;
}
//-------------------------------------------------------------

double hdist2_poincare(complex p0, complex p1){
    complex temp;
    return hlerp2_poincare(p0, p1, 0., temp);
}
//--------------------------------------------------------


double hlerp2_uhp(complex& p0, complex& p1, double t, double aspect, complex& result){
    complex p0_, p1_, p_;

    // convert to disk...
    uhp2d(p0[0], p0[1]/aspect, p0_);
    uhp2d(p1[0], p1[1]/aspect, p1_);

    // do the lerp...
    double ret = hlerp2_poincare(p0_, p1_, t, p_);

    // convert back...
    d2uhp(p_[0], p_[1], result);
    result[0] = result[0];
    result[1] = result[1] * aspect;
    return ret;
}
//--------------------------------------------------------


double hlerp2_simple_inversion(complex& p0, complex& p1, double t, double aspect, bool conformal, complex& result){
    complex p0_;
    complex p1_;
    complex u0; // put it out here since we use it again
    {
        double invLength0 = 1.0/sqrt(dot(p0,p0));
        double invLength1 = 1.0/sqrt(dot(p1,p1));
        complex u1;
        u0 = p0 * invLength0;
        u1 = p1 * invLength1;
        complex u;
        u = u0 + u1;
        u = u * (1./sqrt(dot(u,u))); // normalize
        p1_ = u * sqrt(invLength1);
        p0_ = u0 * sqrt(invLength0);
    }
    double ret = hlerp2_poincare(p0_, p1_, t, result);

    if (ret != -1.0){
        double finalLength = 1.0/dot(result,result); // square of inverse of length of result so far
        complex u;
        u = result * sqrt(finalLength); // unit vector in direction of result so far
        complex halfway;
        halfway = u * dot(u0,u);
        complex fullway;
        fullway = halfway*2.0 + u0*(-1.0);
        result = fullway * finalLength;
    }
    return ret;
}
//---------------------------------------------------------------------

// distance to circle centered at (0,-1).
// very accurate for points near the origin.
double distToCircleCenteredAtMinus1(double x, double y){
    return (x*x + y*(y+2.0)) / (sqrt(x*x + (y+1.0)*(y+1.0)) + 1.0);
}
//---------------------------------------------------------------------

// so we can get insanely accurate near the origin! use the above function.
double hlerp2_polar(complex& p0, complex& p1, double t, double aspect, bool conformal, complex& result)
{
    // Try to think in the space where the earth is centered at (0,-1)...
    double x0 = p0[0];
    double y0 = p0[1]-1.0; // XXX remove -1 when calling semantics changed
    double x1 = p1[0];
    double y1 = p1[1]-1.0; // XXX remove -1 when calling semantics changed
    double actualAlt0 = distToCircleCenteredAtMinus1(x0,y0);
    double actualAlt1 = distToCircleCenteredAtMinus1(x1,y1);

    // angle=0 means the positive y axis (not the positive x axis!)
    // so very small values of x produce very small angles, accurately.
    double angle0 = atan2(-x0,y0+1.0);
    double angle1 = atan2(-x1,y1+1.0);

    // tweak angle1 so it's < 180 degrees from angle0
    {
        if (angle1 > angle0 + PI)
            angle1 -= 2.0*PI;
        else if (angle1 < angle0 - PI)
            angle1 += 2.0*PI;
    }

    double alt0 = log1p(actualAlt0);
    double alt1 = log1p(actualAlt1);
    if (!conformal){
        alt0 = actualAlt0;
        alt1 = actualAlt1;
    }

    complex p0_(angle0, alt0);
    complex p1_(angle1, alt1);
    double ret = hlerp2_uhp(p0_, p1_, t, aspect, result);
    {
        double angle = result[0];
        double alt = result[1];
        double actualAlt = conformal ? expm1(alt) : alt;
        double r = actualAlt+1.0;
        double x = -r * sin(angle);
        // rewrite it the perspicuous way...
        double y = actualAlt - r * cosf1(angle);
        result = complex(x, y+1.0); // XXX remove the +1 when using correct space
    }
    return ret;
}
//----------------------------------------------------------------------


// Much of this algorithm adapted from Don Hatch (www.hadron.org)
//
bool hspline2_polar(double t0, complex& p0, double t1, complex& p1, double t2, complex& p2, double t3, complex& p3, double t, double aspect, bool conformal, LerpFunction& lerp, complex& result)
{
    // Normalize t's so that t1=0 and t2=1...
    {
        double transT = -t1;
        double scaleT = 1./(t2-t1);
        t0 = (t0+transT)*scaleT;
        t1 = 0.; // = (t1+transT)*scaleT;
        t2 = 1.; // = (t2+transT)*scaleT;
        t3 = (t3+transT)*scaleT;
        t = (t+transT)*scaleT;
    }


    // Calculate the matrix of w coefficients, good for any t...
    double W[4][4];
    {
        double invT[4][4] = {
                                {1,0,-3,2},
                                {0,0,3,-2},
                                {0,1,-2,1},
                                {0,0,-1,1},
                            };
        double thatHairyMatrixOnTheRight[4][4] = {
            {0, 0, -1/((1-t0)*(-t0)),              0                      },
            {1, 0, 1/((1-t0)*(-t0))-(-t0)/(1-t0), -(t3-1)/t3              },
            {0, 1, (-t0)/(1-t0),                   (t3-1)/t3-1/(t3*(t3-1))},
            {0, 0, 0,                              1/(t3*(t3-1))          },
        };
        mxm(W, thatHairyMatrixOnTheRight, invT);
    }

    double w0, w1, w2, w3;
    {
        double tVec[4] = {1, t, t*t, t*t*t};
        double wVec[4];
        mxv(wVec, W, tVec); // wVec = W * tVec
        w0 = wVec[0];
        w1 = wVec[1];
        w2 = wVec[2];
        w3 = wVec[3];
    }

    {
        double w1_01 = (w0+w1 == 0. ? .5 : w1/(w0+w1));
        double w3_23 = (w2+w3 == 0. ? .5 : w3/(w2+w3));
        double w23 = w2+w3;
        complex A;
        complex B;

        if (lerp.apply(p0,p1, w1_01, aspect, conformal, A) == -1.0) return false;
        if (lerp.apply(p2,p3, w3_23, aspect, conformal, B) == -1.0) return false;
        if (lerp.apply(A,B,   w23,   aspect, conformal, result) == -1.0) return false;
    }
    return true;
}
//------------------------------------------------------


// circular-inversion isometry between Poincare disk and upper half plane
void canonicalCircularInvert(double x, double y, complex& result){
    double scale = 2.0 / (x*x + (y+1.0)*(y+1.0));
    result[0] = x * scale;
    result[1] = (y+1.0) * scale - 1.0;
}
//------------------------------------------------------

bool hlerp3_polar(complex& p0, complex& p1, double t, double aspect, complex& result){

    // in the not-so-distant future

    return false;
}
//------------------------------------------------------

// compose with a reflection of the disk about the x axis, for a more intuitive conversion...
void d2uhp(double x, double y, complex& result){
    canonicalCircularInvert(x, -y, result);
}
//------------------------------------------------------

void uhp2d(double x, double y, complex& result){
    canonicalCircularInvert(x, y, result);
    result[1] = -result[1];
}
//------------------------------------------------------


//---------------------------------------------------------------------------