// ------------------------------------------------------------------------- //
// --- This example is a 3-stage pipeline version of                     --- //
// --- the multiple-baseline stereo application                          --- //
// --- Author: Anne Benoit                                               --- //
// --- Last modification: 16/11/2004                                     --- //
// ------------------------------------------------------------------------- //
#include <sys/times.h>
#include "../eSkel.c"
#include "stereo.h"

#define DEBUG  0     // set to 1 for debugging mode, 0 for normal mode
#define WINX   13    // window rows
#define WINY   13    // window columns

// For eSkel
#define STAGES 3

// For display
#define CASEDEAL 1
#define CASE3ST  2
#define CASESEQ  3
#define CASEMPI  4

double  dsecsG=0, esecsG=0;
double st3secs[ITERS];  // times between completions at stage 3, 
                        // st3secs[0] must be ignored, leaving
                        // ITERS-1 interesting readings

double truethrput;

// ------------------------------------------------------------------------- //
// gendiffimg forms the difference image
void gendiffimg(int ref[(ROWS+WINY)*COLS], int m1[(ROWS+WINY)*COLS], int m2[(ROWS+WINY)*COLS],
		float diffimg[(ROWS+WINY)*COLS], int curdisp) {
  int  i,j;

  if (DEBUG) printf("Entering gendiffimg, curdisp=%d \n",curdisp);
  
  for (i=0;i<ROWS+WINY;i++)
    for (j=0;j<COLS-2*curdisp;j++)
      diffimg[i*COLS+j] = (ref[i*COLS+j] - m1[i*COLS+j+curdisp])*(ref[i*COLS+j] - m1[i*COLS+j+curdisp]) +
	              (ref[i*COLS+j] - m2[i*COLS+j+2*curdisp])*(ref[i*COLS+j] - m2[i*COLS+j+2*curdisp]);
}

// ------------------------------------------------------------------------- //
// updatedispimg sums the difference image over a moving window and updates
// the current best error and best disparity images.
void updatedispimg(float diffimg[(ROWS+WINY)*COLS], float curbesterr[ROWS*COLS], 
		   int curbestdisp[ROWS*COLS], int curdisp) {
  int   i,j;
  float sum,csum[COLS];

  for (j=0;j<COLS;j++) {
    csum[j] = 0.0;
    for (i=0;i<WINX;i++)
      csum[j] = csum[j] + diffimg[i*COLS+j];
  }

  for (i=0;i<ROWS;i++) {
    sum = 0.0;
    for (j=0;j<7;j++)
      sum = sum + csum[j];
    
    if (sum < curbesterr[i*COLS+0]) {
      curbesterr[i*COLS+0]  = sum;
      curbestdisp[i*COLS+0] = curdisp;
    }

    for (j=1;j<7;j++) {
      sum = sum + csum[j+6];
      if (sum < curbesterr[i*COLS+j]) {
	curbesterr[i*COLS+j]  = sum;
	curbestdisp[i*COLS+j] = curdisp;
      }
    }

    for (j=7;j<COLS-6;j++) {
      sum = sum - csum[j-7];
      sum = sum + csum[j+6];
      if (sum < curbesterr[i*COLS+j]) {
	curbesterr[i*COLS+j]  = sum;
	curbestdisp[i*COLS+j] = curdisp;
      }
    }

    for (j=COLS-6;j<COLS;j++) {
      sum = sum - csum[j-7];
      if (sum < curbesterr[i*COLS+j]) {
	curbesterr[i*COLS+j]  = sum;
	curbestdisp[i*COLS+j] = curdisp;
      }
    }
    
    for (j=0;j<COLS;j++)
      csum[j] = csum[j] - diffimg[i*COLS+j] + diffimg[(i+WINY)*COLS+j];

  }
  

}

