#include <iostream>
#include <cmath>

#include <ICOMETLog.hxx>

#include "IDelaunay2D.hxx"

//  This code was implemented after the algorithm described in Delaunay-tree,
//  however it has been recoded to meet our requirements.  

// The author of Delaunay-tree is:
//
//  Olivier.Devillers@sophia.inria.fr
//  2004 route des Lucioles,
//  BP 93 06902 Sophia Antipolis Cedex 
//  (33) 93 65 77 63
//  Fax (33) 93 65 76 43
//  http://www.inria.fr:/prisme/personnel/devillers/devillers.html
// 

/// A simple concrete representation of a point that will be connected into a
/// mesh.
class IInternalXYPoint : public COMET::IMeshPoint {
public :
    /// Create a new point.
    IInternalXYPoint(double x, double y) : fX(x), fY(y) { }
    virtual ~IInternalXYPoint() {}

    /// Return the X value of the point.
    double X(void) const {return fX;}
    
    /// Return the X value of the point.
    double Y(void) const {return fY;}
    
private :
    // The X Coordinate of the point.
    double fX;

    // The Y Coordinate of the point.
    double fY;
};

/// Find the sum of two points.
static inline COMET::IMeshXYPoint sum(const COMET::IMeshPoint& a,
                               const COMET::IMeshPoint& b) { 
    return COMET::IMeshXYPoint(a.X()+b.X(), a.Y()+b.Y());
}

/// Find the difference between two points
static inline COMET::IMeshXYPoint difference(const COMET::IMeshPoint& a,
                                      const COMET::IMeshPoint& b) { 
    return COMET::IMeshXYPoint(a.X()-b.X(), a.Y()-b.Y());
}

/// Find the inner product of two points.
static inline double innerProduct(const COMET::IMeshPoint& a,
                                  const COMET::IMeshPoint& b) { 
    return a.X()*b.X() + a.X()*b.X();
}

/// Find the outer product of two points.
static inline double outerProduct(const COMET::IMeshPoint& a,
                          const COMET::IMeshPoint& b) { 
    return a.X()*b.Y() - a.Y()*b.X();
}

COMET::IDelaunayTriangle::IDT_list::~IDT_list(void) { 
    if (fNext) {
        delete fNext;
        fNext = NULL;
        fKey = NULL;
    }
}

// Check if a point is inside the circumcircle of the current node and
// return true if it is.  If this is true, the triangle will need to be
// split.
bool COMET::IDelaunayTriangle::Conflict(COMET::IMeshPoint *p) {
    switch(fFlag.is_infinite()) {
    case 4:
        return false;
    case 3:
        return true;
    case 2:
        return (0<=innerProduct(difference(*p,*fVertices[0]),
                                sum(*fVertices[1],*fVertices[2])));
    case 1:
        if (fFlag.is_last_finite()) {
            return (0<=outerProduct(difference(*p,*fVertices[2]),
                                    difference(*fVertices[2],*fVertices[0])));
        }
        else {
            return (0<=outerProduct(difference(*p,*fVertices[0]),
                                  difference(*fVertices[0],*fVertices[1])));
        }

    case 0:
        //  compute the det 4*4 column: x,y,x**2+y**2,1 for p and fVertices
        //  [0,1,2] 
        double x  = p->X();
        double y  = p->Y();
        double x0 = fVertices[0]->X();
        double y0 = fVertices[0]->Y();
        double x1 = fVertices[1]->X();
        double y1 = fVertices[1]->Y();
        double x2 = fVertices[2]->X();
        double y2 = fVertices[2]->Y();
        x1-=x0;
        y1-=y0;
        x2-=x0;
        y2-=y0;
        x-=x0;
        y-=y0;
        double z1=(x1*x1)+(y1*y1);
        double z2=(x2*x2)+(y2*y2);
        double alpha=(y1*z2)-(z1*y2);
        double beta=(x2*z1)-(x1*z2);
        double gamma=(x1*y2)-(y1*x2);
        return ((alpha*x)+(beta*y)+gamma*((x*x)+(y*y))  <= 0);
    }
    return false;
}

