/*********************************************
 *
 * Cedric Pradalier
 * DEA 2000/2001 
 * INRIA Rhones Alpes
 * http://cedric.pradalier.free.fr/index.html
 * mail : http://cedric.pradalier.free.fr/mail.html
 *
 * *******************************************/


#include <stdlib.h>
#include <stdio.h>
#include <iostream.h>
#include <math.h>


#include "Polygon.h"
#include "LocalisationGraph.h"
#include "Vector.h"
#include "Balise.h"

#define THETA_MIN 0
#define THETA_MAX 360
#define PRECISION 10


#define D2R(X) ((X)*M_PI/180.0)

double ALPHA=D2R(120);
double PORTEE=150;
unsigned int THETA_STEP=5;
LocalisationGraph::Configuration START;
LocalisationGraph::Configuration END;
LocalisationGraph::Path PATH;
LocalisationGraph * GRAPH;



struct Pos : public Object
{
    double x,y;
    Polygon * visibility;
    Pos() {x=y=0;visibility=NULL;}
    Pos(double X,double Y,Polygon * P){x=X;y=Y;visibility=P;}
    ~Pos(){delete visibility;}
};


Vector * readBalisesPos(FILE * bp,Polygon * workspace)
{
    int i,nb;
    double x,y;
    Vector * res = new Vector();

    nb = 0;
    fscanf(bp," %d ",&nb);
    for (i=0;i<nb;i++)
    {
        fscanf(bp," %le %le ",&x,&y);
        Polygon * visiblity = workspace->computeVisibility(x,y);
        res->addElement(new Pos(x,y,visiblity));
    }
    res->trimToSize();
    return res;
}




Vector * buildBeacons(int theta_deg,double portee,double alpha,
        Vector * balisesPos, Polygon * workSpace)
{
    short int i;

    Vector * destination = new Vector();

    for(i=0;i<balisesPos->size();i++)
    {
        Pos * pos = (Pos *)(balisesPos->elementAt(i));
        Balise * b = new Balise(pos->x,pos->y,portee,alpha,
                D2R(180+theta_deg),PRECISION);
        b->intersectWith(pos->visibility);
        destination->addElement(b);
    }
    return destination;

}


bool InitLocGraph(char * ws_name,char * beacons_name)
{
    int theta;
    Vector * balises = NULL;
    Polygon * workSpace = NULL;
    Vector * balisesPos = NULL;
    FILE *ws, *bp;
    if ((ws=fopen(ws_name,"r"))==NULL)
    {
        printf("Can't open %s\n",ws_name);
        return false;
    }
    if ((bp=fopen(beacons_name,"r"))==NULL)
    {
        printf("Can't open %s\n",beacons_name);
        return false;
    }
    workSpace = new Polygon();
    workSpace->Read(ws);
    fclose(ws);

    balisesPos = readBalisesPos(bp,workSpace);
    fclose(bp);

    GRAPH = new LocalisationGraph(workSpace,D2R(THETA_STEP));

    for (theta=THETA_MIN;theta<=THETA_MAX;theta+=THETA_STEP)
    {
        printf(".");fflush(stdout);
        /***********************************************
         *
         * calcul des couches du graphe de localisation
         *
         * *********************************************/
        balises = buildBeacons(theta,PORTEE,ALPHA,balisesPos,workSpace);
        GRAPH->addLayer(balises,D2R(theta),workSpace);

        /***********************************************
         *
         * Nettoyage
         *
         * *********************************************/
        balises->deleteAll();
        delete balises;
    }

    /***********************************************
     *
     * Construction des arcs du graphe de localisation
     *
     * *********************************************/

    cout << "\nLinking\n";
    GRAPH->buildGraphLinks();
    cout << "Exploration\n";
    GRAPH->markConnexPart();

    GRAPH->buildTriStrips();

    return true;
}


void PrintHelp()
{
    printf("Commands : \n");
    printf("Init <Workspace polygon:%%s> <Beacons list:%%s> <Range:%%e> \n <Opening:%%e> <Theta step:%%d>\n");
   printf("\tInitialize the localization graph\n");
   printf("\tTheta step in degrees (typically 5 degrees).\n");
   printf("Read <Graph file>\n");
   printf("\tRead a graph from disk\n");
   printf("Save <Graph file>\n");
   printf("\tSave a graph to disk\n");
   printf("Start <x> <y> <theta>\n");
   printf("\tDefine initial configuration\n");
   printf("End <x> <y> <theta>\n");
   printf("\tDefine final configuration\n");
   printf("Path\n");
   printf("\tFind a path between initial and final conf.\n");
   printf("Step <index>\n");
   printf("\tDisplay configuration <index> from last path\n");
   printf("Connex\n");
   printf("\tGet number of connex parts in loc. space.\n");
   printf("Volume\n");
   printf("\tCompute volume used by loc. space.\n");
   printf("VtkExport <Graph file> <WorkSpace file>\n");
   printf("\tExport graph for vtk visualization\n");
   printf("Quit\n");
   printf("\tQuit ;-)\n");
   printf("Help\n");
   printf("\tHmmm, I can't remember...\n");
}