// ------------------------------------------------------------------------- //
// error displays an error message raised in testdata
void error(int i, int j, int tardis, int disp) {
  
  printf("*** ERROR in testdata at position (%d,%d). Expected disp %d but got %d *** \n",
	 i,j,tardis,disp);
  
}

// ------------------------------------------------------------------------- //
// testdata is testing output (matched with getdata)
void testdata(int disp[ROWS*COLS]) {

  int i,j,ipos,jpos;
  int tardis,isize,jsize;

  //if (DEBUG) printf("Entering testdata\n");
  
  tardis = 12;
  isize=37;
  jsize=37;

  ipos = (ROWS-isize)/2 - 1 ;
  jpos = (COLS-jsize)/2 - tardis - 1;


  for (i=ipos+10;i<=ipos+isize-11;i++)
    for (j=jpos+10;j<=jpos+jsize-11;j++)
      if (disp[i*COLS+j]!=tardis) 
	error(i,j,tardis,disp[i*COLS+j]);
  
  for (i=0;i<=ipos-10;i++)
    for (j=0;j<=jpos+jsize+2*tardis+9;j++)
      if (disp[i*COLS+j]!=0) 
	error(i,j,0,disp[i*COLS+j]);
  
  for (i=0;i<=ipos+isize+9;i++)
    for (j=jpos+jsize+2*tardis+9;j<COLS;j++)
      if (disp[i*COLS+j]!=0) 
	error(i,j,0,disp[i*COLS+j]);
  
  for (i=ipos+isize+9;i<ROWS;i++)
    for (j=jpos-10;j<COLS;j++)
      if (disp[i*COLS+j]!=0) 
	error(i,j,0,disp[i*COLS+j]);

  for (i=ipos-10;i<ROWS;i++)
    for (j=0;j<=jpos-10;j++)
      if (disp[i*COLS+j]!=0) 
	error(i,j,0,disp[i*COLS+j]);

}




// ------------------------------------------------------------------------- //
void printperf(int runtype, int np, double dsecs, double esecs, int iter, double truethrput, double rawtime) {
  double secs;
  double dpercent, epercent;

  if (DEBUG)
    printf("Entering printperf, %f %f %d \n",dsecs,esecs,iter);

  dsecs = dsecs/iter;
  esecs = esecs/iter;
  secs  = dsecs+esecs;

  //printf("%f %f %f \n",dsecs,esecs,secs);
  
  dpercent = dsecs*100/secs;
  epercent = esecs*100/secs;

  //printf("%f %f \n",dpercent,epercent);

  printf("***********************************************\n");
  printf("* Multiple-Baseline Stereo (%3d x %3d)        *\n", ROWS, COLS);
  printf("***********************************************\n");
  printf("* Time for one image set:                     *\n");
  printf("* diff     : %9.6f (%2.0f percent)           *\n",dsecs,dpercent);
  printf("* error    : %9.6f (%2.0f percent)           *\n",esecs,epercent);
  printf("* total(s) : %9.6f                        *\n"    ,secs);
  printf("***********************************************\n\n");
  //printf("* Throughput :   %6.2f frames/sec            *\n",1.0/(dsecs+esecs));
  //printf("***********************************************\n");

  switch (runtype) {
    case CASEDEAL:
      printf("Deal%d,",np-2);
      break;
    case CASE3ST:
      printf("3ST,");
      break;
    case CASESEQ:
      printf("Seq,");
      break;
    case CASEMPI:
      printf("MPI,");
      break;
  }
  printf(" %d procs,", np);
  printf(" throughput was %4.2f frames/sec, run-time on P%d was %4.2f secs, %d images.\n",truethrput, np-1, rawtime, ITERS);
}