COMET::IDelaunayTriangle::~IDelaunayTriangle() {
    if (fFlag.is_infinite() == 3) {
        // Delete the vertex.
    }
    if (fDaughters) {
        delete fDaughters;
    }
}

COMET::IDelaunayTriangle::IDelaunayTriangle(COMET::IDelaunay2D *owner) {
    owner->AddTriangle(this);
    double scale = 10000.0;
    fVertices[0] = new IInternalXYPoint(1.0*scale,  0.0*scale );
    fVertices[1] = new IInternalXYPoint(-0.5*scale,  0.8660254*scale);
    fVertices[2] = new IInternalXYPoint(-0.5*scale, -0.8660254*scale);

    fFlag.infinite(3);
    fFlag.Bogus();
    fNumber = 0;
    fDaughters = NULL;
}

COMET::IDelaunayTriangle::IDelaunayTriangle(COMET::IDelaunay2D *owner,
                                     COMET::IDelaunayTriangle *root, int i) {
    owner->AddTriangle(this);
    fVertices[0] = root->fVertices[0] ;
    fVertices[1] = root->fVertices[1] ;
    fVertices[2] = root->fVertices[2] ;
    fFlag.infinite(4);
    fFlag.Bogus();
    fNumber = 0;
    fDaughters = NULL;
    fNeighbors[i] = root ;
    root->fNeighbors[i] = this ;
}


// The triangle is created in ccw order.  The circumdisk and flatness are not
// computed.
COMET::IDelaunayTriangle::IDelaunayTriangle(COMET::IDelaunay2D *owner,
                                     COMET::IDelaunayTriangle *f, 
                                     COMET::IMeshPoint *c, int i) {
    owner->AddTriangle(this);
    switch(f->fFlag.is_infinite())
    {
    case 0: 
        fFlag.infinite(0);
        break;
    case 1: 
        if (f->fFlag.is_last_finite()) {
            fFlag.infinite((i==1) ? 0 : 1);
        }
        else {
            fFlag.infinite((i==2) ? 0 : 1);
        }
        if (fFlag.is_infinite()) {
            if (f->fFlag.is_last_finite()) {
                if (i==0) fFlag.last_finite();
            }
            else {
                if (i==1) fFlag.last_finite();
            }
        }
        break;
    case 2:
        fFlag.infinite((i==0) ? 2 : 1);
        if (i==1) fFlag.last_finite();
        break;
    case 3:
        fFlag.infinite(2);
        break;
    }
    fNumber = 0;
    fDaughters = NULL;
    f->fDaughters = new IDT_list(f->fDaughters, this);
    f->fNeighbors[i]->fDaughters 
        = new IDT_list(f->fNeighbors[i]->fDaughters, this);
    f->fNeighbors[i]->fNeighbors[ f->fNeighbors[i]->NeighborIndex(f) ] = this;
    fVertices[0] = c;
    fNeighbors[0] = f->fNeighbors[i];
    switch (i) {
    case 0:
        fVertices[1] = f->fVertices[1];
        fVertices[2] = f->fVertices[2];
        break;
    case 1:
        fVertices[1] = f->fVertices[2];
        fVertices[2] = f->fVertices[0];
        break;
    case 2:
        fVertices[1] = f->fVertices[0];
        fVertices[2] = f->fVertices[1];
        break;
    }
}


//*****************************************************************************
//
//  Purpose:
//
//    COMET::IDelaunayTriangle::FindConflict returns an alive node in conflict.
//
//  Author:
//
//    Olivier Devillers
//
COMET::IDelaunayTriangle* COMET::IDelaunayTriangle::FindConflict(COMET::IMeshPoint *p) {
    
    if (!Conflict(p)) return NULL;

    if (!fFlag.HasSplit()) return this;

    for (IDT_list* l = fDaughters; l; l = l->GetNext()) {
        if (l->GetNode()->fNumber != fNumber) {
            l->GetNode()->fNumber = fNumber;
            COMET::IDelaunayTriangle* n=l->GetNode()->FindConflict(p);
            if (n) return n;
        }
    }

    return NULL;
}

