/*********************************************
 *
 * Cedric Pradalier
 * DEA 2000/2001 
 * INRIA Rhones Alpes
 * http://cedric.pradalier.free.fr/index.html
 * mail : http://cedric.pradalier.free.fr/mail.html
 *
 * *******************************************/
#include "LocalisationGraph.h"
#include "Polygon.h"
#include <stack.h>
#include <math.h>
#include "Balise.h"
#include "TriangleStrip.h"


void LocalisationGraph::Node::PrintConfiguration(FILE * fp)
{
    fprintf(fp,"Theta = %e\n",theta);
    poly->Print(fp);
}

void LocalisationGraph::Node::Print(FILE * fp)
{
    int i;
    fprintf(fp,"Address = %p\n",this);
    fprintf(fp,"Theta = %e\n",theta);
    fprintf(fp,"Connex Id = %d\n",connId);
    poly->Print(fp);
    fprintf(fp,"Connections[%d] : ",connection.size());
    for (i=0;i<connection.size();i++)
            fprintf(fp,"%p ",connection.elementAt(i));
    fprintf(fp,"\n\n");
}


void LocalisationGraph::Node::exploreAndMark(int mark)
{
    queue<Node*> Q;
    Node *current, *son;
    connId = mark;
    Q.push(this);
    int i;

    while (!Q.empty())
    {
        current = Q.front();
        Q.pop();
        for (i=0;i<current->connection.size();i++)
        {
            son = (Node*)(current->connection.elementAt(i));
            if (son->connId < 0)
            {
                son->connId = mark;
                Q.push(son);
            }
        }
    }
}

void LocalisationGraph::Layer::Print(FILE*fp)
{
        int j;
        fprintf(fp,"Layer %f : %d\n",theta,nodes->size());
        for (j=0;j<nodes->size();j++)
                ((Node *)(nodes->elementAt(j)))->Print(fp);
}

LocalisationGraph::LocalisationGraph(Polygon *ws,double tstep)
{
        if (ws == NULL) {printf("Unexpected Null workspace\n");}
        if (tstep == 0) {printf("Unexpected zero theta step\n");}
        workSpace = ws;
        theta_step = tstep;
        ConnexParts = -1;
        graph = new Vector();
}

void LocalisationGraph::buildTriStrips()
{
        int i;
        for (i=0;i<graph->size();i++)
                ((Layer*)(graph->elementAt(i)))->buildTriStrips();
}

void LocalisationGraph::Layer::buildTriStrips()
{
        int i;
        for (i=0;i<nodes->size();i++)
                ((Node*)(nodes->elementAt(i)))->buildTriStrips();
}

void LocalisationGraph::Node::buildTriStrips()
{
        poly->buildTriStrips();
}

LocalisationGraph::LocalisationGraph(FILE * fp)
{
        int i,n;
        address_mapper amap;
        fscanf(fp," #Layers = %d ",&n);
        fscanf(fp," Theta step = %le ",&theta_step);
        fscanf(fp," Connex Parts = %d ",&ConnexParts);
        fscanf(fp," Work space = ");
        workSpace = new Polygon();
        workSpace->Read(fp);
        graph = new Vector();
        for (i=0;i<n;i++)
                graph->addElement(new Layer(fp,&amap));
        // update pointer links
        for (i=0;i<n;i++)
                ((Layer*)(graph->elementAt(i)))->updateLinks(&amap);
}

LocalisationGraph::Layer::Layer(FILE * fp,address_mapper* amap)
{
        int j,n;
        fscanf(fp," Layer %le : %d ",&theta,&n);
        nodes = new Vector();
        for (j=0;j<n;j++)
                nodes->addElement(new Node(fp,amap));
}

LocalisationGraph::Node::Node(FILE * fp,address_mapper* amap)
{
    int i,a,n;
    score = -1;
    fscanf(fp," Address = %x ",&a);
    (*amap)[a]=this;
    fscanf(fp,"Theta = %le\n",&theta);
    fscanf(fp," Connex Id = %d ",&connId);
    poly=new VisibilityPolygon(fp);
    fscanf(fp," Connections[ %d ] : ",&n);
    for (i=0;i<n;i++)
    {
            fscanf(fp," %x ",&a);
            connection.addElement((Object*)(a));
    }
}