// The first stage: data generation
void stage1 (void) { 
  int    count; 
  int   *len;
  void **data;

  // The three arrays to transmit
  int *ref, *m1, *m2;
  
  int size;
  int i,j,ipos,jpos;
  int tardis,isize,jsize;

  // We transmit a data of type MPI_int_array, of length 3
  // (one item per array to transmit)
  len = (int *) malloc (sizeof (int));
  len[0] = 3;
  
  MPI_Type_size(MPI_int_array,&size);
  data = (void **) malloc (sizeof(void *));
  *data = (void *) malloc (size * len[0]);
  
  ref           =  (int *) &((int *)(data[0]))[0];
  m1            =  (int *) &((int *)(data[0]))[(ROWS+WINY)*COLS];
  m2            =  (int *) &((int *)(data[0]))[2*(ROWS+WINY)*COLS];
    
  // Filling the arrays
  tardis = 12;
  isize=37;
  jsize=37;
  
  for (count=0;count<ITERS;count++) {

    for (i=0;i<6;i++)
      for (j=0;j<COLS;j++) {
	ref[i*COLS+j]=0;
	m1[i*COLS+j] =0;
	m2[i*COLS+j] =0;
      }
    
    for (i=6;i<ROWS+6;i++)
      for (j=0;j<COLS;j++) {
	ref[i*COLS+j]=i+j;
	m1[i*COLS+j] =i+j;
	m2[i*COLS+j] =i+j;
      }
    
    for (i=ROWS+6;i<ROWS+13;i++)
      for (j=0;j<COLS;j++) {
	ref[i*COLS+j]=0;
	m1[i*COLS+j] =0;
	m2[i*COLS+j] =0;
      }
    
    ipos = (ROWS-isize)/2 + 6 - 1;
    jpos = (COLS-jsize)/2 - tardis - 1;
    
    for (i=ipos;i<ipos+isize;i++)
      for (j=jpos;j<jpos+jsize;j++) {
	ref[i*COLS+j]         = i-ipos + j-jpos;
	m1[i*COLS+j+tardis]   = i-ipos + j-jpos;
	m2[i*COLS+j+2*tardis] = i-ipos + j-jpos;
      }
    
    Give (data, len);
  }  
  return;
}


// treatment of one set of images
// should be rewritten with deal and nested pipeline
void stage2 (void) {
  int i,j; 
  int *len;
  int **len_ptr;
  void **data, **dataout;
  int *ref, *m1, *m2;
  float curbesterr[ROWS*COLS];
  int curbestdisp[ROWS*COLS];

  float diffimg[(ROWS+WINY)*COLS]; // intermediate image

  int curdisp,size;
  double  secs;
  
  MPI_Type_size(MPI_int_array,&size);

  len = (int *) malloc (sizeof (int));
  len_ptr = (int **) malloc (sizeof (int *));

  dataout = (void **) malloc (sizeof(void *));
  *dataout = (void *) malloc (size);

  while (data = Take(len_ptr)) {
    len = *len_ptr;
    ref           =  &((int *) data[0])[0];
    m1            =  &((int *) data[0])[(ROWS+WINY)*COLS];
    m2            =  &((int *) data[0])[2*(ROWS+WINY)*COLS];

    for (i=0;i<ROWS;i++)
      for (j=0;j<COLS;j++) {
        curbesterr[i*COLS+j]  = 9999999.0;
	curbestdisp[i*COLS+j] = 0;
    }   

    for (curdisp=0;curdisp<MAXDISP;curdisp++) {
      
      secs = MPI_Wtime();
      gendiffimg(ref,m1,m2,diffimg,curdisp);
      dsecsG += MPI_Wtime() - secs;
      
      secs = MPI_Wtime();
      updatedispimg(diffimg,curbesterr,curbestdisp,curdisp);
      esecsG += MPI_Wtime() - secs;
    }
    
    len[0] = 1;
    *dataout = curbestdisp;
    
    Give(dataout, len);
  }
} 


