#include <stdio.h> 
#include <stdlib.h> 
#include <math.h> 
#include "X-graph.h" 
#include <mpi.h> 
#define TRUE 1
#define FALSE 0
typedef struct path {
    // number of places
    int n;
    // position of places
    double * x;
    double * y;
    // ordering of places
    int * o;
    int * o_best;
    double pathlength_best;
    // workspace
    int * stack;
    // simulated annealing
    int anneal;
    double temperature;
    double cooling_factor;
} Path;

MPI_Status status;
int rank;
int size;


void random_initByTime(int rank) {
    time_t ltime;

    time(<ime);
    srand((unsigned) ltime + 100*rank);
    //srand(rank);
}


void alloc_path(Path * thePath,int n) {
    thePath->n=n;
    thePath->x = (double *)malloc(sizeof(double)*n);
    thePath->y = (double *)malloc(sizeof(double)*n);
    thePath->o = (int *)malloc(sizeof(int)*n);
    thePath->o_best = (int *)malloc(sizeof(int)*n);
    thePath->stack = (int *)malloc(sizeof(int)*n);
    thePath->anneal=TRUE;
    thePath->temperature=1;
    thePath->cooling_factor=0.999;
}


void free_path(Path * thePath) {
    free(thePath->x);
    free(thePath->y);
    free(thePath->o);
    free(thePath->o_best);
    free(thePath->stack);
}


double path_length(Path * thePath, int * o) {
    int i;
    double length;
    int num_steps = thePath->n-1;
    length=0.0;
    for(i=0;i<num_steps;i++) {
        length+=sqrt(pow((thePath->x[o[i]]-
            thePath->x[o[i+1]]),2)+
            pow((thePath->y[o[i]]-
            thePath->y[o[i+1]]),2));
    }

    return length;
}


void randomize_order(Path * thePath,int first_city,int last_city) {
    // pick a random starting point,
    // then go through remaining options until all are selected.

    // easiest choice, create a list, and pick randomly from list,
    // modifying list as one goes.
    int i,j;
    int pick;
    int nstack;

    nstack = last_city-first_city+1;
    for (i=0;i<nstack;i++) {
        thePath->stack[i]=thePath->o[first_city+i];
    }

    for(i=first_city;i<last_city;i++) {
        //choose random city from stack;
        pick = (int)((double)nstack*(double)rand()/(double)RAND_MAX);
        thePath->o[i]=thePath->stack[pick];
        //remove city from stack and move everyone else up
        nstack--;
        for(j=pick;j<nstack;j++) {
            thePath->stack[j]=thePath->stack[j+1];
        }
    }
}


void reverse_order(Path * thePath,int first_city,int last_city) {
    // pick a random starting point,
    // then go through remaining options until all are selected.

    // easiest choice, create a list, and pick randomly from list,
    // modifying list as one goes.
    int i,j;
    int pick;
    int nstack;

    nstack = last_city-first_city+1;
    for (i=0;i<nstack;i++) {
        thePath->stack[nstack-i-1]=thePath->o[first_city+i];
    }

    for(i=first_city;i<last_city;i++) {
        thePath->o[i]=thePath->stack[i-first_city];
    }
}