void LocalisationGraph::Layer::updateLinks(address_mapper* amap)
{
        int j;
        for (j=0;j<size();j++)
                getNode(j)->updateLinks(amap);
}

void LocalisationGraph::Node::updateLinks(address_mapper* amap)
{
        int i;
        for (i=0;i<connection.size();i++)
                connection.setElement(i,(*amap)[(int)(connection[i])]);
}

LocalisationGraph::~LocalisationGraph()
{
        graph->deleteAll();
        delete graph;
        graph = NULL;
        VisibilityPolygon::deleteVisibilityInformations();
        delete workSpace;
}
                

Vector* LocalisationGraph::partitionne(Vector *balisesList,Polygon * workSpace)
{
        Polygon * ws = new Polygon(workSpace);
        Balise* balise;
        VisibilityPolygon * P = new VisibilityPolygon(ws);
        Vector *part, *nextPart;
        int i,j;
        part = new Vector();
        part->addElement(P);
        for (i=0;i<balisesList->size();i++)
        {
                balise = (Balise*)(balisesList->elementAt(i));
                nextPart = new Vector();
                for(j=0;j<part->size();j++)
                {
                        P = (VisibilityPolygon*)(part->elementAt(j));
                        P->split(balise,nextPart);
                }
                part->deleteAll();
                delete part;
                part = nextPart;
                nextPart = NULL;
        }
        return part;                
}

void LocalisationGraph::filterPartition(Vector * partition,Vector * usable)
{
        int i;

        for (i=partition->size()-1;i>=0;i--)
        {
                VisibilityPolygon * MP;
                Polygon * P;
                int m;
                MP = ((VisibilityPolygon*)(partition->elementAt(i)));
                P = MP->getPolygon();
                m = MP->getMark();
                if ((m >= 2)&&(P->getArea()>5))
                {
                        partition->deleteElement(i);
                        usable->addElement(MP);
                }
        }
}


void LocalisationGraph::addLayer(Vector *balisesList,
                double theta,Polygon *workSpace)
{
        Layer *layer=new Layer(theta);
        Vector *partition;
        Vector *usable=new Vector();
        int i;
        partition = partitionne(balisesList,workSpace);
        filterPartition(partition,usable);
        for (i=0;i<usable->size();i++)
                layer->addNode(new Node((VisibilityPolygon*) 
                                        (usable->elementAt(i)),theta));
        graph->addElement(layer);
        // Destruction du vecteur, pas des elements car ils sont
        // dans le graphe
        delete usable;
        // Destruction des polygones non utilises dans le graphe
        partition->deleteAll();
        // Destruction du vecteur cree dans partitionne
        delete partition;
}


void LocalisationGraph::Layer::buildLinks()
{
    int i,j;
    for (i=0;i<nodes->size();i++)
    {
        Node * Pi = (Node*)(nodes->elementAt(i));
        for (j=0;j<i;j++)
        {
            Node * Pj = (Node*)(nodes->elementAt(j));
            Polygon *pi, *pj;
            pi = Pi->poly->getPolygon();
            pj = Pj->poly->getPolygon();
            Polygon * Union = pi->Union(pj);
            if (Union->numPartConnex() == 1)
                Union->cut();
            if (Union->numPartConnex() == 1)
            {
                Pi->connection.addElement(Pj);
                Pj->connection.addElement(Pi);
            }
            delete Union;
        }
    }
}
            

    


    
void LocalisationGraph::computeGraphInterLayer(int indexNew,int indexRef)
{
    int i,j;
    Layer * layerNew = (Layer *)(graph->elementAt(indexNew));
    Layer * layerRef = (Layer *)(graph->elementAt(indexRef));
    for (i=0;i<layerNew->size();i++)
    {
        Node * Pi = layerNew->getNode(i);
        for (j=0;j<layerRef->size();j++)
        {
            Node * Pj = layerRef->getNode(j);
            Polygon *pi, *pj;
            pi = Pi->poly->getPolygon();
            pj = Pj->poly->getPolygon();
            Polygon * inter = pi->Intersection(pj);
            if (!inter->isEmpty())
            {
                    Pi->connection.addElement(Pj);
                    Pj->connection.addElement(Pi);
            }
            delete inter;
        }
    }
}



