/*************************************************************************
 File Name  : dymprog.c
 Purpose    : provides enumerative algorithm to compute energies
              for deformable classification or energy minimization
 Release    : Version 1.0
 Date	    : Aug 31,1995

GSNAKE API is jointly developed by the Information Technology Institute (ITI), Singapore, and the School of Applied Science, Nanyang Technological
University (NTU), Singapore. 

These software programs are available to the user without any license or royalty fees. Permission is hereby granted to use, copy, modify, and distribute this software and its documentation for any purpose. ITI and NTU gives no warranty, express, implied, or statuary for the software and/or documentation provided, including, without limitation, waranty of merchantibility and warranty of fitness for a particular purpose. The software provided hereunder is on an "as is"  basis, and ITI and NTU has no obligation to provide maintenance, support, updates, enhancements, or modifications.

GSNAKE API is available for any UNIX compatible system. User feedback, bugs, or software and manual suggestions should be sent via electronic mail to one of the following, who may or may not act on them as he/she desires :

		asschan@ntu.ac.sg
		kflai@iti.gov.sg
************************************************************************/

#include "gsnake.h"
#include "xwindow.h"

/*
 * dynamic programming. Returns NOERROR if successfully performed, MEMORYERROR if otherwise 
 */
int  GSNAKE::mn_dymprog(    short level,	  /* current level */
		            int segmentSpacing,	  /* search spacing */
		            int numSearchSegment, /* search neighborhood */
                            unsigned char blowup, /* blow up ratio */
                            int verbose,	  /* verbose on / off */
			    int showImg,	  /* show image ? */
		            int img_Xoffset,	  /* image offset */
			    int img_Yoffset
			     )
{

	double Jold, Jnew;	/* energy term */
	int numiteration = 1;
	int numcycle;
	int spacing;
	int rotate;
	int numloc;
	int nhoodY;
	short expand;
	GSNAKE *gsnake;
	int pt_Xoffset, pt_Yoffset;
	LOCATIONS searchLocs;
	
	/* use odd number as number of search segments */
	numSearchSegment =  2*((int)numSearchSegment/2) + 1 ;

	/* show gsnake */
	if ( !rawImg )
	{
		printf("\nNo root image in PYRAMID. Unable to minimise.\n");
		return PARAMERROR;
	}
	  
	expand = ROUNDOFF( LEVEL(level) );
	pt_Xoffset = img_Xoffset;
	pt_Yoffset = img_Yoffset;
	
	pt_Xoffset /=  expand ;
        pt_Yoffset /=  expand ; /* removeblowup */
	
	if (showImg)  GSNAKE::show(blowup, expand, showImg, 
			      img_Xoffset, img_Yoffset, 
			      pt_Xoffset, pt_Yoffset); 
			

	if( verbose ) {

		if( global_lambda < 0 )
                    fprintf(stdout, "\n(LOCAL MINMAX) ==>") ;
                else if ( global_lambda  > 1 )
                    fprintf(stdout, "\n(LOCAL LAMBDA's) ==>") ;
                else
                    fprintf(stdout, "\n(LAMBDA = %.3f) ==>",
                                global_lambda) ;
		fprintf(stdout," level : %d\n\n", level );
                fflush(stdout) ;
        }


	/* allocate memory for possible location to move */
	if ( searchLocs.init(numsnaxel, numSearchSegment) == MEMORYERROR ) {
	    fprintf(stderr, 
		"ERROR : <GSNAKE::dymprog> memory allocation error\n");

	    return MEMORYERROR;
	}

	for (numcycle = 1; numcycle <= 3; numcycle++) {

           spacing = (numcycle == 1) ? segmentSpacing : 1 ;
           rotate = (numcycle == 3) ? 1 : 0 ;

           computeAvgLength();

	   /* compute gsnake energy */
	   Jnew = ESnake( level );

	   gsnake = new GSNAKE ;
	   do {

		/* duplicate gsnake */
		duplicate(gsnake);

		/* get all possible location */
		searchLocs.generateLoc( mode, head, tail,
			numSearchSegment/2, 0, spacing, rotate ); 

		/* invoke dynamic programming */
		dymprog(spacing, &searchLocs);

		Jold = Jnew;

		/* compute new energy */
		Jnew = ESnake( level );

		if (showImg)
			GSNAKE::move( gsnake, pt_Xoffset, pt_Yoffset);  
		
                if (verbose ) {
	
                    fprintf(stdout, "  %s",  (!direction) ? " " :
                        ( (direction > 0) ? "+" : "-" ) ) ;
                    fprintf(stdout, "(dp:%d:%d:%d) E = %.5f\n",
                                numcycle, numSearchSegment, numiteration,
                                Jnew);
                    fflush(stdout) ;

		    /* display the new location */

                }

		numiteration ++;

	   } while ( Jnew < Jold );

	   /* copy to original snake */
	   gsnake->duplicate(this);
	   delete gsnake ;
	 
	} /* end for */

	if (showImg) GSNAKE::show(blowup, expand, showImg, img_Xoffset, 
			      img_Yoffset, pt_Xoffset, pt_Yoffset ); 

	if(verbose)
                fprintf(stdout, "  Ext = %.3f, E = %.3f\n",
                        Eext, Esnake) ;

        return NOERROR ;
}