void COMET::IDelaunayTriangle::Circle(double& xc, double& yc, double& r) {
    const double epsilon = 1E-6;

    if (std::abs(fVertices[1]->Y()-fVertices[0]->Y()) < epsilon) {
        double m2 = - (fVertices[2]->X()-fVertices[1]->X())
            / (fVertices[2]->Y()-fVertices[1]->Y());
        double mx2 = (fVertices[1]->X() + fVertices[2]->X())/2.0;
        double my2 = (fVertices[1]->Y() + fVertices[2]->Y())/2.0;
        xc = (fVertices[1]->X() + fVertices[0]->X())/2.0;
        yc = m2 * (xc - mx2) + my2;
    }
    else if (std::abs(fVertices[2]->Y()-fVertices[1]->Y()) < epsilon) {
        double m1 = - (fVertices[1]->X()-fVertices[0]->X()) 
            / (fVertices[1]->Y()-fVertices[0]->Y());
        double mx1 = (fVertices[0]->X() + fVertices[1]->X())/2.0;
        double my1 = (fVertices[0]->Y() + fVertices[1]->Y())/2.0;
        xc = (fVertices[2]->X() + fVertices[1]->X())/2.0;
        yc = m1 * (xc - mx1) + my1;
    }
    else {
        double m1 = - (fVertices[1]->X()-fVertices[0]->X()) 
            / (fVertices[1]->Y()-fVertices[0]->Y());
        double m2 = - (fVertices[2]->X()-fVertices[1]->X()) 
            / (fVertices[2]->Y()-fVertices[1]->Y());
        double mx1 = (fVertices[0]->X() + fVertices[1]->X())/2.0;
        double mx2 = (fVertices[1]->X() + fVertices[2]->X())/2.0;
        double my1 = (fVertices[0]->Y() + fVertices[1]->Y())/2.0;
        double my2 = (fVertices[1]->Y() + fVertices[2]->Y())/2.0;
        xc = (m1 * mx1 - m2 * mx2 + my2 - my1)/(m1 - m2);
        yc = m1 * (xc - mx1) + my1;
    }
    
    double dx = fVertices[1]->X() - xc;
    double dy = fVertices[1]->Y() - yc;
    r = sqrt(dx*dx + dy*dy);
}

COMET::IDelaunay2D::IDelaunay2D(void) {
    Init();
}

void COMET::IDelaunay2D::Init(void) {
    fNumber = 0;
    fRoot = new COMET::IDelaunayTriangle(this);
    new COMET::IDelaunayTriangle(this, fRoot, 0);
    new COMET::IDelaunayTriangle(this, fRoot, 1);
    new COMET::IDelaunayTriangle(this, fRoot, 2);
    fRoot->GetNeighbor(0)->SetNeighbor(1,fRoot->GetNeighbor(1));
    fRoot->GetNeighbor(0)->SetNeighbor(2,fRoot->GetNeighbor(2));
    fRoot->GetNeighbor(1)->SetNeighbor(0,fRoot->GetNeighbor(0));
    fRoot->GetNeighbor(1)->SetNeighbor(2,fRoot->GetNeighbor(2));
    fRoot->GetNeighbor(2)->SetNeighbor(0,fRoot->GetNeighbor(0));
    fRoot->GetNeighbor(2)->SetNeighbor(1,fRoot->GetNeighbor(1));
    double scale = 10000.0;
    COMET::IMeshPoint* point = new IInternalXYPoint(-1.0*scale,  1.0*scale );
    AddPoint(point);
    TMeshPointSet::iterator p = fPoints.find(point);
    fPoints.erase(p);
    point = new IInternalXYPoint(1.0*scale,  -1.0*scale );
    AddPoint(point);
    p = fPoints.find(point);
    fPoints.erase(p);
}

COMET::IDelaunay2D::~IDelaunay2D(void) {
    for (std::vector<COMET::IDelaunayTriangle*>::iterator t = fChildren.begin();
         t != fChildren.end();
         ++t) {
        delete *t;
    }
}

COMET::IDelaunay2D::ITriangleIterator COMET::IDelaunay2D::BeginTriangles() {
    return COMET::IDelaunay2D::ITriangleIterator(fRoot);
}

