/* Last edited on 2009-12-14 18:15:03 by stolfilocal */

#include <estimaDistancia_b.h>
#include <estimaDistancia_EURGB.h>
#include <estimaDistancia_EUGRADYUV.h>
#include <geraYuvGrad.h>
#include <operacoes_b.h>
#include <assert.h>


#define True 1
#define False 0

float calcula_dist_Monoescala( 
    float_image_t *A,
    float_image_t *B,
    int bgZero,
    QDIST_t qualDist,
    int *ndistExP
  )
{
  if (qualDist == QDIST_EURGB)
    { return calcula_dist_EURGB(A, B, bgZero, ndistExP); }
  else if (qualDist == QDIST_EUGRADYUV)
    { return calcula_dist_EUGRADYUV(A, B, bgZero, ndistExP); }
  else if (qualDist == QDIST_EUGRAD)
    { return calcula_dist_EUGRAD(A, B, bgZero, ndistExP); }
  else
    { demand(FALSE, "distancia não implementada"); }

}

void acumula_dist_Multiescala
  (
    float_image_t *A,
    float_image_t *B,
    int bgZero,
    QDIST_t qualDist,
    float *distAcc,
    double lambda,
    int *ndistExP
  )
{
  float distRes = calcula_dist_Monoescala(A, B, bgZero, qualDist, ndistExP);
  (*distAcc) += lambda*distRes;
}
        
void estima_dist_Monoescala( 
    float_image_t *ALo, 
    float_image_t *AHi,
    float_image_t *AMdlo,
    float_image_t *AMdhi,
    float_image_t *ASdlo,
    float_image_t *ASdhi,
    float_image_t *BLo,
    float_image_t *BHi,
    float_image_t *BMdlo,
    float_image_t *BMdhi,
    float_image_t *BSdlo,
    float_image_t *BSdhi,
    int res,
    int res_max,
    int bgZero,
    int usa_IA,
    int usa_MD_SD,
    QDIST_t qualDist,
    float dist[],
    int *ndistExP,
    int *ndistIAP
  )
{
  if (qualDist == QDIST_EURGB)
    { estima_dist_EURGB
        ( ALo,AHi,AMdlo,AMdhi,ASdlo,ASdhi,
          BLo,BHi,BMdlo,BMdhi,BSdlo,BSdhi,
          res,res_max,bgZero,usa_IA,usa_MD_SD,dist,
          ndistExP,ndistIAP
        );
    }
  else if (qualDist == QDIST_EUGRADYUV)
    { 
	if (ALo->sz[1]>1)
	{
	   estima_dist_EUGRADYUV
           ( ALo,AHi,AMdlo,AMdhi,ASdlo,ASdhi,
             BLo,BHi,BMdlo,BMdhi,BSdlo,BSdhi,
             res,res_max,bgZero,usa_IA,usa_MD_SD,dist,
             ndistExP,ndistIAP
           );
	}
	else { dist[0]=0.0; dist[1]=1.0; dist[2]=0.5; }
    }
  else if (qualDist == QDIST_EUGRAD)
    { 
	if (ALo->sz[1]>1)
	{
	   estima_dist_EUGRAD
           ( ALo,AHi,AMdlo,AMdhi,ASdlo,ASdhi,
             BLo,BHi,BMdlo,BMdhi,BSdlo,BSdhi,
             res,res_max,bgZero,usa_IA,usa_MD_SD,dist,
             ndistExP,ndistIAP
           );
	}
	else { dist[0]=0.0; dist[1]=1.0; dist[2]=0.5; }
    }
}