/*
 * main function to invoke dynamic proogramming
 */
double GSNAKE::dymprog( int spacing, 	  	/* desire spacing */
			LOCATIONS *searchLocs,  /* search locations */
		        int marginalize	  	/* classification */
		        )  
{
	double Jnow = 0;	/* current energy */
	double MinPath = 0;	/* minimun energy path */
	double peak = 0;	/* compute peak summation  for 
				   classification purpose */
	double term;
	double path;

	register int i0, i1, i2;
	int index, ind_1, ind_2;
	SNAXEL *prev, *now, *next;

	computeAvgLength();		/* av. length */

	/* set initial value */
	searchLocs->clearLoc();
	
	for (index=0,prev=head; index<numsnaxel-2 ;
		index++, prev=prev->getNext()){

	    ind_1 = (index+1)%(numsnaxel);	/* now */
	    ind_2 = (index+2)%(numsnaxel);	/* next */

	    now = prev->getNext( mode, head );
	    next = now->getNext( mode, head );

	    for (i1=0; i1<searchLocs->getNumLoc(); i1++) {	/* now */

		 now->putCol( searchLocs->getCol( ind_1, i1 ) );
		 now->putRow( searchLocs->getRow( ind_1, i1 ) );

		 for ( i2=0; i2<searchLocs->getNumLoc(); i2++) { /* next */

		    next->putCol( searchLocs->getCol( ind_2, i2 ) );
		    next->putRow( searchLocs->getRow( ind_2, i2 ) );

		    searchLocs->putNewSum( i1, i2, 0 );
		    searchLocs->putNewMax( i1, i2, -VERY_BIG );

		    for (i0=0; i0<searchLocs->getNumLoc(); i0++) { /* prev */

			prev->putCol( searchLocs->getCol( index, i0 ) );
			prev->putRow( searchLocs->getRow( index, i0 ) );

			/* compute energy of current point */
			Jnow = -1*ESnaxel( now, ind_1, i1, spacing, searchLocs);  

			/* if first snaxel, add energy of 1st snaxel */
			if ( index == 0 )
			   Jnow -= ESnaxel( prev, index, i0, spacing, 
			   			searchLocs);

			/* if last snaxel, add energy of last snaxel */
			else if (ind_2 == numsnaxel - 1)
			   Jnow -= ESnaxel(next, ind_2, i2, spacing, 
			   			searchLocs);

			if (marginalize) {
				searchLocs->putNewSum( i1, i2,   
					searchLocs->getNewSum( i1, i2 ) + 
					exp(Jnow/sig_nu_sqr) *
					searchLocs->getOldSum( i0, i1 ) );
			}

			if ( (term = Jnow + searchLocs->getOldMax( i0, i1 )) > 
				searchLocs->getNewMax( i1, i2 ) ) {

				searchLocs->putNewMax( i1, i2, term );
				searchLocs->putMaxInd( index, i1, i2, i0 );
			}

		    } /*i0 */
		} /* i2 */
	    } /* i1 */

	    for ( i1 = 0; i1 < searchLocs->getNumLoc(); i1++ )
		for (i2 = 0; i2 < searchLocs->getNumLoc(); i2++ ) {

		   if ( marginalize ) {
			searchLocs->putOldSum( i1, i2, 
				searchLocs->getNewSum( i1, i2 ) );
		   }

		   searchLocs->putOldMax( i1, i2, 
				searchLocs->getNewMax( i1, i2 ) );
	    }

	} /* end for */

	/* perform backtracking */
	for ( i1 = 0; i1<searchLocs->getNumLoc(); i1++)
	   for ( i2 = 0; i2<searchLocs->getNumLoc(); i2++) {
		peak += searchLocs->getNewSum( i1, i2 );
		if ( (path=exp(searchLocs->getNewMax( i1, i2 ))) > MinPath ) {
			MinPath = path;
			searchLocs->putMI( ind_1, i1 );
			searchLocs->putMI( ind_2, i2 );
		}
	}

	/* look for best path */
	for ( i0 = ind_1 - 1; i0 >= 0; i0-- )
	{
		searchLocs->putMI( i0, 
			searchLocs->getMaxInd( i0, 
				searchLocs->getMI( i0+1 ),
				searchLocs->getMI( i0+2 ) ) );
	}

	/* update snake */
	for (now = head, i0 = 0; now && i0 < numsnaxel;
		now = now->getNext(), i0++ ) {
		now->putCol( searchLocs->getCol(i0, searchLocs->getMI( i0 ) ) );
		now->putRow( searchLocs->getRow(i0, searchLocs->getMI( i0 ) ) );
	}

	return peak;
}