COMET::IDelaunay2D::ITriangleIterator COMET::IDelaunay2D::fEnd;
COMET::IDelaunay2D::ITriangleIterator& COMET::IDelaunay2D::EndTriangles() {
    return fEnd;
}

COMET::IMeshPoint* COMET::IDelaunay2D::AddPoint(COMET::IMeshPoint *point) {
    // Protect against NULL pointers.
    if (!point) {
        throw;
    }
    // Add the point to the mesh and check if it was already added.
    COMET::IMeshPoint* p = COMET::IMesh::AddPoint(point);
    if (p != point) return p;
    int i;                      // the index of a neighbor... used a lot.

    fRoot->SetNumber(++fNumber);

    int throttle = 100*fPoints.size();
#define THROTTLE(str) {if (--throttle<0) {COMETSevere(str);return NULL;}}

    // This finds the triangle that will become the mother of the triangle
    // that will contain the new point.
    COMET::IDelaunayTriangle* n = fRoot->FindConflict(p);
    if (!n) return p;

    // Do a sanity check to make sure that the point was not already added.
    // This should NEVER be true;
    for (i=0; (int) i < (3- (int) n->GetFlag().is_infinite()); ++i) {
        if ((p->X()==n->GetVertex(i)->X())&&(p->Y()==n->GetVertex(i)->Y())) {
            throw;
        }
        THROTTLE("Loop 1");
    }

    // Mark the mother triangle as invalid so it can be split.  Really need to
    // change the flag name so this isn't quite so violent... really bugs me.
    n->GetFlag().Split();

    // we will turn cw around first vertex of n, till next triangle
    // is not in conflict
    COMET::IMeshPoint* q = n->GetVertex(0);
    
    while (n->GetNeighbor(i=n->ClockwiseNeighbor(q))->Conflict(p)) {
        n = n->GetNeighbor(i);
        n->GetFlag().Split();
        THROTTLE("Find Conflict");
    }

    COMET::IDelaunayTriangle* first, *last;
    first = last = new COMET::IDelaunayTriangle(this,n,p,i);
    // we will turn cw around r, till next triangle is not in conflict
    COMET::IMeshPoint* r = n->GetVertex((((int) i)+2)%3);
    while (true) {
        THROTTLE("Find Conflict");
        i = n->ClockwiseNeighbor(r);
        if (n->GetNeighbor(i)->GetFlag().HasSplit()) {
            n = n->GetNeighbor(i); 
            continue;
        }
        if (n->GetNeighbor(i)->Conflict(p)) {
            n = n->GetNeighbor(i);
            n->GetFlag().Split();
            continue;
        }
        break;
    }
    // n is split by p
    // n->GetNeighbor(i) is not in conflict with p
    // r is vertex i+1 of n
    while (true) {
        THROTTLE("Find Conflict");
        COMET::IDelaunayTriangle* created = new COMET::IDelaunayTriangle(this,n,p,i);
        created->SetNeighbor(2,last);
        last->SetNeighbor(1,created);
        last=created;
        r = n->GetVertex((((int) i)+2)%3);   // r turn in n ccw

        if (r==q) break;
        // we will turn cw around r, till next triangle is not in conflict
        while (true) {
            THROTTLE("Find Conflict");
            i = n->ClockwiseNeighbor(r);
            if (n->GetNeighbor(i)->GetFlag().HasSplit()) {
                n = n->GetNeighbor(i);
                continue;
            }
            if (n->GetNeighbor(i)->Conflict(p)) {
                n = n->GetNeighbor(i);
                n->GetFlag().Split();
                continue;
            }
            break;
        }
    }
    first->SetNeighbor(2,last);
    last->SetNeighbor(1,first);
    return p;
}

