/*---------------------------------------------------------------------------*/
// Baseline Wavelet Transform Coder Construction Kit
//
// Geoff Davis
// gdavis@cs.dartmouth.edu
// http://www.cs.dartmouth.edu/~gdavis
//
// Copyright 1996 Geoff Davis 9/11/96
//
// Permission is granted to use this software for research purposes as
// long as this notice stays attached to this software.
//
/*---------------------------------------------------------------------------*/

// File: depacketize.cc (IB)
// Modification of decode.cc

#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <iostream.h>
#include <fstream.h>
#include <time.h>
#include "transform.h"
#include "coeffset.h"
#include "allocator.h"
#include "quantizer.h"
#include "global.h"
#include "image.h"
#include "conceal.h"
#include "packet.h"

#define TH 512.0         // threshold for edge detection
#define CONCEAL 0       // conceal: 0 = no, 1 = bilin, 2 = bicub, 3 = median
                        // (change the loop im ConcealMedian for option 3)
                        // 4 = bilin for LL, LH, HL, median for others
						// 5 = MAP, 6 = testing
#define BILIN_ONLY 1    // bilinear (or edge-directional?)
#define FIND_NEAREST 1  // dispersive packetization?
#define RASTER 0        // raster format
#define BICUBIC 0       // use bicubic interpolation?
#define TIGHTGRID 1     // use tight grid?

#define MISSINGVAL 10000000

/*---------------------------------------------------------------------------*/
void decompress     (Image **image, Wavelet *wavelet, int nStages, 
		     int capacity, int paramPrecision, char *filename, 
		     int nQuant, int monolayer, int *missing);
		     
int in_band(int x, int y, int sbHsize, int sbVsize);
void concealMedian(CoeffSet **coeff, int *missing, int LLSize, int nSets);		     

void conceal_zero(CoeffSet **coeff, int *missing, int hor, int ver, int nSets);
void concealBilin(CoeffSet **coeff, int *missing, int hor, int ver, int nSets);
void concealBicub(CoeffSet **coeff, int *missing, int LLSize);		     
Real bicubInt(Real c[4][4], Real x1, Real x2);
Real compare(char *imagefile1, char *imagefile2); 
void gridAdapt(CoeffSet **coeff, int LLHsize, int LLsize, Real th, Real thdet, 
               int i, Real *x1, Real *x2, int *HFVhoriz, int *HFVvert, 
               int *horiz, int *vert);
void my_delay (int n);
void concealMAP(CoeffSet **coeff, int *missing, int hor, int ver, int nSets);
void gettree2d(double **tree, CoeffSet **coeff, int xLL, int yLL, int hor, int ver, int treesize, int Nlevels, int Nbands);
void est_decay(double *ah, double *av, double *ad, double **tree, int treesize, int Nlevels, int Nbands);
void testtree(CoeffSet **coeff);
/*---------------------------------------------------------------------------*/