void random_swap(Path * thePath) {
    // pick two cities at random and swap them.

    int first_city;
    int second_city;
    int temp_o;

    first_city=1+
        (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    second_city=1+
        (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    while(second_city==first_city) {
        second_city=1+
            (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    }

    temp_o = thePath->o[first_city];
    thePath->o[first_city]=thePath->o[second_city];
    thePath->o[second_city]=temp_o;
}
}


void random_swap2(Path * thePath) {
    // pick two cities at random and swap them.
    // the second city is after the first

    int first_city;
    int second_city;
    int temp_o;

    first_city=1+
        (int)((double)(thePath->n-3)*(double)rand()/(double)RAND_MAX);
    second_city=first_city+1;

    temp_o = thePath->o[first_city];
    thePath->o[first_city]=thePath->o[second_city];
    thePath->o[second_city]=temp_o;
}


void random_swap3(Path * thePath) {
    // pick one cities at random and move it to a random spot.

    int start;
    int end;
    int i;
    int temp_o;

    start=1+
        (int)((double)(thePath->n-3)*(double)rand()/(double)RAND_MAX);
    end=1+
        (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    while(end==start) {
        end=1+
            (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    }

    if(start<end) {
        //temp_o = thePath->o[start];
        //for(i=start;i<end;i++) {
            //thePath->o[i]=thePath->o[i+1];
        //}
        //thePath->o[end]=temp_o;
    } else {
        temp_o = thePath->o[start];
        for(i=start;i>end;i--) {
            thePath->o[i]=thePath->o[i-1];
        }
        thePath->o[end]=temp_o;
    }

}



void random_swap4(Path * thePath) {
    // pick two cities and reverse everything between them
    int nstack,i,j,pick;
    int first_city;
    int second_city;
    int temp;

    first_city=1+
        (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    second_city=1+
        (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    while(second_city==first_city) {
        second_city=1+
            (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    }
    if(first_city>second_city) {
        temp=first_city;
        second_city=first_city;
        first_city=temp;
    }

    reverse_order(thePath,first_city,second_city); 
}


void random_swap5(Path * thePath) {
    // pick two cities at random, the second
    // greater than the first, but cannot include
    // the first or last city, randomize between them

    int nstack,i,j,pick;
    int first_city;
    int second_city;
    int temp;

    first_city=1+
        (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    second_city=1+
        (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    while(second_city==first_city) {
        second_city=1+
            (int)((double)(thePath->n-2)*(double)rand()/(double)RAND_MAX);
    }
    if(first_city>second_city) {
        temp=first_city;
        second_city=first_city;
        first_city=temp;
    }

    randomize_order(thePath,first_city,second_city);
}


void random_swap6(Path * thePath) {
    //swap the endpoints

    int temp_o;
    temp_o = thePath->o[0];
    thePath->o[0]=thePath->o[thePath->n-1];
    thePath->o[thePath->n-1]=temp_o;
}



void regular_path(Path * thePath) {
    int i;
    double theta;
    double rad;
    double step;

    rad=5.0;
    step=6.28/(double)(thePath->n);
    
    for(i=0;i<thePath->n;i++) {
        theta=i*step;
        thePath->x[i] = rad*cos(theta);
        thePath->y[i] = rad*sin(theta);
    }
    // randomize order and reset best path
    for(i=0;i<thePath->n;i++) {
        thePath->o[i]=i;
    }
    randomize_order(thePath,1,thePath->n-2);
    for(i=0;i<thePath->n;i++) {
        thePath->o_best[i]=thePath->o[i];
    }
    thePath->pathlength_best=path_length(thePath,thePath->o_best);
}


void randomize_path(Path * thePath,int neighbors) {
    int i;
    double plusminus;
    for(i=0;i<thePath->n;i++) {
        plusminus = (double)rand()/(double)RAND_MAX;
        thePath->x[i] = 4.0*((double)rand()/(double)RAND_MAX-0.5);
        if (neighbors>1) {
            if(plusminus<0.5) {
                thePath->x[i]+=5.0;
            } else {
                thePath->x[i]-=5.0;
            }
        }
        plusminus = (double)rand()/(double)RAND_MAX;
        thePath->y[i] = 4.0*((double)rand()/(double)RAND_MAX-0.5);
        if(neighbors>2) {
            if(plusminus<0.5) {
                thePath->y[i]+=5.0;
            } else {
                thePath->y[i]-=5.0;
            }
        }
    }
    // randomize order and reset best path
    for (i=0;i<thePath->n;i++) {
        thePath->o[i]=i;
        thePath->o_best[i]=thePath->o[i];
    }
    thePath->pathlength_best=path_length(thePath,thePath->o);
}


int check_length(Path * thePath) {
    double length,diff,test;
    int i;
    length = path_length(thePath,thePath->o);
    thePath->pathlength_best = path_length(thePath,thePath->o_best);
    if (length<thePath->pathlength_best) {
        thePath->pathlength_best=length;
        for(i=0;i<thePath->n;i++) {
            thePath->o_best[i]=thePath->o[i];
        }
        if(thePath->anneal) thePath->temperature *= thePath->cooling_factor;
        return TRUE;
    } else {
        if(thePath->anneal) {
            diff = fabs(length-thePath->pathlength_best);
            test = (double)rand()/(double)RAND_MAX;
            if(test<exp(-diff/thePath->temperature)) {
                thePath->pathlength_best=length;
                for(i=0;i<thePath->n;i++) {
                    thePath->o_best[i]=thePath->o[i];
                }
                thePath->temperature *= thePath->cooling_factor;
                return TRUE;
            } else {
                for(i=0;i<thePath->n;i++) {
                    thePath->o[i]=thePath->o_best[i];
                }
                thePath->temperature *= thePath->cooling_factor;
                return FALSE;
            }
        } else {
            for(i=0;i<thePath->n;i++) {
                thePath->o[i]=thePath->o_best[i];
            }
            return FALSE;
        }
    }
}


void draw(xgraph * theGraph, Path * thePath,int is_random) {
    xgraphPre(theGraph);
    int i;

    XSetForeground(theGraph->dpy,theGraph->gc,theGraph->whiteColor);
    for(i=0;i<thePath->n-1;i++) {
        if((thePath->o_best[i+1]==thePath->o_best[i]+1 ||
            thePath->o_best[i+1]==thePath->o_best[i]-1) && !is_random) {
            XSetForeground(
                theGraph->dpy,theGraph->gc,theGraph->greenPixel->pixel);
        }
        XDrawLine(theGraph->dpy,theGraph->buffer,theGraph->gc,
            xgraphXReal2Disp(theGraph,thePath->x[thePath->o_best[i]]),
            xgraphYReal2Disp(theGraph,thePath->y[thePath->o_best[i]]),
            xgraphXReal2Disp(theGraph,thePath->x[thePath->o_best[i+1]]),
            xgraphYReal2Disp(theGraph,thePath->y[thePath->o_best[i+1]]));
        if((thePath->o_best[i+1]==thePath->o_best[i]+1 ||
            thePath->o_best[i+1]==thePath->o_best[i]-1) && !is_random) {
            XSetForeground(theGraph->dpy,theGraph->gc,theGraph->whiteColor);
        }
    }
    XSetForeground(theGraph->dpy,theGraph->gc,theGraph->redPixel->pixel);
    XFillRectangle(theGraph->dpy,theGraph->buffer,theGraph->gc,
        xgraphXReal2Disp(theGraph,thePath->x[0])-4,
        xgraphYReal2Disp(theGraph,thePath->y[0])-4,
        8,8);
    for(i=1;i<thePath->n-1;i++) {
        XFillRectangle(theGraph->dpy,theGraph->buffer,theGraph->gc,
            xgraphXReal2Disp(theGraph,thePath->x[i])-2,
            xgraphYReal2Disp(theGraph,thePath->y[i])-2,
            4,4);
    }
    XFillRectangle(theGraph->dpy,theGraph->buffer,theGraph->gc,
        xgraphXReal2Disp(theGraph,thePath->x[thePath->n-1])-4,
        xgraphYReal2Disp(theGraph,thePath->y[thePath->n-1])-4,
        8,8);

    xgraphPost(theGraph);
}


int check_regular(Path * thePath,int * o) {
    int i;
    int in_order=1;
    for(i=0;i<thePath->n-1&&in_order;i++) {
        if(thePath->o_best[i+1]==thePath->o_best[i]+1 ||
            thePath->o_best[i+1]==thePath->o_best[i]-1) {
        } else {
            in_order=0;
        }
    }
    return in_order;
}

int main(int argc, char ** argv) {
    Path * thePath;
    int n=500;
    int i,j,k,l;
    int * ibuffer;
    double dbuffer;
    double * xybuffer;
    int itmax_outer=100000;
    int itmax_inner=10;
    double length;
    int random_type;
    int anneal_flag;
    double diff;
    double test;
    int is_random;
    int good_rank;
    int good_rank_temp;
    int good_rank_temp2;
    int neighbors=1;
    int do_display=1;
    int draw_counter=0;
    int draw_repeat=100;
    int do_print=0;
    int junk;
    xgraph * theGraph;

    MPI_Init(&argc,&argv);
    MPI_Comm_rank(MPI_COMM_WORLD,&rank);
    MPI_Comm_size(MPI_COMM_WORLD,&size);

    if(rank==0) {
        printf("Usage: travel n_cities n_neighbors i_outer i_inner temp cooling do_display do_print\n");
    }

    if(argc>1) {
        sscanf(argv[1],"%d",&n);
    }
    if(argc>2) {
        sscanf(argv[2],"%d",&neighbors);
    }
    if(neighbors!=-1&&neighbors!=1&&neighbors!=2&&neighbors!=4) {
        neighbors=1;
        printf("Problem with # neighborhoods, set to one\n");
    }
    is_random=1;
    if(neighbors==-1) {
        is_random=0;
    }
    if(argc>3) {
        sscanf(argv[3],"%d",&itmax_outer);
    }
    if(argc>4) {
        sscanf(argv[4],"%d",&itmax_inner);
    }

    //seed random number generator

    //allocate memory
    thePath = (Path *)malloc(sizeof(Path));
    alloc_path(thePath,n);
    ibuffer = (int *)malloc(sizeof(int)*n);
    xybuffer = (double *)malloc(sizeof(double)*n*2);

    if(argc>5) {
        sscanf(argv[5],"%lf",&thePath->temperature);
    }
    if(argc>6) {
        sscanf(argv[6],"%lf",&thePath->cooling_factor);
    }
    if(argc>7) {
        sscanf(argv[7],"%d",&do_display);
    }
    if(argc>8) {
        sscanf(argv[8],"%d",&do_print);
    }

    theGraph = (xgraph*)malloc(sizeof(xgraph));
    if (rank==0&&do_display) xgraphSetup(theGraph,600,600);

    srand(0);
    // set up initial path
    if (rank==0) {
        if(is_random) {
            randomize_path(thePath,neighbors);
        } else {
            regular_path(thePath);
        }
        for(j=0;j<thePath->n;j++) {
            xybuffer[j]=thePath->x[j];
            xybuffer[j+thePath->n]=thePath->y[j];
            ibuffer[j]=thePath->o[j];
        }
    }
    MPI_Bcast(xybuffer,2*thePath->n,MPI_DOUBLE,0,MPI_COMM_WORLD);
    MPI_Bcast(ibuffer,thePath->n,MPI_DOUBLE,0,MPI_COMM_WORLD);
    if (rank!=0) {
        for(j=0;j<thePath->n;j++) {
            thePath->x[j]=xybuffer[j];
            thePath->y[j]=xybuffer[j+thePath->n];
            thePath->o[j]=ibuffer[j];
        }
    }
    for(j=0;j<thePath->n;j++) {
        thePath->o_best[j]=thePath->o[j];
    }
    thePath->pathlength_best=path_length(thePath,thePath->o_best);
    random_initByTime(rank);

    xgraphSetXRange(theGraph,-10.0,10.0);
    xgraphSetYRange(theGraph,-10.0,10.0);

    // test a number of different orderings
    good_rank=0;
    for(i=0;i<itmax_outer;i++){
        for(j=0;j<itmax_inner;j++) {
            if(!thePath->anneal) {
                randomize_order(thePath,1,thePath->n-2);
            } else {
                random_type = (int)(15.0*(double)rand()/(double)RAND_MAX);
                switch(random_type) {
                    case 14:
                        random_swap6(thePath);
    //swap the endpoints
                     break;
                    case 13:
                        random_swap5(thePath);
    // pick two cities at random, the second
    // greater than the first, but cannot include
    // the first or last city, randomize between them
                     break;
                    case 12:
                    case 11:
                    case 10:
                        random_swap4(thePath);
    // pick two cities and reverse everything between them
                     break;
                    case 8:
                    case 7:
                    case 6:
                        random_swap3(thePath);
    // pick one cities at random and move it to a random spot.
                     break;
                    case 5:
                    case 4:
                    case 3:
                        random_swap2(thePath);
    // pick two cities at random and swap them.
    // the second city is after the first
                     break;
                    case 2:
                    case 1:
                    case 0:
                    default:
                        random_swap(thePath);
    // pick two cities at random and swap them.
                     break;
                }
            }
            check_length(thePath);
            length = thePath->pathlength_best;
        }
        check_length(thePath);
        if(rank!=0) {
            for(j=0;j<thePath->n;j++) ibuffer[j]=thePath->o_best[j];
            dbuffer = path_length(thePath,thePath->o_best);
            MPI_Send(&dbuffer,1,MPI_DOUBLE,0,100,MPI_COMM_WORLD);
            MPI_Send(ibuffer,thePath->n,MPI_INT,0,200,MPI_COMM_WORLD);
        } else {
            good_rank_temp=good_rank;
            good_rank_temp2=0;
            for(k=1;k<size;k++) {
                MPI_Recv(&dbuffer,1,MPI_DOUBLE,k,100,MPI_COMM_WORLD,&status);
                MPI_Recv(ibuffer,thePath->n,MPI_INT,k,
                    200,MPI_COMM_WORLD,&status);
                diff = fabs(length-thePath->pathlength_best);
                test = (double)(size+1)/(double)size*
                    (double)rand()/(double)RAND_MAX;
                anneal_flag=0;
                if(test<exp(-diff/thePath->temperature)&&thePath->anneal) {
                    anneal_flag=1;
                }
                if(dbuffer<thePath->pathlength_best||anneal_flag) {
                    for(j=0;j<thePath->n;j++) {
                        thePath->o_best[j]=ibuffer[j];
                        thePath->o[j]=ibuffer[j];
                    }
                    thePath->pathlength_best=path_length(thePath,
                        thePath->o_best);
                    thePath->pathlength_best=dbuffer;
                    good_rank_temp2=1;
                    good_rank_temp=k;
                } else {
                }
            }
            if(good_rank_temp2==0) {good_rank_temp=0;}
            if(good_rank_temp!=good_rank) {
                //printf("SWITCH!!!! %d %d\n",good_rank,good_rank_temp);
                good_rank=good_rank_temp;
            }
        }
        if(rank==0) {
            for(j=0;j<thePath-%gt;n;j++) ibuffer[j]=thePath-%gt;o_best[j];
            dbuffer = thePath-%gt;pathlength_best;
        }
        MPI_Bcast(&dbuffer,1,MPI_DOUBLE,0,MPI_COMM_WORLD);
        MPI_Bcast(ibuffer,thePath-%gt;n,MPI_INT,0,MPI_COMM_WORLD);
        if(rank!=0) {
            for(j=0;j<thePath-%gt;n;j++) thePath-%gt;o_best[j]=ibuffer[j];
            for(j=0;j<thePath-%gt;n;j++) thePath-%gt;o[j]=ibuffer[j];
            thePath-%gt;pathlength_best=path_length(thePath,ibuffer);
        }
        if(!is_random) {
            if(check_regular(thePath,thePath-%gt;o_best)){
                printf("EXACT PATH FOUND\n");
                free_path(thePath);
                free(thePath);
                free(theGraph);
                free(ibuffer);
                free(xybuffer);
                MPI_Finalize();
                exit(0);
            }
        }
        if(rank==0&&do_display&&draw_counter++%draw_repeat==0) draw(theGraph,thePath,is_random);
        if(rank==0&&do_print) {
            printf("ITER: %d LENGTH: %lf TEMP: %lf \n",i,length,thePath-%gt;temperature);
        }
    }
    //printf("LENGTH: %d %lf %lf \n",i,length,thePath-%gt;temperature);

    // free alocated memory
    free_path(thePath);
    free(thePath);
    free(theGraph);
    free(ibuffer);
    free(xybuffer);

    MPI_Finalize();