void COMET::IDelaunay2D::Close() {
    for (ITriangleIterator t = BeginTriangles(); t!=EndTriangles(); ++t) {
        COMET::IMeshPoint* v0 = t->GetVertex(0);
        COMET::IMeshPoint* v1 = t->GetVertex(1);
        COMET::IMeshPoint* v2 = t->GetVertex(2);
        if (!dynamic_cast<IInternalXYPoint*>(v0)
            && !dynamic_cast<IInternalXYPoint*>(v1))
            AddEdge(new COMET::IMeshEdge(v0,v1));
        if (!dynamic_cast<IInternalXYPoint*>(v1)
            && !dynamic_cast<IInternalXYPoint*>(v2))
            AddEdge(new COMET::IMeshEdge(v1,v2));
        if (!dynamic_cast<IInternalXYPoint*>(v2)
            && !dynamic_cast<IInternalXYPoint*>(v0))
            AddEdge(new COMET::IMeshEdge(v2,v0));
    }
}

void COMET::IDelaunay2D::Paint(Option_t* opt) {
    COMET::IMesh::Paint(opt);

#ifdef DRAW_CIRCLES
    /// The code to draw the Delaunay Circles should go here (controlled by
    /// the "circles" option).  It's not here since root doesn't have a circle
    /// primative for the pad and it's a lot of code to draw.
    for (ITriangleIterator t = BeginTriangles(); t!=EndTriangles(); ++t) {
        double xc, yc, r;
        t->Circle(xc,yc,r); 
        std::cout << " " << xc << " " << yc << " " << r << std::endl;
    }
#endif

}

COMET::IDelaunay2D::ITriangleIterator::ITriangleIterator() {}

COMET::IDelaunay2D::ITriangleIterator::ITriangleIterator(COMET::IDelaunayTriangle* node) {
    fQueue.push(node);
    fSeen.insert(node);
    PackQueue();
    if ((*this)->GetFlag().GetValue() != 0) this->operator ++();
}

COMET::IDelaunay2D::ITriangleIterator::ITriangleIterator(
    const COMET::IDelaunay2D::ITriangleIterator& i) 
    : fQueue(i.fQueue), fSeen(i.fSeen) { }

COMET::IDelaunay2D::ITriangleIterator::~ITriangleIterator() {}

COMET::IDelaunayTriangle& COMET::IDelaunay2D::ITriangleIterator::operator *() const {
    return *(fQueue.front());
}

COMET::IDelaunayTriangle* COMET::IDelaunay2D::ITriangleIterator::operator ->() const {
    return fQueue.front();
}

COMET::IDelaunay2D::ITriangleIterator& COMET::IDelaunay2D::ITriangleIterator::operator ++() {
    do {
        fQueue.pop();
        if (fQueue.size()<1) break;
        PackQueue();
    } while (fQueue.front()->GetFlag().GetValue() != 0);
    return *this;
}

bool COMET::IDelaunay2D::ITriangleIterator::operator == (ITriangleIterator& rhs) {
    if (fQueue.size()<1 && rhs.fQueue.size()<1) return true;
    if (fQueue.size()<1) return false;
    if (rhs.fQueue.size()<1) return false;
    return (fQueue.front() == rhs.fQueue.front());
}

bool COMET::IDelaunay2D::ITriangleIterator::operator != (ITriangleIterator &rhs) {
    return !(operator ==(rhs));
}

void COMET::IDelaunay2D::ITriangleIterator::PackQueue() {
    if (fQueue.size()<1) return;
    COMET::IDelaunayTriangle* t = fQueue.front();
    if (fSeen.find(t->GetNeighbor(0)) == fSeen.end()) {
        fQueue.push(t->GetNeighbor(0));
        fSeen.insert(t->GetNeighbor(0));
    }
    if (fSeen.find(t->GetNeighbor(1)) == fSeen.end()) {
        fQueue.push(t->GetNeighbor(1));
        fSeen.insert(t->GetNeighbor(1));
    }
    if (fSeen.find(t->GetNeighbor(2)) == fSeen.end()) {
        fQueue.push(t->GetNeighbor(2));
        fSeen.insert(t->GetNeighbor(2));
    }
}

std::ostream& operator << (std::ostream& s, COMET::IDelaunayTriangle& t) {
    s << &t << "->(" << t.GetVertex(0) 
      << ", " << t.GetVertex(1) 
      << ", " << t.GetVertex(2)
      << ")";
    return s;
}