int main (int argc, char **argv)
{
  char *program = argv[0];

  if (argc < 6)  {
    fprintf (stderr, "Usage: %s [encoded image] [decoded image] [original image] [data file] [result file]\n", program);
    exit(0);
  }

  char *infile_name = argv[1];
  char *outfile_name = argv[2];
  char *origfile_name = argv[3];
  char *datafile_name = argv[4];
  char *resfile_name = argv[5];
  
  printf ("Reading compressed image %s, writing %s\n\n", 
	  infile_name, outfile_name);

  // Create a new wavelet from the 7/9 Antonini filterset
  Wavelet *wavelet = new Wavelet (&Antonini);
  // Adelson & Simoncelli 9-tap QMF
  //Wavelet *wavelet = new Wavelet (&Adelson);

  // Coding parameters
  int nStages = 4;          // # of stages in the wavelet transform
  int capacity = 512;       // capacity of histogram for arithmetic coder
  int paramPrecision = 4;   // precision for stored quantizer parameters
  int nQuant = 10;          // # of quantizers to examine for allocation (10)
  int monolayer = FALSE;    // TRUE for non-embedded uniform quantizer
                            // FALSE for multilayer quantizer


  Image *reconstruct;
  
  int i, status, Nexper, mp, missing[16];
  Real psnr, variance, res[2000];      // may have to increase the size of res
  FILE *dfile, *rfile;
  char outname[80];
  
  if ((dfile = fopen(datafile_name, "r")) == NULL)
    error ("Unable to open file %s", datafile_name);
    
  Nexper = 0; // number of experiments
  
  while(!feof(dfile))
  {
    for (i = 0; i < 16; i++) missing[i] = 0;
    
#ifdef PGM
    // for saving each image separately
    //sprintf(outname, "%s%03d%s", outfile_name, Nexper, ".pgm");
    
    // for keeping only last image
    sprintf(outname, "%s%s", outfile_name, ".pgm");
#endif

    Nexper++;
    mp = 0;
    
    do
    {
      status = fscanf(dfile, "%d ", &mp);
      if (status != 1)
        printf("\n Error reading file %s", datafile_name);
      else if (mp != 10000)
      {
        //printf("%d\n",mp);
        missing[mp] = 1;     
      }
    }
    while (!feof(dfile) && mp != 10000);  // 10000 marks end of line
        
    decompress (&reconstruct, wavelet, nStages, capacity, paramPrecision,
    	        infile_name, nQuant, monolayer, missing);
    	        
#ifdef PGM
    reconstruct->savePGM (outname);
#else
    //reconstruct->saveRaw (outfile_name);
    // Currently index is 0 - change for loop
    printf("saving raster file...\n");
    reconstruct->saveRas (outfile_name, 0);
#endif

    delete reconstruct;
    my_delay(2);
    
#ifdef PGM
    psnr = compare (outname, origfile_name);
#else
    psnr = compare (outfile_name, origfile_name);
#endif

    printf("%f\n", psnr);
    res[Nexper-1] = psnr;
    
    Image *reconstruct;
  }
  
  fclose(dfile);
  
  //delete reconstruct;
  //delete wavelet;
  
  if ((rfile = fopen(resfile_name, "w")) == NULL)
    error ("Unable to open file %s", resfile_name);
  
  printf("result file opened\n");

  // find average PSNR
  psnr = 0.0;
  for (i = 0; i < Nexper; i++) 
  {
    fprintf(rfile, "%f\n", res[i]); 
    psnr += res[i];
  }  
  
  psnr /= (Real)Nexper;
  
  // find variance in PSNR
  variance = 0.0;
  for (i = 0; i < Nexper; i++)  
    variance += (psnr-res[i])*(psnr-res[i]);
    
  variance /=(Real)Nexper;

  fprintf(rfile, "Average PSNR: %f\n", psnr);
  fprintf(rfile, "Variance in PSNR: %f\n", variance);

  printf("results written\n");
  
  fclose(rfile);
  
  return 0;
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/
// Decompress an image
//
// Image **image         Place for decompressed image to be stored
// Wavelet *wavelet      Wavelet to use for transform
// int nStages           # of stages to use in transform
// int capacity          Capacity of histograms for arithmetic coder
// int paramPrecision    Precision for storing quantizer parameters --
//                          precision = n means params are stored with
//                          accuracy 2^(-n)
// int maxQuant          Maximum # of different quantizer resolutions
//                          used on any subband.  
// char *filename        Name of compressed file
// int monolayer         TRUE for non-embedded quantizer, FALSE for embedded

/*---------------------------------------------------------------------------*/
void decompress (Image **image, Wavelet *wavelet, int nStages, 
		 int capacity, int paramPrecision, char *filename, 
		 int maxQuant, int monolayer, int *missing)
{
  int i, j;
  char hfilename[80];
  //  int cnt[4][3], stp[4][3], pts[4][3];

  // -------- Portions added or modified ---------------------------------
  // open compressed header file
  sprintf(hfilename, "%c%s",'h', filename);
  //ifstream hfile (hfilename, ios::in | ios::nocreate | ios::bin);
  ifstream hfile (hfilename, ios::in | ios::nocreate | ios::binary);
  if (!hfile) {
    error ("Unable to open file %s", hfilename);
  }

  // Create I/O interface object for header file
  Decoder *hdecoder = new Decoder (hfile);

  // Read image dimensions from file
  int hsize = hdecoder->readPositive ();
  int vsize = hdecoder->readPositive ();
  //  printf ("hsize = %d, vsize = %d\n", hsize, vsize);
  
  
  // Create an empty transform of the appropriate size -- fill it in
  //    as coefficients are decoded
  WaveletTransform *transform = 
    new WaveletTransform (wavelet, hsize, vsize, nStages);

  // For each subband allocate a CoeffSet, an EntropyCoder, and a
  //    Quantizer (don't need to know anything about errors here)
  int nSets = transform->nSubbands;
  CoeffSet **coeff = new CoeffSet* [nSets];
  EntropyCoder **entropy = new EntropyCoder* [nSets];
  Quantizer **quant = new Quantizer* [nSets];
  // Quantizer precision for each subband
  int *precision = new int [nSets];

  for (i = 0; i < nSets; i++) {
    if (monolayer) {
      // Use uniform quantizer and single layer escape coder for each
      //   subband
      entropy[i] = new EscapeCoder (capacity);
      // Assume all subbands have pdf's centered around 0 except the
      //   low pass subband 0
      quant[i]   = new UniformQuant 
	((MonoLayerCoder *)entropy[i], paramPrecision, i != 0, nStages);
    } else {
      // Use a layered quantizer with dead zones and a layered entropy
      //   coder for each subband
      entropy [i] = new LayerCoder (maxQuant, i != 0, capacity);
      // Assume all subbands have pdf's centered around 0 except the
      //   low pass subband 0
      quant [i] = new LayerQuant ((MultiLayerCoder *)entropy[i], 
                                  paramPrecision, i != 0, maxQuant, nStages);
    }
    // Indicate that each set of coefficients to be read corresponds
    //    to a subband
    coeff[i]   = new CoeffSet (transform->subband(i),
			       transform->subbandSize[i], quant[i]);
  }

  // read number of repetitions
  int *n = new int  [nSets];
  for (i = 0; i < nSets; i++)
    n[i] = hdecoder->readPositive ();
  
  
  for (i = 0; i < nSets; i++) {
    // Read quantizer parameters for each subband
    coeff[i]->readHeader (hdecoder, precision[i], i);   
    //printf("%d header read\n",i);
  }
  
  // close header file
  delete hdecoder;
  hfile.close ();
  
  
  // Open packet files and create decoders
  const int nP = 16;
  Decoder **sdecoder = new Decoder* [nP];
  char sfilename[nP][80];
  ifstream *sfile = new ifstream [nP];
  
/*  int missing[16];  // array of missing packets
  
  for (i = 0; i < 16; i++) 
  {
    // Create output filename 
    sprintf(sfilename[i], "%c%s%03d", 's', filename, i);
    sfile[i].open(sfilename[i], ios::in | ios::nocreate | ios::bin);
    if (!sfile[i]) 
    {
      //error ("Unable to open file %s", sfilename[i]);
      sfile[i].close();
      printf("Unable to open file %s - dummy decoder created\n", sfilename[i]);
      sfile[i].open("dummy", ios::in | ios::nocreate | ios::bin);
      missing[i] = 1;
      //sdecoder[i] = NULL;
    }
    else 
    { 
      missing[i] = 0;
    // Create I/O interface object for subband file
    sdecoder[i] = new Decoder (sfile[i]);
    }
  }
*/


  // define packet objects - 05/17/01
  PacketDec **pkt = new PacketDec* [nP];
  EntropyCoder **entLL = new EntropyCoder* [nP];
  EntropyCoder **entHF = new EntropyCoder* [nP];
  for (i = 0; i < nP; i++) 
  {
    // packet object
    entLL[i] = new LayerCoder (maxQuant, 0, capacity);
    entHF[i] = new LayerCoder (maxQuant, 1, capacity);
    pkt[i] = new PacketDec((MultiLayerCoder *)entLL[i], 
                           (MultiLayerCoder *)entHF[i]);
    
    // Create input filename 
    sprintf(sfilename[i], "%c%s%03d", 's', filename, i);
    if (missing[i]) 
    {
      //error ("Unable to open file %s", sfilename[i]);
      printf("Unable to open file %s - dummy decoder created\n", sfilename[i]);
      //sfile[i].open("dummy", ios::in | ios::nocreate | ios::bin);
	  sfile[i].open("dummy", ios::in | ios::nocreate | ios::binary);
    }
    else 
    { 
      // Create I/O interface object for subband file
      //sfile[i].open(sfilename[i], ios::in | ios::nocreate | ios::bin);
	  sfile[i].open(sfilename[i], ios::in | ios::nocreate | ios::binary);
      sdecoder[i] = new Decoder (sfile[i]);
    }
  }
  
  //printf("Decoders opened\n");
  //my_delay(2);
  
  // Decode coefficients
  for (i = 0; i < nSets; i++) 
  {
    for (j = 0; j < n[i]; j++)
      coeff[i]->decode (sdecoder, precision[i], i, missing, pkt, j);
      
    //printf("subband %d decoded\n", i);
  }
  
  // Close files
  for (i = 0; i < nP; i++) 
  {
    sfile[i].close ();
  }
  //printf("Packet files closed\n");
    
  delete sdecoder;
  delete n;
  
  for (i = 0; i < nP; i++)
  {
    delete entLL[i];
    delete entHF[i];
    delete pkt[i];
  }
  
  delete entLL;
  delete entHF;
  delete pkt;
  
  //my_delay(2);
    
  //printf("decoder deleted\n");
  
  // Error concealment
  
  switch (CONCEAL)
  {
    case 0 : conceal_zero(coeff, missing, hsize, vsize, nSets); // no concealment (replace by 0)
			 break;          
    case 1 : concealBilin(coeff, missing, hsize, vsize, nSets);
             break;
    case 2 : concealBicub(coeff, missing, transform->subbandSize[0]);
             break;
    case 3 : concealMedian(coeff, missing, transform->subbandSize[0], nSets);
             break;
    case 4 : concealMedian(coeff, missing, transform->subbandSize[0], nSets);
             concealBilin(coeff, missing, hsize, vsize, nSets);
             break;
	case 5 : concealMAP(coeff, missing, hsize, vsize, nSets);
			 break;
	case 6 : testtree(coeff);
			 break;

    default: break;
  }

/*  
//----------------------------------------------------------------
// write LL coefficients into file LLcoeff2
  FILE *llfile;
  
  if ((llfile = fopen("LLcoeff2", "w")) == NULL)
    error ("Unable to open file LLcoeff2");
  else
    for (i = 0; i < coeff[0]->nData; i++)
      fprintf(llfile, "%f\n", coeff[0]->data[i]);
      
  fclose(llfile);    
//-----------------------------------------------------------------
*/

  // ---------- End of additions and modifications -------------------

  // Clean up
  for (i = 0; i < nSets; i++) {
    delete entropy[i];
    delete quant[i];
    delete coeff[i];
  }
  delete [] entropy;
  delete [] quant;
  delete [] coeff;
  delete [] precision;

  //my_delay(2);
  
  // Allocate image and invert transform
  *image = new Image (hsize, vsize);
  transform->invert (*image);
  delete transform;
  
  printf("Memory cleared\n");
  
}

// ------------------------- Added ------------------------------------

int in_band(int x, int y, int sbHsize, int sbVsize)
{
  if (x >= 0 && y >= 0 && x < sbHsize && y < sbVsize)
    return 1;
  else
    return 0;
}

/*---------------------------------------------------------------------------*/

void conceal_zero(CoeffSet **coeff, int *missing, int hor, int ver, int nSets)
{
  // replace missing samples with zeros
  
  int sb, i; 
 
  for (sb = 0; sb < nSets; sb++)
  {
	  for (i = 0; i < coeff[sb]->nData; i++)
	  {
		  if (coeff[sb]->data[i] > (double)MISSINGVAL/2.0)   
		  {
			  coeff[sb]->data[i] = 0.0;
		  }
	  }
  }


}


void concealMedian(CoeffSet **coeff, int *missing, int LLsize, int nSets)
{
  int sbHsize, sbVsize;                   
  int nNeighbors, i, j, sb, x, y, pos;
  Real nbhd[8];                           // neighborhood
   
  sbHsize = sbVsize = (int)floor(sqrt(LLsize)); // change for variable size
  
  // median interpolation in high-freq subbands
  for (sb = 3; sb < nSets; sb++)
  {
    if (sb==4 || sb==7 || sb==10 || sb==13 || sb==16 || sb==19)
    {
      sbHsize *= 2;
      sbVsize *= 2;
    }
    
    //printf("Subband %d, size %dx%d\n", sb, sbHsize, sbVsize);
    
    for (i = 0; i < coeff[sb]->nData; i++)
    {
      if (missing[coeff[0]->packet(sb,i)-1])   
      {
        // this coefficient is missing - find available nearest neighbors
        x = (int) i%sbHsize;
        y = (int) floor(i/sbHsize);
        
        // initialize nbhd
        nNeighbors = 0;
        for (j = 0; j < 8; j++) nbhd[j] = 0.0;
        
        // (x-1,y-1)
        pos = (y-1)*sbHsize + (x-1);
        if ( in_band(x-1,y-1,sbHsize,sbVsize) && 
             !(missing[coeff[0]->packet(sb,pos)-1]) )
        {
          nbhd[nNeighbors] = coeff[sb]->data[pos];
          nNeighbors++;
        }
        
        // (x,y-1)
        pos = (y-1)*sbHsize + (x);
        if ( in_band(x,y-1,sbHsize,sbVsize) && 
             !(missing[coeff[0]->packet(sb,pos)-1]) )
        {
          nbhd[nNeighbors] = coeff[sb]->data[pos];
          nNeighbors++;
        }
        
        // (x+1,y-1)
        pos = (y-1)*sbHsize + (x+1);
        if ( in_band(x+1,y-1,sbHsize,sbVsize) && 
             !(missing[coeff[0]->packet(sb,pos)-1]) )
        {
          nbhd[nNeighbors] = coeff[sb]->data[pos];
          nNeighbors++;
        }
        
        // (x-1,y)
        pos = (y)*sbHsize + (x-1);
        if ( in_band(x-1,y,sbHsize,sbVsize) && 
             !(missing[coeff[0]->packet(sb,pos)-1]) )
        {
          nbhd[nNeighbors] = coeff[sb]->data[pos];
          nNeighbors++;
        }
        
        // (x+1,y)
        pos = (y)*sbHsize + (x+1);
        if ( in_band(x+1,y,sbHsize,sbVsize) && 
             !(missing[coeff[0]->packet(sb,pos)-1]) )
        {
          nbhd[nNeighbors] = coeff[sb]->data[pos];
          nNeighbors++;
        }
        
        // (x-1,y+1)
        pos = (y+1)*sbHsize + (x-1);
        if ( in_band(x-1,y+1,sbHsize,sbVsize) && 
             !(missing[coeff[0]->packet(sb,pos)-1]) )
        {
          nbhd[nNeighbors] = coeff[sb]->data[pos];
          nNeighbors++;
        }
        
        // (x,y+1)
        pos = (y+1)*sbHsize + (x);
        if ( in_band(x,y+1,sbHsize,sbVsize) && 
             !(missing[coeff[0]->packet(sb,pos)-1]) )
        {
          nbhd[nNeighbors] = coeff[sb]->data[pos];
          nNeighbors++;
        }
        
        // (x+1,y+1)
        pos = (y+1)*sbHsize + (x+1);
        if ( in_band(x+1,y+1,sbHsize,sbVsize) && 
             !(missing[coeff[0]->packet(sb,pos)-1]) )
        {
          nbhd[nNeighbors] = coeff[sb]->data[pos];
          nNeighbors++;
        }
        
        // median interpolation
        coeff[sb]->data[i] = scalar_median(nNeighbors, nbhd);
      }
    }   
  }
  
}

/*---------------------------------------------------------------------------*/

void concealBilin(CoeffSet **coeff, int *missing, int hor, int ver, int nSets)
{
  // For now, do simple linear interpolation from as many nearest neighbors
  // as possible
  
  int nNeighbors, i, j, LLHsize, LLVsize; 
  int hNeighbors, vNeighbors, d45Neighbors, d135Neighbors;
  int pos1, pos2, dist, edgeH, edgeV, edgeD45, edgeD135;
  Real localSum, diff, hSum, vSum, d45Sum, d135Sum;
  Real offsetH, offsetV, diffH, diffV; 
    
  LLHsize = (int) (hor/(int)pow(2.0, (double)((nSets-1)/3)));
  LLVsize = (int) (ver/(int)pow(2.0, (double)((nSets-1)/3)));

  
  // HL subband - linear interpolation vertically
  for (i = 0; i < coeff[1]->nData; i++)
  {
    //if (missing[coeff[0]->packet(1,i)-1])   // 05/14/2001: coeff[1] ??
    //if (floor(coeff[1]->data[i]) == MISSINGVAL)   
	if (coeff[1]->data[i] > (double)MISSINGVAL/2.0)   
    {
      // this coefficient is missing
      // find closest non-missing coefficient above and below
      //printf("Found a missing coefficient in HL!\n");
      pos1 = pos2 = i;
      while ((pos1-LLHsize >= 0) && 
             (coeff[1]->data[pos1] > (double)MISSINGVAL/2.0))
             //(missing[coeff[0]->packet(1,pos1)-1]))
               pos1 -= LLHsize;
      while ((pos2+LLHsize < coeff[1]->nData) && 
             (coeff[1]->data[pos2] > (double)MISSINGVAL/2.0))
             //(missing[coeff[0]->packet(1,pos2)-1]))
               pos2 += LLHsize;
	  if (coeff[1]->data[pos1] > (double)MISSINGVAL/2.0)
		  pos1 = i;
	  if (coeff[1]->data[pos2] > (double)MISSINGVAL/2.0)
		  pos2 = i;

      if ((pos2 == i) && (pos1 < i))
        coeff[1]->data[i] = coeff[1]->data[pos1];
      else if ((pos1 == i) && (pos2 > i))
        coeff[1]->data[i] = coeff[1]->data[pos2];
      else if ((pos1 < i) && (pos2 > i))
      {
        //printf("pos1 = %d, pos2 = %d\n", pos1, pos2);
        dist = (int)floor((i-pos1)/LLHsize);
        diff = coeff[1]->data[pos2] - coeff[1]->data[pos1];  
        coeff[1]->data[i] = coeff[1]->data[pos1] +
        	            (Real)dist*diff/(Real)floor((pos2-pos1)/LLHsize);

		//printf("subband 1: (x,y) = (%d,%d), coeff = %e\n", i%LLHsize, i/LLHsize, coeff[1]->data[i]);
      }
      else
      {
        // case: pos2 = pos1 = i
        printf("No neighbors in HL subband at location %d\n",i);
        coeff[1]->data[i] = 0.0;  
      }
    }
  }

  // LH subband - linear interpolation horizontally
  for (i = 0; i < coeff[2]->nData; i++)
  {
    //if (missing[coeff[0]->packet(2,i)-1])
    if (coeff[2]->data[i] > (double)MISSINGVAL/2.0)
    {
      // this coefficient is missing
      // find closest non-missing coefficient to the left and right      
      //printf("Found a missing coefficient in LH!\n");
      pos1 = pos2 = i;
      while ((floor((pos1-1)/LLHsize) == (floor(pos1/LLHsize))) && 
             (pos1-1 >= 0) && (coeff[2]->data[pos1] > (double)MISSINGVAL/2.0))
             //(missing[coeff[0]->packet(2,pos1)-1]))
               pos1--;
      while ((floor((pos2+1)/LLHsize) == (floor(pos2/LLHsize))) && 
             (pos2+1 < coeff[2]->nData) && (coeff[2]->data[pos2] > (double)MISSINGVAL/2.0))
             //(missing[coeff[0]->packet(2,pos2)-1]))
               pos2++;

	  if (coeff[2]->data[pos1] > (double)MISSINGVAL/2.0)
		  pos1 = i;
	  if (coeff[2]->data[pos2] > (double)MISSINGVAL/2.0)
		  pos2 = i;

      if ((pos2 == i) && (pos1 < i))
        coeff[2]->data[i] = coeff[2]->data[pos1];
      else if ((pos1 == i) && (pos2 > i))
        coeff[2]->data[i] = coeff[2]->data[pos2];
      else if ((pos1 < i) && (pos2 > i))
      {
        //printf("pos1 = %d, i = %d, pos2 = %d\n", pos1, i, pos2);
		
        dist = i-pos1;
        diff = coeff[2]->data[pos2] - coeff[2]->data[pos1];  
        coeff[2]->data[i] = coeff[2]->data[pos1] +
        	            (Real)dist*diff/(Real)(pos2-pos1);

		//printf("subband 2: (x,y) = (%d,%d), coeff = %e\n", i%LLHsize, i/LLHsize, coeff[2]->data[i]);
      }
      else
      {
        // case: pos2 = pos1 = i
        printf("No neighbors in LH subband at location %d\n",i);
        coeff[2]->data[i] = 0.0;  
      }
    }
  }

  
  // LL subband
  for (i = 0; i < coeff[0]->nData; i++)
  {
    //if (missing[coeff[0]->packet(0,i)-1])
    if (coeff[0]->data[i] > (double)MISSINGVAL/2.0)
    {
      //printf("Missing coefficient at position %d from LL subband\n", i);
      // this coefficient is missing
            
      hNeighbors = 0;
      vNeighbors = 0;
      nNeighbors = 0;
      d45Neighbors = 0;
      d135Neighbors = 0;
      hSum = 0.0;
      vSum = 0.0;
      d45Sum = 0.0;
      d135Sum = 0.0;
      localSum = 0.0;
      edgeH = 0;
      edgeV = 0;
      
      int jmax = FIND_NEAREST ? LLHsize : 1; 
      
      if (fabs(coeff[1]->data[i]) > TH)
      {
        edgeV = 1;  // vertical edge (edge encountered in horiz. direction)
        //printf("Vertical edge detected\n");
      }  
      if (fabs(coeff[2]->data[i]) > TH)
      {
        edgeH = 1;  // horizontal edge (edge encountered in vert. direction)
        //printf("Horizontal edge detected\n");
      } 
            
      for(j = 1; j <= jmax; j++)
	  {
       if (i-j*LLHsize >= 0)
        //if (!missing[coeff[0]->packet(0,i-j*LLHsize)-1])
        if (coeff[0]->data[i-j*LLHsize] < (double)MISSINGVAL/2.0)
        { 
          nNeighbors++; 
          localSum += coeff[0]->data[i-j*LLHsize];
          vNeighbors++;
          vSum += coeff[0]->data[i-j*LLHsize];
          break;
        }
	  }
      for(j = 1; j <= jmax; j++)
	  {
		  // if (i-j >= 0)
       if (i-j >= LLHsize*(i/LLHsize))
        //if (!missing[coeff[0]->packet(0,i-j)-1])
        if (coeff[0]->data[i-j] < (double)MISSINGVAL/2.0)
        { 
          nNeighbors++; 
          localSum += coeff[0]->data[i-j];
          hNeighbors++; 
          hSum += coeff[0]->data[i-j];
          break;
        }
	  }
      for(j = 1; j <= jmax; j++)  
	  {
       //if (i+j < coeff[0]->nData)
	   if (i-j >= LLHsize*(i/LLHsize+1))
        //if (!missing[coeff[0]->packet(0,i+j)-1])
        if (coeff[0]->data[i+j] < (double)MISSINGVAL/2.0)
        { 
          nNeighbors++; 
          localSum += coeff[0]->data[i+j];
          hNeighbors++; 
          hSum += coeff[0]->data[i+j];
          break;
        }
	  }
      for(j = 1; j <= jmax; j++)
	  {
       if (i+j*LLHsize < coeff[0]->nData)
        //if (!missing[coeff[0]->packet(0,i+j*LLHsize)-1])
        if (coeff[0]->data[i+j*LLHsize] < (double)MISSINGVAL/2.0)
        { 
          nNeighbors++; 
          localSum += coeff[0]->data[i+j*LLHsize];
          vNeighbors++; 
          vSum += coeff[0]->data[i+j*LLHsize];
          break;
        }
	  }
            
      edgeD45=0; edgeD135=0;
      offsetH = 0.0; offsetV = 0.0;
      diffH = 0.0; diffV = 0.0;

/*        
      if (edgeV && edgeH && vNeighbors > 0 && hNeighbors > 0 &&
          i-LLHsize >= 0 && i+LLHsize >= 0 && !BILIN_ONLY)
          // note: this is old algorithm and not executed for BILIN_ONLY, 
          // so MISSINGVAL not included 
        { 
          // diagonal, determine which one
          if (!missing[coeff[0]->packet(0,i-1)-1] &&
              !missing[coeff[0]->packet(0,i+1)-1] &&
              !missing[coeff[0]->packet(0,i-LLHsize)-1] &&
              !missing[coeff[0]->packet(0,i+LLHsize)-1])             
            { 
              if ((coeff[0]->data[i-1] >= coeff[0]->data[i+1] &&
                 coeff[0]->data[i-LLHsize] >= coeff[0]->data[i+LLHsize]) ||
                 (coeff[0]->data[i-1] <= coeff[0]->data[i+1] &&
                 coeff[0]->data[i-LLHsize] <= coeff[0]->data[i+LLHsize]))
                { 
                  edgeD45 = 1; edgeD135 = 0;
                }
              else if 
               ((coeff[0]->data[i-1] >= coeff[0]->data[i+1] &&
                 coeff[0]->data[i-LLHsize] <= coeff[0]->data[i+LLHsize]) ||
                 (coeff[0]->data[i-1] <= coeff[0]->data[i+1] &&
                  coeff[0]->data[i-LLHsize] >= coeff[0]->data[i+LLHsize]))
                {  
                  edgeD45 = 0; edgeD135 = 1;
                }    
            }
            
          if (edgeD45 && d45Neighbors > 0)
            coeff[0]->data[i] = d45Sum/(Real)d45Neighbors;
          else if (edgeD135 && d135Neighbors > 0)
            coeff[0]->data[i] = d135Sum/(Real)d135Neighbors;
          else if (nNeighbors > 0)
            coeff[0]->data[i] = localSum/(Real)nNeighbors;
        }
               
      else if (edgeV && !edgeH && vNeighbors > 0 && !BILIN_ONLY)
        coeff[0]->data[i] = vSum/(Real)vNeighbors;
      else if (edgeH && !edgeV && hNeighbors > 0 && !BILIN_ONLY)
        coeff[0]->data[i] = hSum/(Real)hNeighbors;
      else */
	  if (nNeighbors > 0)
	  {
		  coeff[0]->data[i] = localSum/(Real)nNeighbors;
		  //printf("(x,y) = (%d,%d), localSum = %e, nNeighbors = %d, coeff = %6.2f\n", 
		  //  i%LLHsize, i/LLHsize, localSum, nNeighbors, coeff[0]->data[i]);
	  }
      else
      {
        printf("No neighbors in LL subband at location %d\n",i);
        coeff[0]->data[i] = 0.0;   
      }
        
    }
  }
  
  // higher frequency subbands (3, 4, ..., nSets) missing coeffs set to zero
  for (j = 3; j < nSets; j++)
    for (i = 0; i < coeff[j]->nData; i++)
      if (coeff[j]->data[i] > (double)MISSINGVAL/2.0)
      {
        //printf("missig HF coeff no. %d in band %d\n",i,j);
        coeff[j]->data[i] = 0.0;
      }
    
  
}

/*---------------------------------------------------------------------------*/

void concealBicub(CoeffSet **coeff, int *missing, int LLsize)
{
  // Bicubic interpolation of LL coefficients
  // See "Numerical Recipes in C", pp. 125-128
  
  int i, j, k, l, LLHsize, LLVsize; // change for variable size
  //int m1, m2;
  Real xx, cl[16], x[16], c[4][4];  // for loose grid
  Real xxt, clt[16], xt[16], ct[4][4];
  Real x1, x2;
  //Real thr;   
  LLHsize = LLVsize = (int)floor(sqrt(LLsize));
  
  Real y[4], yt[4];  // 4 LL coefficients surrounding the missing one, 
                     // counterclockwise, from lower left
  Real y1[4], y2[4], y12[4], yt1[4], yt2[4], yt12[4]; 
                        // corresponding 1st and 2nd order derivatives
  
  //Real LL[LLVsize+4][LLHsize+4]; // LL subband symmetrically extended
  Real **LL = new Real* [LLVsize+4];
  for (i = 0; i < LLVsize+4; i++)
	  LL[i] = new Real [LLHsize+4];
  
  int row, col;
  
  static int wt[16][16] = {
    {  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 },
    {  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0,  0 },
    { -3,  0,  0,  3,  0,  0,  0,  0, -2,  0,  0, -1,  0,  0,  0,  0 },
    {  2,  0,  0, -2,  0,  0,  0,  0,  1,  0,  0,  1,  0,  0,  0,  0 },
    {  0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 },
    {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0 },
    {  0,  0,  0,  0, -3,  0,  0,  3,  0,  0,  0,  0, -2,  0,  0, -1 },
    {  0,  0,  0,  0,  2,  0,  0, -2,  0,  0,  0,  0,  1,  0,  0,  1 },
    { -3,  3,  0,  0, -2, -1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 },
    {  0,  0,  0,  0,  0,  0,  0,  0, -3,  3,  0,  0, -2, -1,  0,  0 },
    {  9, -9,  9, -9,  6,  3, -3, -6,  6, -6, -3,  3,  4,  2,  1,  2 },
    { -6,  6, -6,  6, -4, -2,  2,  4, -3,  3,  3, -3, -2, -1, -1, -2 },
    {  2, -2,  0,  0,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 },
    {  0,  0,  0,  0,  0,  0,  0,  0,  2, -2,  0,  0,  1,  1,  0,  0 },
    { -6,  6, -6,  6, -3, -3,  3,  3, -4,  4,  2, -2, -2, -2, -1, -1 },
    {  4, -4,  4, -4,  2,  2, -2, -2,  2, -2, -2,  2,  1,  1,  1,  1 } };    
   
     
  // create LL - symmetric extension of LL subband
  for (i=0; i<coeff[0]->nData; i++) 
    LL[(int)floor(i/LLHsize)+2][(int)i%LLHsize+2] = coeff[0]->data[i]; 
 
  // symmetric extension for corners 
  LL[0][0] = LL[3][3]; LL[0][1] = LL[2][3]; 
  LL[1][0] = LL[3][2]; LL[1][1] = LL[2][2]; 
 
  LL[0][LLHsize+2] = LL[2][LLHsize]; 
  LL[0][LLHsize+3] = LL[3][LLHsize]; 
  LL[1][LLHsize+2] = LL[2][LLHsize+1]; 
  LL[1][LLHsize+3] = LL[3][LLHsize+1]; 
 
  LL[LLVsize+2][0] = LL[LLVsize][2]; 
  LL[LLVsize+3][0] = LL[LLVsize][3]; 
  LL[LLVsize+2][1] = LL[LLVsize+1][2]; 
  LL[LLVsize+3][1] = LL[LLVsize+1][3]; 
 
  LL[LLVsize+2][LLHsize+2] = LL[LLVsize+1][LLHsize+1]; 
  LL[LLVsize+2][LLHsize+3] = LL[LLVsize][LLHsize+1]; 
  LL[LLVsize+3][LLHsize+2] = LL[LLVsize+1][LLHsize]; 
  LL[LLVsize+3][LLHsize+3] = LL[LLVsize][LLHsize]; 
 
  // symmetric extension for boundaries 
  for (i=2; i<LLVsize+2; i++) 
  { 
    LL[i][0] = LL[i][3];
    LL[i][1] = LL[i][2];
    LL[i][LLHsize+2] = LL[i][LLHsize+1];
    LL[i][LLHsize+3] = LL[i][LLHsize];
  }
  for (i=2; i<LLHsize+2; i++) 
  { 
    LL[0][i] = LL[3][i];
    LL[1][i] = LL[2][i];
    LL[LLVsize+2][i] = LL[LLVsize+1][i];
    LL[LLVsize+3][i] = LL[LLVsize][i];
  }
  

  // calculate quantities for bicubic interpolation 
  for (i=0; i<coeff[0]->nData; i++) 
    if (missing[coeff[0]->packet(0,i)-1]) 
    { 
      // missing coefficient - process its four neighbors 
      row = (int) floor(i/LLHsize) + 2; // row relative to LL 
      col = (int) i%LLHsize + 2;        // col relative to LL
      
      // loose grid, as in Hemami & Gray
      // upper left 
      y[3] = LL[row-1][col-1]; 
      // vertical derivative at [row-1][col-1] 
      y1[3] = (LL[row][col-1] - LL[row-2][col-1])/2; 
      // horizontal derivative at [row-1][col-1] 
      y2[3] = (LL[row-1][col] - LL[row-1][col-2])/2; 
      // cross derivative at [row-1][col-1] 
      y12[3] = (LL[row][col] - LL[row][col-2] - LL[row-2][col] + 
                LL[row-2][col-2])/4; 
      
      // upper right
      y[2] = LL[row-1][col+1];
      // vertical derivative at [row-1][col+1] 
      y1[2] = (LL[row][col+1] - LL[row-2][col+1])/2; 
      // horizontal derivative at [row-1][col+1] 
      y2[2] = (LL[row-1][col+2] - LL[row-1][col])/2; 
      // cross derivative at [row-1][col+1] 
      y12[2] = (LL[row][col+2] - LL[row][col] - LL[row-2][col+2] + 
                LL[row-2][col])/4;
      
      // lower right
      y[1] = LL[row+1][col+1];
      // vertical derivative at [row+1][col+1] 
      y1[1] = (LL[row+2][col+1] - LL[row][col+1])/2; 
      // horizontal derivative at [row+1][col+1] 
      y2[1] = (LL[row+1][col+2] - LL[row+1][col])/2; 
      // cross derivative at [row+1][col+1] 
      y12[1] = (LL[row+2][col+2] - LL[row+2][col] - LL[row][col+2] + 
                LL[row][col])/4;
      
      // lower left 
      y[0] = LL[row+1][col-1]; 
      // vertical derivative at [row+1][col-1] 
      y1[0] = (LL[row+2][col-1] - LL[row][col-1])/2; 
      // horizontal derivative at [row+1][col-1] 
      y2[0] = (LL[row+1][col] - LL[row+1][col-2])/2; 
      // cross derivative at [row+1][col-1] 
      y12[0] = (LL[row+2][col] - LL[row+2][col-2] - LL[row][col] + 
                LL[row][col-2])/4;
      // end of loose grid processing
      
      // start processing tight grid (rotated 45 degrees counterclockwise)
      // upper left 
      yt[3] = LL[row-1][col]; 
      // vertical derivative at [row-1][col] 
      yt1[3] = (LL[row][col-1] - LL[row-2][col+1])/2; 
      // horizontal derivative at [row-1][col] 
      yt2[3] = (LL[row][col+1] - LL[row-2][col-1])/2; 
      // cross derivative at [row-1][col] 
      yt12[3] = (LL[row][col] - LL[row-1][col-1] - LL[row-1][col+1] + 
                LL[row-2][col])/4; 
      
      // upper right
      yt[2] = LL[row][col+1];
      // vertical derivative at [row][col+1] 
      yt1[2] = (LL[row+1][col] - LL[row-1][col+2])/2; 
      // horizontal derivative at [row][col+1] 
      yt2[2] = (LL[row+1][col+2] - LL[row-1][col])/2; 
      // cross derivative at [row][col+1] 
      yt12[2] = (LL[row+1][col+1] - LL[row][col] - LL[row][col+2] + 
                LL[row-1][col+1])/4;
      
      // lower right
      yt[1] = LL[row+1][col];
      // vertical derivative at [row+1][col] 
      yt1[1] = (LL[row+2][col-1] - LL[row][col+1])/2; 
      // horizontal derivative at [row+1][col] 
      yt2[1] = (LL[row+2][col+1] - LL[row][col-1])/2; 
      // cross derivative at [row+1][col] 
      yt12[1] = (LL[row+2][col] - LL[row+1][col-1] - LL[row+1][col+1] + 
                LL[row][col])/4;
      
      // lower left 
      yt[0] = LL[row][col-1]; 
      // vertical derivative at [row][col-1] 
      yt1[0] = (LL[row+1][col-2] - LL[row-1][col])/2; 
      // horizontal derivative at [row][col-1] 
      yt2[0] = (LL[row+1][col] - LL[row-1][col-2])/2; 
      // cross derivative at [row][col-1] 
      yt12[0] = (LL[row+1][col-1] - LL[row][col-2] - LL[row][col] + 
                LL[row-1][col-1])/4;
      // end of tight grid processing
        
   
      // coefficients needed for bicubic interpolation
      // "Numerical Recipes in C", pp. 126
      // d1d2 = d1 = d2 = 1 
      
      // for loose grid
      for (j = 0; j < 4; j++)
      {
        x[j] = y[j];
        x[j+4] = y1[j];
        x[j+8] = y2[j];
        x[j+12] = y12[j];
      }
      
      for (j = 0; j <= 15; j++)
      {
        xx = 0.0;
        for (k = 0; k <= 15; k++)  
          xx += wt[j][k]*x[k];
        cl[j] = xx;
      }  
      
      l = 0;
      for (k = 0; k < 4; k++)
        for (j = 0; j < 4; j++)
          c[k][j] = cl[l++];
      
      // for tight grid
      for (j = 0; j < 4; j++)
      {
        xt[j] = yt[j];
        xt[j+4] = yt1[j];
        xt[j+8] = yt2[j];
        xt[j+12] = yt12[j];
      }
      
      for (j = 0; j <= 15; j++)
      {
        xxt = 0.0;
        for (k = 0; k <= 15; k++)  
          xxt += wt[j][k]*xt[k];
        clt[j] = xxt;
      }  
      
      l = 0;
      for (k = 0; k < 4; k++)
        for (j = 0; j < 4; j++)
          ct[k][j] = clt[l++];
      
      
    
      // Examine LH and HL subbands and find interpolation point
             
      // HL subband - HP filtered in horizontal direction - vertical edges
      
      // Daubechies 9/7
      // Look for three neighboring coeffs with magnitudes "large" wrt
      // neighbors, or two "large" ones with similar magnitudes of opposite
      // sign separated by 1 point. "Similar" here means difference in 
      // magnitude is < 20%
      // looking for a vertical edge
      
      
      // Define two thresholds, one for edge detection and one for
      // interpolating point manipulation. Edge detection threshold should
      // be lower since it is mainly used for determining whether the edge
      // is horizontal/vertical or diagonal - this info is used in deciding
      // which grid to use. 
      
      x1 = 0.5; x2 = 0.5;
      Real th = 2.0;   // threshold for interpolating point manipulation
      Real thdet = 2.0; // threshold for edge detection
      int horiz = 0;  // horizontal edge
      int vert = 0;   // vertical edge
      int HFVhoriz = 0;
      int HFVvert = 0;

      // loop until HFV does not occur both ways

      do
      {
        gridAdapt(coeff, LLHsize, LLsize, th, thdet, i, &x1, &x2, 
                  &HFVhoriz, &HFVvert, &horiz, &vert);
        
        //printf("HFVhoriz = %d, HFVvert = %d, th = %f\n", HFVhoriz, 
        //                                                 HFVvert, th);

        th = th+0.1;
        thdet = thdet+0.1;
      } 
      while ((HFVhoriz) && (HFVvert)); 
      
      //printf("HFVhoriz = %d, HFVvert = %d\n", HFVhoriz, HFVvert);
      
      if (TIGHTGRID)
      {
        Real x1t, x2t;
        
        // remap for tight grid
        x1t = 0.5 + x1 - x2;
        x2t = -0.5 + x1 + x2;
        
        // clip into the range [0,1]
        if (x1t < 0) x1t = 0;
        else if (x1t > 1) x1t = 1;
        if (x2t < 0) x2t = 0;
        else if (x2t > 1) x2t = 1;
        
        x1 = x1t;
        x2 = x2t;
      }

      
      // for check
      //x1 = 0.5; x2 = 0.5; 
      //horiz = 0; vert = 0;
      
      // if the coefficient is not near the image boundary, then interpolate
      if  (
            //((x1 < 0.5) || (x1 > 0.5) || (x2 < 0.5) || (x2 > 0.5)) &&
            //((horiz) || (vert)) &&
            (i%LLHsize > 1) && (i%LLHsize < LLHsize-2) 
            && (i > 2*LLHsize) && (i+2*LLHsize < LLsize)
          )
          {
            Real bicvalue;
            
            // if HFV in any one direction - interpolate bilin in opposite
            if ((HFVhoriz) && !(HFVvert))
            { 
                bicvalue = 0.5*coeff[0]->data[i+LLHsize] + 
                            0.5*coeff[0]->data[i-LLHsize];  
                printf("  Doing linear vertical interpolation\n");               
            }
            else if (!(HFVhoriz) && (HFVvert))
            {
                bicvalue = 0.5*coeff[0]->data[i-1] + 
                            0.5*coeff[0]->data[i+1];
                printf("  Doing linear horizontal interpolation\n");               
            }  
            else if (!(HFVhoriz) && !(HFVvert))
            //else if (!(HFVhoriz) && !(HFVvert) && (horiz || vert)) //scheme 2
            {
             if (!TIGHTGRID)
               bicvalue = bicubInt(c, x1, x2); //loose
             else
               bicvalue = bicubInt(ct, x1, x2);  //tight
               
             if (horiz || vert)
                 printf("  Doing bicubic interpolation at (%f,%f)\n",x1,x2);
            }
            
            else   // leave  bilinear
            {
              bicvalue = coeff[0]->data[i]; 
            }            
             coeff[0]->data[i] = bicvalue;
          }
    }
}

/*---------------------------------------------------------------------------*/

Real bicubInt(Real c[4][4], Real x1, Real x2)
{
  // bicubic interpolation - "Numerical Recipes in C", pp. 127
  
  int i;
  Real value = 0.0;
  
  for (i = 3; i >= 0; i--)
    value = x1*value + ((c[i][3]*x2+c[i][2])*x2+c[i][1])*x2+c[i][0];
 
  return value;
}

/*---------------------------------------------------------------------------*/

Real compare(char *imagefile1, char *imagefile2)
{
   Image *image1, *image2;


#ifdef PGM    
   image1 = new Image (imagefile1);
   image2 = new Image (imagefile2);
#else
   image1 = new Image (imagefile1, 720, 486, 0);
   image2 = new Image (imagefile2, 720, 486, 0);
#endif

   if ((image1->hsize != image2->hsize) || (image1->vsize != image2->vsize))
     error ("Unequal image sizes:  %s is size %d x %d, %s is size %d x %d\n", 
	    imagefile1, imagefile2, image1->hsize, image1->vsize, image2->hsize,
	    image2->vsize);

   printf ("comparing %s to %s (%d x %d images)\n", imagefile1, imagefile2,
	   image1->hsize, image1->vsize);


   Real mse = image1->compare_mse (image2);

   delete image1;
   delete image2;   

   Real psnr;
   
   if (mse > 0)
     psnr = 20.0 * log(255.0/sqrt(mse))/log(10.0);
   else
     psnr = 0.0; //identical images

   //return mse;
   return psnr;
}

/*---------------------------------------------------------------------------*/

void gridAdapt(CoeffSet **coeff, int LLHsize, int LLsize, Real th, Real thdet, 
               int i, Real *x1, Real *x2, int *HFVhoriz, int *HFVvert, 
               int *horiz, int *vert)

{
      *HFVvert = 0;
      *HFVhoriz = 0;

      int up = 0;  
      int pm1 = -1;
      int p = -1;
      int pp1 = -1;


      // find if edge is vertical
      // first check if window is in the same row
      // now check for HFV in horiz. dir.:  > 3 large coeffs
      if 
         (
          ((int)(i/LLHsize) == (int)((i-4)/LLHsize)) && 
          ((int)(i/LLHsize) == (int)((i+3)/LLHsize)) &&
          (    //check for 4 large ones
           (
            min(
                min(
                    min(
                        abv(coeff[1]->data[i-1]), 
                        abv(coeff[1]->data[i])
                       ),
                    abv(coeff[1]->data[i+1])
                   ),
                abv(coeff[1]->data[i-2])
               ) >
            thdet
           ) ||   //check for other 4
           (
            min(
                min(
                    min(
                        abv(coeff[1]->data[i-1]), 
                        abv(coeff[1]->data[i])
                       ),
                    abv(coeff[1]->data[i+1])
                   ),
                abv(coeff[1]->data[i+2])
               ) >
            thdet
           ) || 
           (
            min(
                min(
                    min(
                        abv(coeff[1]->data[i-1]), 
                        abv(coeff[1]->data[i])
                       ),
                    abv(coeff[1]->data[i-3])
                   ),
                abv(coeff[1]->data[i-2])
               ) >
            thdet
           ) ||  // check for 5 large ones
           (
            min(
                min(
                    min(
                        min(abv(coeff[1]->data[i-1]), 
                            abv(coeff[1]->data[i-3])
                           ),
                        abv(coeff[1]->data[i])
                       ),
                    abv(coeff[1]->data[i+1])
                   ),
                abv(coeff[1]->data[i-2])
               ) >
            thdet
           ) ||  // check for 5 large ones
           (
            min(
                min(
                    min(
                        min(abv(coeff[1]->data[i-1]), 
                            abv(coeff[1]->data[i+2])
                           ),
                        abv(coeff[1]->data[i])
                       ),
                    abv(coeff[1]->data[i+1])
                   ),
                abv(coeff[1]->data[i-2])
               ) >
            thdet 
           ) 
          )  
         )
           {
             *HFVhoriz = 1;
             //printf("horizontal HFV found at (%d,%d)\n",(int)i/LLHsize+1,i%LLHsize+1);
           }
      else if 
         (       
          ((int)(i/LLHsize) == (int)((i-4)/LLHsize)) && 
          ((int)(i/LLHsize) == (int)((i+2)/LLHsize)) &&  
          ((
            min(min(abv(coeff[1]->data[i-2]), abv(coeff[1]->data[i-1])),
                                           abv(coeff[1]->data[i])) >
            thdet   
           ) ||
           ((
             min(abv(coeff[1]->data[i-2]), abv(coeff[1]->data[i])) >
             thdet
            ) &&
             (abv(abv(coeff[1]->data[i-2])-abv(coeff[1]->data[i]))/abv(coeff[1]->data[i]) < 0.2)
         )))
           // coeffs found at i-2, i-1, i
          { 
            *vert = 1;
            //printf("vertical edge found at (%d,%d)\n",(int)i/LLHsize+1,i%LLHsize+1);
          }
      else if 
         (
          ((int)(i/LLHsize) == (int)((i-3)/LLHsize)) && 
          ((int)(i/LLHsize) == (int)((i+3)/LLHsize)) &&
          ((
            min(min(abv(coeff[1]->data[i-1]), abv(coeff[1]->data[i])),
                                              abv(coeff[1]->data[i+1])) >
            thdet
           ) ||
           ((
             min(abv(coeff[1]->data[i-1]), abv(coeff[1]->data[i+1])) >
             thdet
            ) &&
             (abv(abv(coeff[1]->data[i-1])-abv(coeff[1]->data[i+1]))/abv(coeff[1]->data[i+1]) < 0.2)
         )))
           // coeffs found at i-1, i, i+1
           { 
             *vert = 1;
             //printf("vertical edge found at (%d,%d)\n",(int)i/LLHsize+1,i%LLHsize+1);
           }
       else
           { 
             *vert = 0;
           }

      // find parameters for interpolation point calculation
      // first check if window is in the same row
      if (       
          ((int)(i/LLHsize) == (int)((i-4)/LLHsize)) && 
          ((int)(i/LLHsize) == (int)((i+2)/LLHsize)) &&  
          ((
            min(min(abv(coeff[1]->data[i-2]), abv(coeff[1]->data[i-1])),
                                           abv(coeff[1]->data[i])) >
            th
           ) ||
           ((
             min(abv(coeff[1]->data[i-2]), abv(coeff[1]->data[i])) >
             th
            ) &&
             (abv(abv(coeff[1]->data[i-2])-abv(coeff[1]->data[i]))/abv(coeff[1]->data[i]) < 0.2)
         )))
           // coeffs found at i-2, i-1, i
          { 
            pm1 = i-2; p = i-1; pp1 = i; 
          }
      else if 
         (
          ((int)(i/LLHsize) == (int)((i-3)/LLHsize)) && 
          ((int)(i/LLHsize) == (int)((i+3)/LLHsize)) &&
          ((
            min(min(abv(coeff[1]->data[i-1]), abv(coeff[1]->data[i])),
                                              abv(coeff[1]->data[i+1])) >
            th
           ) ||
           ((
             min(abv(coeff[1]->data[i-1]), abv(coeff[1]->data[i+1])) >
             th
            ) &&
             (abv(abv(coeff[1]->data[i-1])-abv(coeff[1]->data[i+1]))/abv(coeff[1]->data[i+1]) < 0.2)
         )))
           // coeffs found at i-1, i, i+1
           { 
             pm1 = i-1; p = i; pp1 = i+1; 
           }
       else
           { 
             pm1 = -1; p = -1; pp1 = -1; 
           }
       
       // find value of parameter m, then decide on interpolation point
       if (p == i-1)
       {
         // vertical "up" edge
         up = 1;
         *x1 = 0.75; //?
       }
       
       else if (p == i)
       {
         // vertical "down" edge
         *x1 = 0.25;  //?  
       }     
         
       up = 0;    
      
      
      // LH subband - HP filtered in vertical direction - horizontal edges
      
      // Daubechies 9/7
      
      // find if edge is horizontal
      // first check if the window is entirely in the column
      
      if    
        (
          (i-4*LLHsize >= 0) && (i+3*LLHsize <= LLsize) && 
          ((
            min(
                min(
                    min(
                        abv(coeff[2]->data[i-LLHsize]), 
                        abv(coeff[2]->data[i])
                       ),
                    abv(coeff[2]->data[i+LLHsize])
                   ),
                abv(coeff[2]->data[i-2*LLHsize])
               ) >
            thdet
           ) ||
           (
            min(
                min(
                    min(
                        abv(coeff[2]->data[i-LLHsize]), 
                        abv(coeff[2]->data[i])
                       ),
                    abv(coeff[2]->data[i+LLHsize])
                   ),
                abv(coeff[2]->data[i+2*LLHsize])
               ) >
            thdet
           ) || 
           (
            min(
                min(
                    min(
                        abv(coeff[2]->data[i-LLHsize]), 
                        abv(coeff[2]->data[i])
                       ),
                    abv(coeff[2]->data[i-3*LLHsize])
                   ),
                abv(coeff[2]->data[i-2*LLHsize])
               ) >
            thdet
           ) ||  // check for 5 large ones
           (
            min(
                min(
                    min(
                        min(abv(coeff[2]->data[i-LLHsize]), 
                            abv(coeff[2]->data[i-3*LLHsize])
                           ),
                        abv(coeff[2]->data[i])
                       ),
                    abv(coeff[2]->data[i+LLHsize])
                   ),
                abv(coeff[2]->data[i-2])
               ) >
            thdet
           ) ||  // check for 5 large ones
           (
            min(
                min(
                    min(
                        min(abv(coeff[2]->data[i-LLHsize]), 
                            abv(coeff[2]->data[i+2*LLHsize])
                           ),
                        abv(coeff[2]->data[i])
                       ),
                    abv(coeff[2]->data[i+LLHsize])
                   ),
                abv(coeff[2]->data[i-2*LLHsize])
               ) >
            thdet
           )
          )  
         )
           {
             *HFVvert = 1; 
             //printf("vertical HFV found at (%d,%d)\n",(int)i/LLHsize+1,i%LLHsize+1);
           }
      else if 
         (        
          (i-4*LLHsize >= 0) && (i+2*LLHsize <= LLsize) &&
          ((
            min(min(abv(coeff[2]->data[i-2*LLHsize]), abv(coeff[2]->data[i-LLHsize])),
                                           abv(coeff[2]->data[i])) >
            thdet
           ) ||
           ((
             min(abv(coeff[2]->data[i-2*LLHsize]), abv(coeff[2]->data[i])) >
             thdet
            ) &&
             (abv(abv(coeff[2]->data[i-2*LLHsize])-abv(coeff[2]->data[i]))/abv(coeff[2]->data[i]) < 0.2)
         )))
           // coeffs found at i-2*LLHsize, i-LLHsize, i
           { 
             *horiz = 1;
             //printf("horizontal edge found at (%d,%d)\n",(int)i/LLHsize+1,i%LLHsize+1);
           }
      else if 
         (
          (i-3*LLHsize >= 0) && (i+3*LLHsize <= LLsize) &&
          ((
            min(min(abv(coeff[2]->data[i-LLHsize]), abv(coeff[2]->data[i])),
                                              abv(coeff[2]->data[i+LLHsize])) >
            thdet
           ) ||
           ((
             min(abv(coeff[2]->data[i-LLHsize]), abv(coeff[2]->data[i+LLHsize])) >
             thdet
            ) &&
             (abv(abv(coeff[2]->data[i-LLHsize])-abv(coeff[2]->data[i+LLHsize]))/abv(coeff[2]->data[i+LLHsize]) < 0.2)
         )))
           // coeffs found at i-LLHsize, i, i+LLHsize
          { 
            *horiz = 1;
            //printf("horizontal edge found at (%d,%d)\n",(int)i/LLHsize+1,i%LLHsize+1);
          }
          
       else
          { 
            *horiz = 0;
          }
      
      
      // find parameters for interpolation point calculation
      // first check if the window is entirely in the column
      if (        
          (i-4*LLHsize >= 0) && (i+2*LLHsize <= LLsize) &&
          ((
            min(min(abv(coeff[2]->data[i-2*LLHsize]), abv(coeff[2]->data[i-LLHsize])),
                                           abv(coeff[2]->data[i])) >
            th
           ) ||
           ((
             min(abv(coeff[2]->data[i-2*LLHsize]), abv(coeff[2]->data[i])) >
             th
            ) &&
             (abv(abv(coeff[2]->data[i-2*LLHsize])-abv(coeff[2]->data[i]))/abv(coeff[2]->data[i]) < 0.2)
         )))
           // coeffs found at i-2*LLHsize, i-LLHsize, i
           { 
             pm1 = i-2*LLHsize; p = i-LLHsize; pp1 = i; 
           }
      else if 
         (
          (i-3*LLHsize >= 0) && (i+3*LLHsize <= LLsize) &&
          ((
            min(min(abv(coeff[2]->data[i-LLHsize]), abv(coeff[2]->data[i])),
                                              abv(coeff[2]->data[i+LLHsize])) >
            th
           ) ||
           ((
             min(abv(coeff[2]->data[i-LLHsize]), abv(coeff[2]->data[i+LLHsize])) >
             th
            ) &&
             (abv(abv(coeff[2]->data[i-LLHsize])-abv(coeff[2]->data[i+LLHsize]))/abv(coeff[2]->data[i+LLHsize]) < 0.2)
         )))
           // coeffs found at i-LLHsize, i, i+LLHsize
          { 
            pm1 = i-LLHsize; p = i; pp1 = i+LLHsize; 
          }
       else
          { 
            pm1 = -1; p = -1; pp1 = -1; 
          }
       
       // find value of parameter m, then decide on interpolation point
       if (p == i-LLHsize)
       {
         // horizontal "up" edge
         up = 1;
         *x2 = 0.75;  //?
       }
       
       else if (p == i)
       {
         // horizontal "down" edge
         *x2 = 0.25;  //?
       }     
       
       //printf("Threshold increased\n");

}


/*---------------------------------------------------------------------------*/

void my_delay (int n)
{
  time_t t1, t2;
  
  t1 = time(NULL);
  t2 = time(NULL);
  
  //wait n seconds
  while (difftime(t1,t2) < n  &&  difftime(t2,t1) < n)
    t2 = time(NULL); 
}

/*---------------------------------------------------------------------------*/

void concealMAP(CoeffSet **coeff, int *missing, int hor, int ver, int nSets)
{
  // MAP and adaptive MAP concealment
  
  int nNeighbors, i, j, k, x, y, lenx, leny, pos;
  float nbhd[12], gam;
  double weight[4]; 
  int available[12];
  
  const int Nlevels = (int) (nSets-1)/3; // number of decomposition levels

  int Nestimates;
  const int treesize = (int) pow(2.0, (double)Nlevels);
  double **tree;
  double ah, av, ad, wh, wv, wd;
  double thr1, thr2, gamLL, gamHF;
  div_t q;
  int **miss, scale;
  int iter, Niter;

  /* IVB 2004/04/09 --------------------------------------------- */
  FILE *MAP_param_file;
  char MAP_param_file_name[100];
  
  // open file with MAP parameters: Niter, gamLL, gamHF, thr1, thr2
  sprintf(MAP_param_file_name, "MAP_parameters.txt");

  if ((MAP_param_file = fopen(MAP_param_file_name, "r")) == NULL)
    error ("Unable to open file %s", MAP_param_file_name);

  fscanf(MAP_param_file,"%d %lf %lf %lf %lf", &Niter, &gamLL, &gamHF, &thr1, &thr2);
  
  fclose(MAP_param_file);

  //printf("%lf %lf %lf %lf \n", gamLL, gamHF, thr1, thr2);
  /* IVB 2004/04/09 ----------------------- end ----------------- */
  
  //thr1 = 0.5; thr2 = 3.0;  // original values form bimc3d_new
  //thr1 = 0.001; thr2 = 100.0;  // 2004/04/09 read above

  // initialize tree structure
  tree = (double**) calloc(treesize, sizeof(double*));
  for (i = 0; i<treesize; i++)
    tree[i] = (double*) calloc(treesize, sizeof(double));
    
  for (i = 0; i < treesize; i++)
    for (k = 0; k < treesize; k++)
      tree[i][k] = 0.0;

  // reserve memory for miss[][]
  miss = (int**) calloc(nSets, sizeof(int*));
  for (i = 0; i < nSets; i++)
  {
	  if (i == 0)
		  scale = (int) (nSets-1)/3;
	  else
		  scale = (int) (nSets-i-1)/3 + 1;

	  lenx = (int) (hor/(int)pow(2.0, (double)scale));
	  leny = (int) (ver/(int)pow(2.0, (double)scale));

	  miss[i] = (int*) calloc(lenx*leny, sizeof(int));
  }

  // MAP concealment, high frequency bands first
  for (i = nSets-1; i >= 0; i--)
  {	  
	  // subband dimensions
	  if (i == 0)
		  scale = (int) (nSets-1)/3;
	  else
		  scale = (int) (nSets-i-1)/3 + 1;

	  lenx = (int) (hor/(int)pow(2.0, (double)scale));
	  leny = (int) (ver/(int)pow(2.0, (double)scale));

	  // initialzie miss[i][]
	  for (j = 0; j < lenx*leny; j++)
		  miss[i][j] = 0;

	  // gamma
	  if (i == 0)
		  //gam = 500.0;  // original value from bimc3d_new
		  //gam = 150.0;
		  gam = gamLL;
	  else
		  //gam = 1.0; // original value from bimc3d_new
		  //gam = 1.0;
		  gam = gamHF;

	  // iterate
	  for (iter = 0; iter < Niter; iter++)
	  {
		  //printf("subband %d, iteration %d\n", i, iter);

		  // search the subband for missing coefficients
		  for (y = 0; y < leny; y++)
		  {
			  for (x = 0; x < lenx; x++)
			  {
				  pos = y*lenx + x;

				  if (coeff[i]->data[pos] > (double)MISSINGVAL/2.0 || miss[i][pos])
				  {
					  // 1) This sample is missing.
					  // 2) Extract neighborhood.
					  // 3) Find MAP estimate.

					  miss[i][pos] = 1;

					  if (i == 0)
					  {
						  // LL band
						  // infer edge behavior through decay estimates
						  // then set weights appropriately

						  ah = 0.0; av = 0.0; ad = 0.0;
              
						  for (j = 0; j < treesize; j++)
							  for (k = 0; k < treesize; k++)
								  tree[j][k] = (double) MISSINGVAL;
              
						  //printf("(%d,%d): gettree... ",x,y); 
						  gettree2d(tree,coeff,x,y,hor,ver,treesize,Nlevels,nSets);

						  //printf("est_decay... "); 
						  est_decay(&ah, &av, &ad, tree, treesize, Nlevels, nSets);


						  
						  //printf("done\n");
						  //printf("ah = %6.2f, av = %6.2f, ad = %6.2f\n", ah, av, ad);
						  
              
						  Nestimates = 3;

						  if (ah < 0.0 || ah > thr2) 
						  {
							  wh = 1.0/thr2;           
							  Nestimates--;
						  }
						  else if (ah < thr1)
						  {
							  wh = 1.0/thr1;           
							  Nestimates--;
						  }
						  else 
						  {
							  wh = 1.0/thr1 - (ah-thr1)/(thr1*thr2);
						  }
						  
						  if (av < 0.0 || av > thr2) 
						  {
							  wv = 1.0/thr2; 
							  Nestimates--;
						  }
						  else if (av < thr1)
						  {
							  wv = 1.0/thr1; 
							  Nestimates--;
						  }
						  else 
						  {
							  wv = 1.0/thr1 - (av-thr1)/(thr1*thr2);
						  }                        
				
						  if (ad < 0.0 || ad > thr2) 
						  {
							  wd = 1.0/thr2;
							  Nestimates--;
						  }
						  else if (ad < thr1)
						  {
							  wd = 1.0/thr1;
							  Nestimates--;
						  }
						  else 
						  {
							  wd = 1.0/thr1 - (ad-thr1)/(thr1*thr2);
						  }
              
						  // wv = wh = wd = 1.0;  /* nonadaptive MAP */
                
						  weight[0] = wv;  /* vertical detail - horizontal edge */
						  weight[1] = wd;  /* diagonal 45 degrees */
						  weight[2] = wh;  /* horizontal detail - vertical edge */
						  weight[3] = wd;  /* diagonal 135 degrees */
              
					  } // if LL subband (i==0)
          
					  nNeighbors = 0;
					  for (j = 0; j < 12; j++)
					  {
						  available[j] = 0;
						  nbhd[j] = 0.0;
					  }
          
					  /* 2) extract neighborhood */
					  if (in_band(x-1,y-1,lenx,leny))
					  {
						  nbhd[0] = coeff[i]->data[(y-1)*lenx+x-1];
						  if (coeff[i]->data[(y-1)*lenx+x-1] < (double)MISSINGVAL/2.0)
						  {
							  available[0] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x,y-1,lenx,leny))
					  {
						  nbhd[1] = coeff[i]->data[(y-1)*lenx+x];
						  if (coeff[i]->data[(y-1)*lenx+x] < (double)MISSINGVAL/2.0)
						  {
							  available[1] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x+1,y-1,lenx,leny))
					  {
						  nbhd[2] = coeff[i]->data[(y-1)*lenx+x+1];
						  if (coeff[i]->data[(y-1)*lenx+x+1] < (double)MISSINGVAL/2.0)
						  {
							  available[2] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x+2,y-1,lenx,leny))
					  {
						  nbhd[3] = coeff[i]->data[(y-1)*lenx+x+2];
						  if (coeff[i]->data[(y-1)*lenx+x+2] < (double)MISSINGVAL/2.0)
						  {
							  available[3] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x-2,y,lenx,leny))
					  {
						  nbhd[4] = coeff[i]->data[(y)*lenx+x-2];
						  if (coeff[i]->data[(y)*lenx+x-2] < (double)MISSINGVAL/2.0)
						  {
							  available[4] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x-1,y,lenx,leny))
					  {
						  nbhd[5] = coeff[i]->data[(y)*lenx+x-1];
						  if (coeff[i]->data[(y)*lenx+x-1] < (double)MISSINGVAL/2.0)
						  {
							  available[5] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x+1,y,lenx,leny))
					  {
						  nbhd[6] = coeff[i]->data[(y)*lenx+x+1];
						  if (coeff[i]->data[(y)*lenx+x+1] < (double)MISSINGVAL/2.0)
						  {
							  available[6] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x+2,y,lenx,leny))
					  {
						  nbhd[7] = coeff[i]->data[(y)*lenx+x+2];
						  if (coeff[i]->data[(y)*lenx+x+2] < (double)MISSINGVAL/2.0)
						  {
							  available[7] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x-2,y+1,lenx,leny))
					  {
						  nbhd[8] = coeff[i]->data[(y+1)*lenx+x-2];
						  if (coeff[i]->data[(y+1)*lenx+x-2] < (double)MISSINGVAL/2.0)
						  {
							  available[8] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x-1,y+1,lenx,leny))
					  {
						  nbhd[9] = coeff[i]->data[(y+1)*lenx+x-1];
						  if (coeff[i]->data[(y+1)*lenx+x-1] < (double)MISSINGVAL/2.0)
						  {
							  available[9] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x,y+1,lenx,leny))
					  {
						  nbhd[10] = coeff[i]->data[(y+1)*lenx+x];
						  if (coeff[i]->data[(y+1)*lenx+x] < (double)MISSINGVAL/2.0)
						  {
							  available[10] = 1;
							  nNeighbors++;
						  }
					  }
					  if (in_band(x+1,y+1,lenx,leny))
					  {
						  nbhd[11] = coeff[i]->data[(y+1)*lenx+x+1];
						  if (coeff[i]->data[(y+1)*lenx+x+1] < (double)MISSINGVAL/2.0)
						  {
							  available[11] = 1;
							  nNeighbors++;
						  }
					  }

					  //printf("band = %d, location = (%d,%d), nNeighbors = %d\n",i,x,y,nNeighbors);


					  /* 3) find MAP estimate */
          
					  // gam = 1.0;   /* nonadaptive MAP */
          
					  if (nNeighbors > 0 && i==0) 
					  {
						  /* LL band */
            
						  /* if strong diagonal edge (both hor and ver), check direction */
						  if (wd > 1.0)
						  {
							  if (available[0] && available[2] && available[9] && available[11])
							  {
								  if ( fabs(nbhd[0]-nbhd[11]) < fabs(nbhd[2]-nbhd[9]) )
								  {
									  /* 135 degree edge */
									  // printf("135 deg. diagonal!!!!!!!!!!!!!!!!!!\n"); 
									  weight[3] = 2.0;
									  weight[1] = 0.5;
								  }
								  else
								  {
									  /* 45 degree edge */
									  // printf("45 deg. diagonal!!!!!!!!!!!!!!!!!!\n"); 
									  weight[3] = 0.5;
									  weight[1] = 2.0;
								  }
							  }
						  }
            
						  coeff[i]->data[pos] = map_est1(nbhd,available,weight, &gam);
            
						  // printf("subband[%d][%d] = %6.2f\n",x,y,coeff[i]->data[pos]); 
              
					  }  
					  else if (nNeighbors > 0)
					  {  
            
						  /* HF bands */
            
						  for (j = 0; j < 4; j++)
							  weight[j] = 1.0;
              
						  q = div(i,3);
						  if (q.rem == 1)
						  {
							  /* horizontal detail - vertical edges */
							  //weight[2] = 2.0; // original value from bimc3d_new
							  weight[2] = 3.0;
						  }
						  else if (q.rem == 2)
						  {
							  /* vertical detail - horizontal edges */
							  //weight[0] = 2.0; // original value from bimc3d_new
							  weight[0] = 3.0;
						  }
            
						  /* nonadaptive MAP */
						  /*
						  weight[0] = 1.0;
						  weight[2] = 1.0;  
						  */

						  coeff[i]->data[pos] = map_est1(nbhd,available,weight, &gam);
            
						  // printf("subband[%d][%d] = %6.2f\n",x,y,coeff[i]->data[pos]);
            
					  }
					  else 
					  {
						  /* no neighbors, high-frequency subband */
						  coeff[i]->data[pos] = 0.0;
					  }


				  } // if missing
			  } // x
		  } // y
	  } // iter
  } // subband i

  

  // free memory
  for (i = 0; i < nSets; i++)
	free (miss[i]);
  free(miss);

  for (i = 0; i<treesize; i++)
    free(tree[i]);
  free(tree);

}

/*****************************************************************************/
/*                           gettree2d()                                     */
/*****************************************************************************/
void gettree2d(double **tree, CoeffSet **coeff, int xLL, int yLL, int hor, int ver, int treesize, int Nlevels, int Nbands)
{
  int x, y, x1, y1, x2, y2, xt, yt, lenx, leny, lev, scale, dimscale;
  
  /* extract LL coefficient */
  dimscale = (int) (Nbands-1)/3;
  lenx = (int) (hor/(int)pow(2.0, (double)dimscale));
  leny = (int) (ver/(int)pow(2.0, (double)dimscale));
  
  tree[0][0] = (double) coeff[0]->data[yLL*lenx+xLL];
  
  for (lev = 0; lev < Nlevels; lev++)
  {
    //  printf("gettree2d(): level = %d\n",lev);
    
    /* lev 0 is the lowest decomposition level, where LL band is */
    
    /* scaling factor to get the size of the block of coefficients which */
    /* corresponds to one LL coefficient */
    scale = (int) pow(2.0, (double) lev);  // different from above dimscale
	dimscale  = (int) pow(2.0, (double)(Nlevels - lev));

	// find dimensions of the subband
	//lenx = (int) (hor/(int)pow(2.0, (double)dimscale));
    //leny = (int) (ver/(int)pow(2.0, (double)dimscale));
	lenx = (int) (hor/dimscale);
    leny = (int) (ver/dimscale);

	
    
    /* --- horizontal detail coefficients --- */
    /* (x1,y1) upper left corner of the block, within the subband */
    x1 = scale*xLL; y1 = scale*yLL;
    /* (x2,y2) lower right corner of the block, within the subband */
    x2 = x1+scale-1; y2 = y1+scale-1;
    /* (xt,yt) upper left corner of the block in the tree */
    xt = scale; yt = 0;  /* for horizontal detail coeffs */
    /* copy the tree */
    for (y = y1; y <= y2; y++)
    {
      for (x = x1; x <= x2; x++)
      {
        if (in_band(x, y, lenx, leny))
        {
          //tree[yt+y-y1][xt+x-x1] = (double) subband[y*hor+x];
		  tree[yt+y-y1][xt+x-x1] = (double) coeff[3*lev+1]->data[y*lenx+x];
        }
        else
        {
          /* part of the tree is outside frame boundary */
          tree[yt+y-y1][xt+x-x1] = (double) MISSINGVAL;
        }
      }
    }

     
    /* --- vertical detail coefficients --- */
    /* (x1,y1) upper left corner of the block, within the subband */
    x1 = scale*xLL; y1 = scale*yLL;
    /* (x2,y2) lower right corner of the block, within the subband */
    x2 = x1+scale-1; y2 = y1+scale-1;
    /* (xt,yt) upper left corner of the block in the tree */
    xt = 0; yt = scale;  /* for vertical detail coeffs */
    /* copy the tree */
    for (y = y1; y <= y2; y++)
    {
      for (x = x1; x <= x2; x++)
      {
        if (in_band(x, y, lenx, leny))
        {
          // tree[yt+y-y1][xt+x-x1] = (double) subband[y*hor+x];
		  tree[yt+y-y1][xt+x-x1] = (double) coeff[3*lev+2]->data[y*lenx+x];
        }
        else
        {
          /* part of the tree is outside frame boundary */
          tree[yt+y-y1][xt+x-x1] = (double) MISSINGVAL;
        }
      }
    }
    
    /* --- diagonal detail coefficients --- */
    /* (x1,y1) upper left corner of the block, within the subband */
    x1 = scale*xLL; y1 = scale*yLL;
    /* (x2,y2) lower right corner of the block, within the subband */
    x2 = x1+scale-1; y2 = y1+scale-1;
    /* (xt,yt) upper left corner of the block in the tree */
    xt = scale; yt = scale;  /* for diagonal detail coeffs */
    /* copy the tree */
    for (y = y1; y <= y2; y++)
    {
      for (x = x1; x <= x2; x++)
      {
        if (in_band(x, y, lenx, leny))
        {
          // tree[yt+y-y1][xt+x-x1] = (double) subband[y*hor+x];
		  tree[yt+y-y1][xt+x-x1] = (double) coeff[3*lev+3]->data[y*lenx+x];
        }
        else
        {
          /* part of the tree is outside frame boundary */
          tree[yt+y-y1][xt+x-x1] = (double) MISSINGVAL;
        }
      }
    }
    
  } /* lev */    
    
}

/*****************************************************************************/
/*                           est_decay()                                     */
/*****************************************************************************/
void est_decay(double *ah, double *av, double *ad, double **tree, int treesize, int Nlevels, int Nbands)
{
  int i, j, x, y, xt, yt, lev, Nh, Nv, Nd, Navail, scale;
  int *ih, *iv, *id; 
  double *yh, *yv, *yd; 
  double sumabs, maxabs, Ch, Cv, Cd, b1, b0, y_mean, x_mean, sum1, sum2;

  ih = (int*) calloc(Nlevels+1, sizeof(int));
  iv = (int*) calloc(Nlevels+1, sizeof(int));
  id = (int*) calloc(Nlevels+1, sizeof(int));
  yh = (double*) calloc(Nlevels+1, sizeof(double));
  yv = (double*) calloc(Nlevels+1, sizeof(double));
  yd = (double*) calloc(Nlevels+1, sizeof(double));
  
  Nh = 0; Nv = 0; Nd = 0;
  for (i = 0; i <= Nlevels; i++)
  {
    ih[i] = 0;
    iv[i] = 0;
    id[i] = 0;
    yh[i] = 0.0;
    yv[i] = 0.0;
    yd[i] = 0.0;
  }
  
  /*
  printf("est_decay(): LL coeff\n");
  */
  
  /* extract LL band coefficient if available */
  if (tree[0][0] < (double)MISSINGVAL/2.0)
  {
    yh[Nh] = (double) log(fabs(tree[0][0]))/log(2.0);
    yv[Nv] = (double) log(fabs(tree[0][0]))/log(2.0);
    yd[Nv] = (double) log(fabs(tree[0][0]))/log(2.0);
    Nh++; Nv++; Nd++;
    ih[0] = 1; iv[0] = 1; id[0] = 1;
  }  
  for (lev = 0; lev < Nlevels; lev++)
  {
    
    scale = (int) pow(2.0, (double) lev);

	//printf("est_decay(): lev = %d, scale = %d\n", lev, scale);
    
    /* --- horizontal detail --- */
    xt = scale; yt = 0;
    Navail = 0; sumabs = 0.0; maxabs = 0.0;
    ih[lev+1] = 0;
    for (y = yt; y <= yt+scale-1; y++)
    {
      for (x = xt; x <= xt+scale-1; x++)
      {
        if (fabs(tree[y][x]) > 0.0 && tree[y][x] < (double)MISSINGVAL/2.0)
        {
          /* coefficient available and safe to take log */
          ih[lev+1] = 1;   /* some coeffs available at this level */
          Navail++;
          sumabs += fabs(tree[y][x]);
          if (fabs(tree[y][x]) > maxabs)
            maxabs = fabs(tree[y][x]);
        }
      }
    }
    //printf("horizontal detail max abs. value = %e\n", maxabs); 
    
    /* if some coeffs available, store log2 of their mean absolute value */
    /* 19-10-01: second option - store log2 of max absolute value */
    if (ih[lev+1])
    {
      /* yh[Nh] = (double) log(sumabs/((double) Navail))/log(2.0);  */
      yh[Nh] = log(maxabs)/log(2.0); 
      /* printf("horizontal log2(MAV) = %6.3f\n", yh[Nh]); */
      Nh++;
    }
    
    /* --- vertical detail --- */
    xt = 0; yt = scale;
    Navail = 0; sumabs = 0.0; maxabs = 0.0;
    iv[lev+1] = 0;
    for (y = yt; y <= yt+scale-1; y++)
    {
      for (x = xt; x <= xt+scale-1; x++)
      {
        if (fabs(tree[y][x]) > 0.0 && tree[y][x] < (double)MISSINGVAL/2.0)
        {
          /* coefficient available and safe to take log */
          iv[lev+1] = 1;   /* some coeffs available at this level */
          Navail++;
          sumabs += fabs(tree[y][x]);
          if (fabs(tree[y][x]) > maxabs)
            maxabs = fabs(tree[y][x]);
        }
      }
    }
    //printf("vertical detail max abs. value = %e\n", maxabs); 
    
    /* if some coeffs available, store log2 of their mean absolute value */
    /* 19-10-01: second option - store log2 of max absolute value */
    if (iv[lev+1])
    {
      /* yv[Nv] = (double) log(sumabs/((double) Navail))/log(2.0);  */
      yv[Nv] = log(maxabs)/log(2.0); 
      /* printf("vertical log2(MAV) = %6.3f\n", yv[Nv]); */
      Nv++;
    }
    
    /* --- diagonal detail --- */
    xt = scale; yt = scale;
    Navail = 0; sumabs = 0.0; maxabs = 0.0;
    id[lev+1] = 0;
    for (y = yt; y <= yt+scale-1; y++)
    {
      for (x = xt; x <= xt+scale-1; x++)
      {
        if (fabs(tree[y][x]) > 0.0 && tree[y][x] < (double)MISSINGVAL/2.0)
        {
          /* coefficient available and safe to take log */
          id[lev+1] = 1;   /* some coeffs available at this level */
          Navail++;
          sumabs += fabs(tree[y][x]);
          if (fabs(tree[y][x]) > maxabs)
            maxabs = fabs(tree[y][x]);
        }
      }
    }
    //printf("diagonal detail max abs. value = %e\n", maxabs); 
    
    /* if some coeffs available, store log2 of their mean absolute value */
    /* 19-10-01: second option - store log2 of max absolute value */
    if (id[lev+1])
    {
      /* yd[Nd] = (double) log(sumabs/((double) Navail))/log(2.0);  */
      yd[Nd] = log(maxabs)/log(2.0);
      /* printf("diagonal log2(MAV) = %6.3f\n", yd[Nd]); */
      Nd++;
    }
      
  } /* lev */
  
  /* now all parameters have been extracted */
  /* fit the linear least squares to yh, yv, yd */
  
  /* printf("Nh = %d, Nv = %d, Nd = %d\n", Nh, Nv, Nd); */
  
  /* ------ horizontal detail ------ */
  if (Nh > 1)
  {
    /* more than one point available on the curve -> can fit a line to it */
    sumabs = 0.0;
    for (i = 0; i < Nh; i++)
    {
      sumabs += yh[i];
    }
    y_mean = sumabs/((double) Nh);
    
    sumabs = 0.0;
    for (i = 0; i <= Nlevels; i++)
    {
      if (ih[i])
      {
        sumabs += ((double) i);
      }
    }
    x_mean = sumabs/((double) Nh);
    
    sum1 = 0.0; sum2 = 0.0;
    j = 0;
    for (i = 0; i <= Nlevels; i++)
    {
      if (ih[i])
      {
        sum1 += (((double) i) - x_mean)*(yh[j] - y_mean);
        sum2 += (((double) i) - x_mean)*(((double) i) - x_mean);
        j++;
      }
    }
    if (fabs(sum2) > 0.0)
    {
      b1 = sum1/sum2;
      b0 = y_mean - b1*x_mean;
      Ch = pow(2.0, b0);
      *ah =  -0.5-b1; 
    }
    else
     *ah = (double)MISSINGVAL;
      
  }
  else
  {
    *ah = (double)MISSINGVAL;
  }
  
  /* ------ vertical detail ------ */
  if (Nv > 1)
  {
    /* more than one point available on the curve -> can fit a line to it */
    sumabs = 0.0;
    for (i = 0; i < Nv; i++)
    {
      sumabs += yv[i];
    }
    y_mean = sumabs/((double) Nv);
    
    sumabs = 0.0;
    for (i = 0; i <= Nlevels; i++)
    {
      if (iv[i])
      {
        sumabs += ((double) i);
      }
    }
    x_mean = sumabs/((double) Nv);
    
    sum1 = 0.0; sum2 = 0.0;
    j = 0;
    for (i = 0; i <= Nlevels; i++)
    {
      if (iv[i])
      {
        sum1 += (((double) i) - x_mean)*(yv[j] - y_mean);
        sum2 += (((double) i) - x_mean)*(((double) i) - x_mean);
        j++;
      }
    }
    if (fabs(sum2) > 0.0)
    {
      b1 = sum1/sum2;
      b0 = y_mean - b1*x_mean;
      Cv = pow(2.0, b0);
      *av = -0.5-b1;   
    }
    else
      *av = (double)MISSINGVAL;
  }
  else
  {
    *av = (double)MISSINGVAL;
  }
  
  /* ------ diagonal detail ------ */
  if (Nd > 1)
  {
    /* more than one point available on the curve -> can fit a line to it */
    sumabs = 0.0;
    for (i = 0; i < Nd; i++)
    {
      sumabs += yd[i];
    }
    y_mean = sumabs/((double) Nd);
    
    sumabs = 0.0;
    for (i = 0; i <= Nlevels; i++)
    {
      if (id[i])
      {
        sumabs += ((double) i);
      }
    }
    x_mean = sumabs/((double) Nd);
    
    sum1 = 0.0; sum2 = 0.0;
    j = 0;
    for (i = 0; i <= Nlevels; i++)
    {
      if (id[i])
      {
        sum1 += (((double) i) - x_mean)*(yd[j] - y_mean);
        sum2 += (((double) i) - x_mean)*(((double) i) - x_mean);
        j++;
      }
    }
    if (fabs(sum2) > 0.0)
    {
      b1 = sum1/sum2;
      b0 = y_mean - b1*x_mean;
      Cd = pow(2.0, b0);
      *ad = -0.5-b1;   
    }
    else
      *ad = (double)MISSINGVAL;
  }
  else
  {
    *ad = (double)MISSINGVAL;
  }

  // free memory
  free(ih); free(iv); free(id);
  free(yh); free(yv); free(yd);
  
}

/*****************************************************************************/
/*                           testtree()                                      */
/*****************************************************************************/
void testtree(CoeffSet **coeff)
{
  int i, j, x, y;
  int treesize = 4;
  double **tree;
    
  // initialize tree structure
  tree = (double**) calloc(treesize, sizeof(double*));
  for (i = 0; i<treesize; i++)
    tree[i] = (double*) calloc(treesize, sizeof(double));
  
  
  coeff[0]->data[0] = 1.0; coeff[0]->data[1] = 2.0;
  coeff[0]->data[2] = 3.0; coeff[0]->data[3] = 4.0;

  coeff[1]->data[0] = 1.0; coeff[1]->data[1] = 2.0;
  coeff[1]->data[2] = 3.0; coeff[1]->data[3] = 4.0;

  coeff[2]->data[0] = 1.0; coeff[2]->data[1] = 2.0;
  coeff[2]->data[2] = 3.0; coeff[2]->data[3] = 4.0;

  coeff[3]->data[0] = 1.0; coeff[3]->data[1] = 2.0;
  coeff[3]->data[2] = 3.0; coeff[3]->data[3] = 4.0;

  coeff[4]->data[0]  = 1.0; coeff[4]->data[1]  = 1.0;
  coeff[4]->data[2]  = 2.0; coeff[4]->data[3]  = 2.0;
  coeff[4]->data[4]  = 1.0; coeff[4]->data[5]  = 1.0;
  coeff[4]->data[6]  = 2.0; coeff[4]->data[7]  = 2.0;
  coeff[4]->data[8]  = 3.0; coeff[4]->data[9]  = 3.0;
  coeff[4]->data[10] = 4.0; coeff[4]->data[11] = 4.0;
  coeff[4]->data[12] = 3.0; coeff[4]->data[13] = 3.0;
  coeff[4]->data[14] = 4.0; coeff[4]->data[15] = 4.0;

  coeff[5]->data[0]  = 1.0; coeff[5]->data[1]  = 1.0;
  coeff[5]->data[2]  = 2.0; coeff[5]->data[3]  = 2.0;
  coeff[5]->data[4]  = 1.0; coeff[5]->data[5]  = 1.0;
  coeff[5]->data[6]  = 2.0; coeff[5]->data[7]  = 2.0;
  coeff[5]->data[8]  = 3.0; coeff[5]->data[9]  = 3.0;
  coeff[5]->data[10] = 4.0; coeff[5]->data[11] = 4.0;
  coeff[5]->data[12] = 3.0; coeff[5]->data[13] = 3.0;
  coeff[5]->data[14] = 4.0; coeff[5]->data[15] = 4.0;

  coeff[6]->data[0]  = 1.0; coeff[6]->data[1]  = 1.0;
  coeff[6]->data[2]  = 2.0; coeff[6]->data[3]  = 2.0;
  coeff[6]->data[4]  = 1.0; coeff[6]->data[5]  = 1.0;
  coeff[6]->data[6]  = 2.0; coeff[6]->data[7]  = 2.0;
  coeff[6]->data[8]  = 3.0; coeff[6]->data[9]  = 3.0;
  coeff[6]->data[10] = 4.0; coeff[6]->data[11] = 4.0;
  coeff[6]->data[12] = 3.0; coeff[6]->data[13] = 3.0;
  coeff[6]->data[14] = 4.0; coeff[6]->data[15] = 4.0;



  for (y = 0; y < 2; y++)
  {
    for (x = 0; x < 2; x++)
    {
      gettree2d(tree,coeff,x,y,8,8,treesize,2,7);
  
      for(j = 0; j < treesize; j++)
      {
        for (i = 0; i < treesize; i++)
        {
          printf("%3.1f ",tree[j][i]);
        }
        printf("\n");
      }
      printf("\n");
    }
  }
  
  for (i = 0; i<treesize; i++)
    free(tree[i]); 
  free(tree);
}