void LocalisationGraph::buildGraphLinks()
{
    int i;
    for (i=0;i<graph->size();i++)
    {
        printf(".");fflush(stdout);
        ((Layer*)(graph->elementAt(i)))->buildLinks();
    }
    printf("\n");
    
    if (graph->size()>=2)
        computeGraphInterLayer(0,graph->size()-1);

    if (graph->size()>2)
        for (i=0;i<graph->size()-1;i++)
        {
            printf(".");fflush(stdout);
            computeGraphInterLayer(i+1,i);
        }
    printf("\n");
}

void LocalisationGraph::buildSecureArea(Polygon * workSpace,char * filename)
{
    Polygon *U, *I, *tmp; 
    Layer *layer;
    Node *N;
    int i,j;
    I = new Polygon(workSpace);
    for (i=0;i<graph->size();i++)
    {
        layer = (Layer*)(graph->elementAt(i));
        U = new Polygon();
        for (j=0;j<layer->size();j++)
        {
            N = layer->getNode(j);
            tmp = U;
            U = U->Union(N->poly->getPolygon());
            delete tmp;
        }
        tmp = I;
        I = I->Intersection(U);
        delete tmp;
        delete U;
    }
    printf("Zone securisee : %f / %f - %f %%\n",I->getArea(),workSpace->getArea(),I->getArea() / workSpace->getArea());
    FILE * fp = fopen(filename,"w");
    if (fp != NULL)
    {
        I->Print(fp);
        fclose(fp);
    }
    else
        printf("Unable to save secure area in %s\n",filename);
    delete I;
}


int LocalisationGraph::markConnexPart()
{
    int mark = 0;
    int maxId = -1;
    int i,j;
    Node * n;
    Layer * level;
    for (i=0;i<graph->size();i++)
    {
        level = (Layer*)(graph->elementAt(i));
        for (j=0;j<level->size();j++)
        {
            n = level->getNode(j);
            if (n->connId > maxId) maxId=n->connId;
            if (n->connId<0)
            {
                n->exploreAndMark(mark);
                mark += 1;
            }
        }
    }
    // Nombre des parties connexes
    if ((mark == 0) && (maxId>-1))
        // Le graphe etait deja marqué (relecture par ex)
        ConnexParts = maxId + 1; 
    else
        ConnexParts = mark;
    return mark;
}       


double LocalisationGraph::computeVolume()
{
        double Volume = 0;
        int i,j;
        for (i=0;i<graph->size();i++)
        {
                Layer * layer = (Layer*)(graph->elementAt(i));
                for (j=0;j<layer->size();j++)
                {
                        Node * N = layer->getNode(j);
                        Polygon * P = N->poly->getPolygon();
                        Volume += P->getArea()*theta_step;
                }
        }
//      cout << "Volume occupe : " << Volume << " / " << 
//              workSpace.getArea()*2*M_PI << endl;
        return Volume/(workSpace->getArea()*2*M_PI);
}


void LocalisationGraph::Print(FILE * fp)
{
        int i;
        fprintf(fp,"#Layers = %d\n",graph->size());
        fprintf(fp,"Theta step = %f\n",theta_step);
        fprintf(fp,"Connex Parts = %d\n",ConnexParts);
        fprintf(fp,"Work space = ");
        workSpace->Print(fp);
        for (i=0;i<graph->size();i++)
                ((Layer*)(graph->elementAt(i)))->Print(fp);
}


// theta en radian entre 0 et 2Pi
LocalisationGraph::Configuration 
LocalisationGraph::getConfiguration(double x,
                double y,double theta)
{
        double t = atan2(sin(theta),cos(theta));
        if (t<0) t = t + 2* M_PI;
        int layer = (int)(rint(t / theta_step));
        return ((Layer*)(graph->elementAt(layer)))->getConfiguration(x,y);
}