void estima_dist_Multiescala( 
    float_image_t *Alo, 
    float_image_t *Ahi,
    float_image_t *AmdLo,
    float_image_t *AmdHi,
    float_image_t *AsdLo,
    float_image_t *AsdHi,
    float_image_t *Blo,
    float_image_t *Bhi,
    float_image_t *BmdLo,
    float_image_t *BmdHi,
    float_image_t *BsdLo,
    float_image_t *BsdHi,
    int res,
    int res_max,
    int bgZero,
    int usa_IA,
    int usa_MD_SD,
    QDIST_t qualDist,
    float *distAcc,
    double lambda[],
    float dist[],
    Interval *distEstIA,
    int *ndistExP,
    int *ndistIAP
  )
{
  int debug = 0;  
  ia_init();
  
  float distRes = calcula_dist_Monoescala(AmdLo,BmdLo,bgZero,qualDist,ndistExP); // Calcula distância exata na escala {res}
  assert((distRes >= 0) && (distRes <= 1.0));

  //distRes+=1.0/64.0; //insere correção do erro de quantização de leitura na imagem exata.

  if (debug){
    fprintf(stderr, "  termo na escala %d: %8.6f * lambda[%d] = %8.6f\n",res,distRes,res,distRes*lambda[res]);
  }
  if ((distEstIA->lo > distRes) || (distEstIA->hi < distRes)){ //verifica se estimativa esta fora do intervalo
    fprintf(stderr, "  ** bug na escala %d: estimativa [%8.6f  %8.6f ]",res,distEstIA->lo,distEstIA->hi);
    fprintf(stderr, " dist(AmdLo,BmdLo) = %8.6f\n",distRes);
    assert (FALSE);
  }
  (*distAcc) += lambda[res]*distRes; //Acumula distancia em *distAcc = lambda_k*distRes
  if (debug){
    fprintf(stderr, "  distAcc = %8.6f\n", (*distAcc));
    fprintf(stderr, "\n");
  }

  // Estimativa da distancia multiescala de {A,B}:
  Interval distTotIA = ia_const((*distAcc),0);
  
  if (res > 0)
    {
      //Calcula estimativa {distZerIA} da distância na escala {0} a partir das versões na escala {res}:
      estima_dist_Monoescala
      ( Alo,Ahi,AmdLo,AmdHi,AsdLo,AsdHi,
        Blo,Bhi,BmdLo,BmdHi,BsdLo,BsdHi,
        res,res_max,bgZero,usa_IA,usa_MD_SD,qualDist,dist,
        ndistExP, ndistIAP
      ); 
      Interval distZerIA = (Interval){ dist[0], dist[1] };
      if (debug){
        fprintf(stderr, "   distZerIA = [%8.6f  %8.6f] ~ %8.6f\n", distZerIA.lo,distZerIA.hi,dist[2]);
      }
            
      //Acumula em {distTotIA} termo da escala zero
      distTotIA = ia_add(distTotIA, ia_scale(distZerIA,lambda[0],1.0)); 
      
      // Atualiza {*distEstIA} com uma estimativa intervalar para a distancia monoescala
      // {dist(Amdlo,Bmdlo)} nas escalas {1..res-1}, ou para {dist(A,B)} se {res=1}:
      if (res == 1)
        { 
          (*distEstIA) = distZerIA;
        }
      else      
        { 
	  if ((qualDist == QDIST_EUGRADYUV) || (qualDist == QDIST_EUGRAD))
	    {
	      // Nao eh possivel estimar a distancia nas escalas inferiores:
	      (*distEstIA) = (Interval){0.0,1.0};
	    }
	  else
	    {
	      // Para a distancia euclidiana,
	      // um limite superior eh a distancia entre as images {A,B} na escala 0;
	      // um limite inferior eh a distancia entre {Amdlo,Bmdlo} na escala {res};
	      // exceto que a quantizacao de {Amdlo} e {Bmdlo} pode aumentar ou diminuir esta distancia.
	      Interval distPosIA = (Interval){ distRes, distZerIA.hi };
	      Interval eps3IA = ia_const(0, 1.0/127.0);  // Estimated image quantization error.
	      distPosIA = ia_add(distPosIA, eps3IA);
	      if (debug){
		fprintf(stderr, "  distPosIA = [%8.6f %8.6f]\n", distPosIA.lo, distPosIA.hi);
		fprintf(stderr, "\n");
	      }
	      (*distEstIA) = distPosIA;
	    }
	}

      if (res > 1)
	{
	  // Acumula em {distTotIA} as estimativas para os termos das escalas {1..res-1} 
     
	  // Calcula peso total das escalas entre 1 e {res-1}:
          int k;
          Interval lambdaAccIA = (Interval){ 0.0, 0.0 };
          for (k=1; k<res; k++) { lambdaAccIA = ia_shift(lambdaAccIA, lambda[k]); }
          if (debug){
            fprintf(stderr, "  lambdaAccIA = [%8.6f %8.6f]\n", lambdaAccIA.lo, lambdaAccIA.hi);
            fprintf(stderr, "\n");
          }
          
          // Acumula os termos {1..res-1}
          distTotIA = ia_add(distTotIA, ia_mul((*distEstIA),lambdaAccIA));
	}

      // Corta em [0_1] pois a distancia acumulada nao pode sair disso:
      if (distTotIA.lo < 0.0) { distTotIA.lo = 0.0; }
      if (distTotIA.hi > 1.0) { distTotIA.hi = 1.0; }
      
      dist[0] = distTotIA.lo;  // Lo
      dist[1] = distTotIA.hi;  // Hi  
      dist[2] = sqrt(((dist[0]*dist[0]) + (dist[1]*dist[1]))/2);  // Md
      if (debug){
        fprintf(stderr, "  dist = [%8.6f  %8.6f] ~ %8.6f\n",dist[0],dist[1],dist[2]);
        fprintf(stderr, "\n");
      }
  } else {
    // A distancia acumulada é a distância exata:
    dist[0] = dist[1] = dist[2] = (*distAcc);
  }
}

double calculaLambda(int cumul,int k,int kmax,double base)
{
   if (! cumul)
   { 
     // Distancia não cumulativa:
     return (k == 0 ? 1.0 : 0.0);
   }
   else 
   { 
     // Distância cumulativa:
     if (base == 1.0)
       { // Pesos uniformes em todas as escalas:
         return 1.0/(kmax+1);
       }
     else
       { // Pesos em progressão geométrica por escala:
         return pow(base,k)*(base-1)/(pow(base,kmax+1)-1);
       }
   }
}