// output and test of the result (correspondance with the input data)
void stage3 (void) {
  int i,j; 
  int **len_ptr = (int **) malloc (sizeof (int *));
  void **data;
  int *curbestdisp;

  double oldsecs = 0, newsecs;
  int itercount = 0;
  
  while (data = Take(len_ptr)) {
    curbestdisp = (int *) *data;
    testdata(curbestdisp);
    newsecs = MPI_Wtime();
    st3secs[itercount++] = newsecs - oldsecs;
    oldsecs = newsecs;
  }

  // Compute and send true throughput
  truethrput = 0;
  for (i=1; i<ITERS; i++) truethrput += st3secs[i];
  truethrput = 1/(truethrput/(ITERS-1));
} 




// ------------------------------------------------------------------------- //
int main (int argc, char *argv[])
{

  // misc
  int     i, count, curdisp;
  float   start;

  // pipeline stuff
  int outmul;
  int mystagenum;  
  int mymult;
  spread_t spreads[STAGES+1] = {SPGLOBAL, SPGLOBAL,SPGLOBAL,SPGLOBAL};
  MPI_Datatype types[STAGES+1] = {MPI_INT, MPI_INT, MPI_INT, MPI_INT};
  Imode_t imodes[STAGES] = {EXPL, EXPL, EXPL};
  eSkel_molecule_t *(*stages[STAGES])(eSkel_molecule_t *) = {
    (eSkel_molecule_t * (*)(eSkel_molecule_t *)) stage1,
    (eSkel_molecule_t * (*)(eSkel_molecule_t *)) stage2,
    (eSkel_molecule_t * (*)(eSkel_molecule_t *)) stage3
  };
   
   
  int size;
  int nprocs;
  double secs, runtime;
  void *buf;
  MPI_Status junk;
 
  // Initializing MPI and eSkel
  MPI_Init(&argc, &argv);
  MPI_Comm_size(MPI_COMM_WORLD, &nprocs);
  SkelLibInit();

  // Starting the application
  printf("Entering the Multiple-Baseline Stereo application, Process %d\n", myrank());
  secs = MPI_Wtime();
  
  // Customized types
  MPI_Type_contiguous((ROWS+WINY)*COLS, MPI_INT, &MPI_int_array);
  MPI_Type_commit(&MPI_int_array);
  types[1]   = MPI_int_array;
  types[2]   = MPI_int_array;

  if (myrank()==0)       mystagenum = 0;
  else if (myrank()==1)  mystagenum = 1;
  else                   mystagenum = 2;
  
  
  Pipeline (STAGES,  imodes, stages, mystagenum, BUF, spreads, types, 
	    NULL, 0, 0, NULL, 0, &outmul, 0, mycomm());

  // Print the total time for each process
  runtime = MPI_Wtime()-secs;

  if (myrank()==0) {
    buf = (double *) malloc(sizeof(double)*2);
    MPI_Recv(buf,2,MPI_DOUBLE,1,MPI_ANY_TAG,mycomm(),&junk); 
    dsecsG = ((double *)buf)[0];
    esecsG = ((double *)buf)[1];
    MPI_Recv(&runtime,1,MPI_DOUBLE,2,MPI_ANY_TAG,MPI_COMM_WORLD,&junk); 
    MPI_Recv(&truethrput,1,MPI_DOUBLE,2,MPI_ANY_TAG,mycomm(),&junk); 
    printperf(CASE3ST,nprocs,dsecsG,esecsG,ITERS,truethrput,runtime);
  }
  
  if (mystagenum==1) {
    buf = (double *) malloc(sizeof(double)*2);
    ((double *)buf)[0]=dsecsG;
    ((double *)buf)[1]=esecsG;
    MPI_Send(buf,2,MPI_DOUBLE,0,0,mycomm());
  }

  if (mystagenum==2) {
    MPI_Send(&runtime,1,MPI_DOUBLE,0,0,MPI_COMM_WORLD);
    MPI_Send(&truethrput,1,MPI_DOUBLE,0,0,mycomm());
  }

  // Finalize MPI
  MPI_Finalize();
  return 0;

}