LocalisationGraph::Configuration 
LocalisationGraph::Layer::getConfiguration(double x,double y)
{
        int i;
        for (i=0;i<nodes->size();i++)
        {
                Node * n = (Node*)(nodes->elementAt(i));
                if (n->contains(x,y))
                        return n;
        }
        return NULL;
}

                        


void LocalisationGraph::resetPath()
{
        int i;
        for (i=0;i<graph->size();i++)
                ((Layer*)(graph->elementAt(i)))->resetPath();
}


void LocalisationGraph::Layer::resetPath()
{
        int i;
        for (i=0;i<nodes->size();i++)
                ((Node*)(nodes->elementAt(i)))->resetPath();
}


void LocalisationGraph::Node::resetPath()
{
        score = -1;
        predecessorInPath = NULL;
}



LocalisationGraph::Path 
LocalisationGraph::findPath(Configuration init,Configuration end)
{

        if (init == end)
        {
            Path path = new Vector();
            path->addElement(init);
            return path;
        }
        
        // recherche du plus court chemin : disjkstra
        int i;
        Heap heap;
        Heap::iterator it;

        resetPath();
        heap.insert(HeapElt(0,init));
        init->score = 0;

        while (!heap.empty())
        {
                Node * pit;
                it = heap.begin();
                heap.erase(it);
                pit = ((*it).second);
#ifdef DEBUG
                pit->Print();
#endif
                for (i=0;i<pit->connection.size();i++)
                {
                        Node * p = (Node*)(pit->connection.elementAt(i));
#ifdef DEBUG
                        printf("\tSon : ");
#endif
                        if ((pit->score + 1 < p->score) || (p->score < 0))
                        {
#ifdef DEBUG
                                printf("****** ");
#endif
                                p->score = pit->score + 1;
                                p->predecessorInPath = pit;
                                heap.insert(HeapElt(p->score,p));
                        }
#ifdef DEBUG
                        p->Print();
#endif
                }
        }
        
        if (end->predecessorInPath == NULL)
                return NULL;
            
        // S'il existe un chemin : extraction
        Vector reverse;
        Vector * result = new Vector();
        Node * tmp = end;
        while (tmp != NULL)
        {
                reverse.addElement(tmp);
                tmp = tmp->predecessorInPath;
        }
        for (i=reverse.size()-1;i>=0;i--)
                result->addElement(reverse.elementAt(i));

        return (Path)result;
}

void LocalisationGraph::printPath(FILE *fp,Path path)
{
        int i;
        if (path == NULL)
        {
                fprintf(fp,"Path length : 0\n");
                return;
        }
        fprintf(fp,"Path length : %d\n",path->size());
        for (i=0;i<path->size();i++)
        {
                fprintf(fp,"Step %d : \n",i);
                ((Node*)(path->elementAt(i)))->PrintConfiguration(fp);
        }
        // fprintf(fp,"Path end\n");
}

void LocalisationGraph::cleanPath(Path path)
{
    delete path;
}

void LocalisationGraph::cleanConfiguration(Configuration c)
{
    // delete c;
}