typedef enum {Init, Start, End, Quit, Path, Step, VtkExport,
    Volume, Connex, Read, Save, Error, Help} Command;

Command prompt(char * buffer,int size)
{
    char cmd[100];
    printf("> ");fflush(stdout);
    fgets(buffer,size,stdin);
    if (sscanf(buffer," %s ",cmd) != 1)
    {
        printf("ERROR : no command\n");
        return Error;
    }
    if (strcasecmp(cmd,"Init") == 0) return Init;
    if (strcasecmp(cmd,"Read") == 0) return Read;
    if (strcasecmp(cmd,"Save") == 0) return Save;
    if (strcasecmp(cmd,"Start") == 0) return Start;
    if (strcasecmp(cmd,"End") == 0) return End;
    if (strcasecmp(cmd,"Quit") == 0) return Quit;
    if (strcasecmp(cmd,"Path") == 0) return Path;
    if (strcasecmp(cmd,"Step") == 0) return Step;
    if (strcasecmp(cmd,"Help") == 0) return Help;
    if (strcasecmp(cmd,"?") == 0) return Help;
    if (strcasecmp(cmd,"Volume") == 0) return Volume;
    if (strcasecmp(cmd,"VtkExport") == 0) return VtkExport;
    if (strcasecmp(cmd,"Connex") == 0) return Connex;

    printf("ERROR : unrecognised keyword\n");
    return Error;
}