LOCATIONS::~LOCATIONS( void )
{
	register short i0, i1;

	for (i0=0; i0<numpts; i0++)
	   for (i1=0; i1<numlocs; i1++) 

		_iMemFree( MaxInd[i0][i1] );

	for (i0=0; i0<numlocs; i0++) {

	     _iMemFree( OldSum[i0] );
	     _iMemFree( NewSum[i0] );
	     _iMemFree( OldMax[i0] );
	     _iMemFree( NewMax[i0] );

	}

	for (i0=0; i0<numpts; i0++) {

	     _iMemFree( row[i0] );
	     _iMemFree( col[i0] );
	     _iMemFree( MaxInd[i0] );
		
	}
	
	

	_iMemFree( OldSum );
        _iMemFree( NewSum );
        _iMemFree( OldMax );
        _iMemFree( NewMax );
	_iMemFree( row );
        _iMemFree( col );
        _iMemFree( MaxInd );
	_iMemFree( MI ); MI = NULL;
	_iMemFree( normalX ); normalX = NULL;
	_iMemFree( normalY ); normalY = NULL;
}
	

/*
 * generateLoc by linear search algorithm
 */
void  LOCATIONS::generateLoc( SNAKEMODE mode,	/* opened / closed snake */
			     SNAXEL *head,	/* head od snake */
			     SNAXEL *tail,	/* tail of snake */
			     int NhoodX,	/* region(x) to move */
			     int NhoodY,	/* region(y) to move */
			     int spacing,
			     int rotate,
			     short verbose )	/* rotate */
{
	register int i, j, k;
	double len;
	SNAXEL *sxptr;

	/* compute possible location */
	for ( sxptr=head, i=0; sxptr; i++, sxptr = sxptr->getNext() ) {

	   /* get normal vector */
	   sxptr->getNormalVec( mode, head, tail, &normalX[i], &normalY[i] );

	   if (rotate) { /* do not use normal vector */

		double dx = normalX[i];
		double dy = normalY[i];

		/* rotate by 90 degree */

		normalX[i] = -dy;
		normalY[i] = dx;

	   }

	   /* assign the location */
	   for ( j=-NhoodX; j<=NhoodX; j++)
	      for (k=-NhoodY; k<=NhoodY; k++) {

		int index = (j+NhoodX)*(2*NhoodY+1) + (k+NhoodY);
		row[i][index] = sxptr->getRow() + 
			spacing * (j*normalY[i]-k*normalX[i]);
		col[i][index] = sxptr->getCol() + 
			spacing * (j*normalX[i]+k*normalY[i]);

		if (verbose)
			xwin_DrawPoint(ROUNDOFF(row[i][index]), 
				ROUNDOFF( col[i][index] )); 
	   }

	}
}