static void printColorTable(FILE * f,const VtkPoints & points)
{
    fprintf(f,"\n\nPOINT_DATA %d\n",points.size());
    fprintf(f,"SCALARS my_scalars float\nLOOKUP_TABLE custom_table\n");
    for (unsigned int i=0;i<points.size();i++)
    {
        fprintf(f,"%f\n",points[i].c);
    }

    fprintf(f,"\nLOOKUP_TABLE custom_table 15\n");
    fprintf(f,"0.2\t0.2\t0.2\t1.0\n");
    fprintf(f,"0.0\t0.0\t0.5\t1.0\n");
    fprintf(f,"0.0\t0.5\t0.5\t1.0\n");
    fprintf(f,"0.0\t0.5\t0.0\t1.0\n");
    fprintf(f,"0.5\t0.5\t0.0\t1.0\n");
    fprintf(f,"0.5\t0.0\t0.0\t1.0\n");
    fprintf(f,"0.5\t0.0\t0.5\t1.0\n");
    fprintf(f,"0.0\t0.0\t1.0\t1.0\n");
    fprintf(f,"0.0\t1.0\t1.0\t1.0\n");
    fprintf(f,"0.0\t1.0\t0.0\t1.0\n");
    fprintf(f,"1.0\t1.0\t0.0\t1.0\n");
    fprintf(f,"1.0\t0.0\t0.0\t1.0\n");
    fprintf(f,"1.0\t0.0\t0.0\t1.0\n");
    fprintf(f,"1.0\t0.0\t1.0\t1.0\n");
    fprintf(f,"0.8\t0.8\t0.8\t1.0\n");
}
    



void LocalisationGraph::VtkPrint(FILE * mainFile,FILE * wsFile,
        double xScale,double yScale,double zScale)
{
    VtkPoints points;
    VtkTriStrips tristrips;
    VtkPolygons polygons;
    unsigned int i,j;
    TriangleStrip * t;
    Layer * l;
    Node * n;
// graph 
    if (getNbConnexParts() <= 0)
        markConnexPart();

    for (i=0;i<(unsigned int)(graph->size());i++)
    {
        l = ((Layer*)(graph->elementAt(i)));
        for (j=0;j<(unsigned int)(l->size());j++)
        {
            n = l->getNode(j);
            t = n->getTriStrip();
            t->collectVtkPoints(points,tristrips,l->theta, n->getId());
        }
    }

    for (i=0;i < points.size();i++)
    {
        VtkPoint3D & P = points[i];
        P.x *= xScale;
        P.y *= yScale;
        P.z *= zScale;
        //double d = P.c;
        P.c = (P.c+1) / (double)(ConnexParts);
        //printf("Avant : %e -- Apres : %e \n",d,P.c);
    }
            
    
    fprintf(mainFile,"# vtk DataFile Version 1.0\n");
    fprintf(mainFile,"Configuration Space\n");
    fprintf(mainFile,"ASCII\nDATASET POLYDATA\n");
    fprintf(mainFile,"POINTS %d float\n",points.size());
    for (i=0;i<points.size();i++)
    {
        fprintf(mainFile,"%f %f %f\n",(points[i].x),
                (points[i].y),(points[i].z));
    }

    fprintf(mainFile,"\nTRIANGLE_STRIPS %d %d",tristrips.size(),
            tristrips.size()+points.size());
    for (i=0;i<tristrips.size();i++)
    {
        fprintf(mainFile,"\n%d ",tristrips[i].size());
        for (j=0;j<tristrips[i].size();j++)
            fprintf(mainFile,"%d ",tristrips[i][j]);
    }

    printColorTable(mainFile,points);

// WorkSpace
    points.erase(points.begin(),points.end());
    workSpace->collectVtkPoints(points,polygons);
    for (i=0;i < points.size();i++)
    {
        VtkPoint3D & P = points[i];
        P.x *= xScale;
        P.y *= yScale;
    }
            
    
    fprintf(wsFile,"# vtk DataFile Version 1.0\n");
    fprintf(wsFile,"workspace\n");
    fprintf(wsFile,"ASCII\nDATASET POLYDATA\n");
    fprintf(wsFile,"POINTS %d float\n",points.size());
    for (i=0;i<points.size();i++)
    {
        fprintf(wsFile,"%f %f %f\n",(points[i].x),
                (points[i].y),(points[i].z));
    }
    fprintf(wsFile,"\nLINES %d %d\n",polygons.size(),
            points.size() + 2 * polygons.size());
    for (i=0;i<polygons.size();i++)
    {
        fprintf(wsFile,"%d ",polygons[i].size()+1);
        for (j=0;j<polygons[i].size();j++)
            fprintf(wsFile,"%d ",polygons[i][j]);
        fprintf(wsFile,"%d\n",polygons[i][0]);
    }
    printColorTable(wsFile,points);

}