void interprete(Command cmd, char * buffer)
{
    char tcmd[10];
    char fname1[256];
    char fname2[256];
    int res;
    unsigned int index;
    double x,y, theta;
    FILE *fp;
    FILE *main_fp,*ws_fp;
    double xS,yS,zS;

    switch (cmd) {
        case Init : 
            {
                GRAPH->cleanConfiguration(START);
                GRAPH->cleanConfiguration(END);
                GRAPH->cleanPath(PATH);PATH=NULL;
                delete GRAPH;
                GRAPH = NULL;START = END = NULL;
                res = sscanf(buffer," %s %s %s %le %le %d ",
                        tcmd,fname1,fname2,
                        &PORTEE,&ALPHA,&THETA_STEP);
                if (res != 6) 
                {
                    printf("ERROR : not enough arguments\n");
                    return;
                }
                if (!InitLocGraph(fname1,fname2)) 
                {
                    printf("ERROR : error during initialization\n");
                    return;
                }
                    
                break;
            }
        case VtkExport : 
            {
                if (GRAPH == NULL) 
                {
                    printf("ERROR : Graph not ready\n");
                    return;
                }
                xS = yS = zS = 1;
                res = sscanf(buffer," %s %s %s %le %le %le ",
                        tcmd,fname1,fname2, &xS,&yS,&zS);
                if (res != 6) 
                {
                    printf("ERROR : not enough arguments\n");
                    return;
                }
                
                main_fp = fopen(fname1,"w");
                if (main_fp==NULL)
                {
                    printf("ERROR : can't open %s for writing\n", fname1);
                    return;
                }
                
                ws_fp = fopen(fname2,"w");
                if (ws_fp==NULL)
                {
                    printf("ERROR : can't open %s for writing\n", fname2);
                    return;
                }
                GRAPH->VtkPrint(main_fp,ws_fp,xS,yS,zS);
                fclose(main_fp);
                fclose(ws_fp);
                    
                break;
            }
        case Read :
            {
                GRAPH->cleanConfiguration(START);
                GRAPH->cleanConfiguration(END);
                GRAPH->cleanPath(PATH);PATH=NULL;
                delete GRAPH;
                GRAPH = NULL;START = END = NULL;
                res = sscanf(buffer," %s %s ",
                        tcmd,fname1);
                if (res != 2) 
                {
                    printf("ERROR : not enough arguments\n");
                    return;
                }
                if ((fp=fopen(fname1,"r")) == NULL)
                {
                    printf("ERROR : can't open %s\n",
                            fname1);
                    return;
                }
                GRAPH = new LocalisationGraph(fp);
                fclose(fp);
                GRAPH->buildTriStrips();
                GRAPH->markConnexPart();
                break;
            }
        case Save :
            {
                if (GRAPH == NULL) 
                {
                    printf("ERROR : Graph not ready\n");
                    return;
                }
                res = sscanf(buffer," %s %s ",
                        tcmd,fname1);
                if (res != 2) 
                {
                    printf("ERROR : not enough arguments\n");
                    return;
                }
                if ((fp=fopen(fname1,"w")) == NULL)
                {
                    printf("ERROR : can't open %s for writing\n",
                            fname1);
                    return;
                }
                GRAPH->Print(fp);
                fclose(fp);
                break;
            }
        case Volume :
            {
                if (GRAPH == NULL) 
                {
                    printf("ERROR : Graph not ready\n");
                    return;
                }
                printf("Used space : %3.1f%%\n",GRAPH->computeVolume()*100.0);
                break;
            }
        case Connex :
            {
                if (GRAPH == NULL) 
                {
                    printf("ERROR : Graph not ready\n");
                    return;
                }
                printf("Connex parts : %d\n",GRAPH->getNbConnexParts());
                break;
            }
        case Start :
            {
                if (GRAPH == NULL) 
                {
                    printf("ERROR : Graph not ready\n");
                    return;
                }
                delete START; START = NULL;
                GRAPH->cleanPath(PATH);PATH=NULL;
                res = sscanf(buffer," %s %le %le %le ",
                        tcmd,&x,&y,&theta);
                if (res != 4) 
                {
                    printf("ERROR : not enough arguments\n");
                    return;
                }
                START = GRAPH->getConfiguration(x,y,theta);
                if (START == NULL) 
                {
                    printf("ERROR : Start conf. is not in the graph\n");
                    return;
                }
                break;
            }
        case End :
            {
                if (GRAPH == NULL) 
                {
                    printf("ERROR : Graph not ready\n");
                    return;
                }
                delete END; END = NULL;
                GRAPH->cleanPath(PATH);PATH=NULL;
                res = sscanf(buffer," %s %le %le %le ",
                        tcmd,&x,&y,&theta);
                if (res != 4) 
                {
                    printf("ERROR : not enough arguments\n");
                    return;
                }
                END = GRAPH->getConfiguration(x,y,theta);
                if (END == NULL) 
                {
                    printf("ERROR : End conf. is not in the graph\n");
                    return;
                }
                break;
            }
        case Path : 
            {
                if (GRAPH == NULL) 
                {
                    printf("ERROR : Graph not ready\n");
                    return;
                }
                if ((START == NULL) || (END == NULL))
                {
                    printf("ERROR : Start and end points must be defined\n");
                    return;
                }
                GRAPH->cleanPath(PATH);PATH=NULL;
                GRAPH->resetPath();
                PATH = GRAPH->findPath(START,END);
                if (PATH == NULL)
                {
                    printf("ERROR : no possible path between start and end\n");
                    return;
                }   
                printf("Path length : %d\n", GRAPH->getPathSize(PATH));
                break;
            }
        case Step :
            {
                unsigned int size;
                if (PATH == NULL) 
                {
                    printf("ERROR : No path previously found\n");
                    return;
                }
                res = sscanf(buffer," %s %d ",tcmd,&index);
                if (res != 2) 
                {
                    printf("ERROR : not enough arguments\n");
                    return;
                }
                size = GRAPH->getPathSize(PATH);
                if (index >= size)
                {
                    printf("ERROR : invalid index\n");
                    return;
                }
                GRAPH->printConfiguration(GRAPH->getStep(PATH,index),stdout);
                break;
            }
        case Quit :
            {
                GRAPH->cleanConfiguration(START);
                GRAPH->cleanConfiguration(END);
                GRAPH->cleanPath(PATH);
                delete GRAPH;
                GRAPH = NULL;START = END = NULL;PATH=NULL;
                break;
            }

        case Help : 
            {
                PrintHelp();
                return;
            }

        default : return;
    }

    printf("OK\n");
}









int main(int argc,char * argv[])
{
    GRAPH = NULL;
    START = END = NULL;
    PATH = NULL;
    char buffer[512];
    Command cmd;

    while (1)
    {
        cmd = prompt(buffer,512);
        interprete(cmd,buffer);
        if (cmd == Quit) return 0;
    }

    return 0;
}