/* 
 * allocate memory for  dynamic programming parameters
 */
int LOCATIONS::init( short _numpts, short _numlocs)
{
	register short i0, i1;

	/* get parameters */
	numpts = _numpts;
	numlocs= _numlocs;

	row = (double **) _iMemAlloc( numpts*sizeof(double *) );
	col = (double **) _iMemAlloc( numpts*sizeof(double *) );

	OldSum = (double **) _iMemAlloc( numlocs*sizeof(double *) );
	NewSum = (double **) _iMemAlloc( numlocs*sizeof(double *) );

	OldMax = (double **) _iMemAlloc( numlocs*sizeof(double *) );
	NewMax = (double **) _iMemAlloc( numlocs*sizeof(double *) );

	MaxInd = (short ***) _iMemAlloc( numpts*sizeof(short **) );
	MI = (short *) _iMemAlloc( numpts*sizeof(short) );

	normalX = (double *) _iMemAlloc( numpts*sizeof(double) );
	normalY = (double *) _iMemAlloc( numpts*sizeof(double) );

	if ( !row || !col || !OldSum || !NewSum || !OldMax || 
	     !NewMax || !MaxInd || !MI || !normalX || !normalY )

	     return MEMORYERROR;

	for (i0=0; i0<numlocs; i0++) {

	     OldSum[i0] = (double *) _iMemAlloc(numlocs*sizeof(double));
	     NewSum[i0] = (double *) _iMemAlloc(numlocs*sizeof(double));
	     OldMax[i0] = (double *) _iMemAlloc(numlocs*sizeof(double));
	     NewMax[i0] = (double *) _iMemAlloc(numlocs*sizeof(double));

	     if ( !OldSum[i0] || !NewSum[i0] || 
		!OldMax[i0] || !NewMax[i0] )

	     	return MEMORYERROR;

	}

	for (i0=0; i0<numpts; i0++) {

	     row[i0] = (double *) _iMemAlloc(numlocs*sizeof(double));
	     col[i0] = (double *) _iMemAlloc(numlocs*sizeof(double));
	     MaxInd[i0] = (short **)_iMemAlloc(numlocs*sizeof(short *));
		
	     if ( !row[i0] || !col[i0] || !MaxInd[i0] )

	     	return MEMORYERROR;
	}
	
	for (i0=0; i0<numpts; i0++)
	   for (i1=0; i1<numlocs; i1++) {

		MaxInd[i0][i1] = (short *) _iMemAlloc(numlocs*sizeof(short));

		if ( !MaxInd[i0][i1] )
		     return MEMORYERROR;
	}

	return NOERROR;
}

void LOCATIONS::clearLoc()
{
	register short i0, i1;

	for (i0=0; i0<numlocs; i0++)
	   for (i1=0; i1<numlocs; i1++) {

		OldMax[i0][i1] = 0;
		OldSum[i0][i1] = 1;
		NewMax[i0][i1] = 0;
		NewSum[i0][i1] = 0;

	}
}