/* Last edited on 2009-12-14 15:57:08 by stolfilocal */

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

//****DEFINIÇÕES INTERNAS

#define DFUNDO (1.0)
#define debug 0

// DFUNDO deve ser pelo menos 1.0 senao calculo de dHiMono falha.

float dLoMono_IA(double aLo,double aHi,double bLo,double bHi,int bgZero);
//calcula o limite inferior com aritmética intervalar

float dHiMono_IA(double aLo,double aHi,double bLo,double bHi,int bgZero);
//calcula o limite superior com aritmética intervalar

// Estas funcoes calculam o limite inferior, limite superior, e valor esperado (no sentido rms)
// da distancia euclidiana entre duas sub-imagens monocromaticas {A,B},
// dados os valores mínimos {aLo,bLo}, valores maximos {aHi,bHi}, valores médios {aMd,bMd}, e
// desvios {aSd,bSd} dos pixels de {A} e {B}. 
// Se {bgZero} eh 1, trata valor 0 como fundo.
Interval distMdSd(double aMdLo,double aMdHi,double aSdLo,double aSdHi,double bMdLo,double bMdHi,double bSdLo,double bSdHi);

Interval dstInterval(double aLo,double aHi,double aMdLo,double aMdHi,double aSdLo,double aSdHi,double bLo,double bHi,double bMdLo,double bMdHi,double bSdLo,double bSdHi,int bgZero,int usa_IA,int usa_MD_SD);

void estima_dist_EURGB_IA
  ( 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,
    float dist[],
    int *ndistIAP
  );
/* Igual a {estima_dist_EURGB}, exceto que usa sempre aritmetica intervalar mesmo
  quando {res = 0}. */

//****IMPLEMENTAÇÕES

float calcula_dist_EURGB
  ( 
    float_image_t *Amd,
    float_image_t *Bmd,
    int bgZero,
    int *ndistExP
  )
{   
  int i=0,j=0,ch=0;
  double somaD2 = 0.0;
 
  for (ch=0;ch< Amd->sz[0];ch++)
    {
      for (i=0;i<Amd->sz[1];i++)
	{
	  for (j=0;j<Amd->sz[2];j++)
	    {
              // Pega valores do pixel e canal:
              float amd = (Amd==NULL?0:float_image_get_sample(Amd,ch,i,j));
              float bmd = (Bmd==NULL?0:float_image_get_sample(Bmd,ch,i,j));

              // Calcula diferenca para o pixel e canal:
              float d = amd - bmd;

              // Acumula as estimativas:
              somaD2 += d*d;
	    }
	}
    }
  int namostras = Amd->sz[1]*Amd->sz[2]*Amd->sz[0];
  (*ndistExP)++;
  return sqrt(somaD2/namostras);
}

void estima_dist_EURGB
  ( 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,
    float dist[],
    int *ndistExP,
    int *ndistIAP
  )
{
  
  if (res==0)
  {
    float valor=calcula_dist_EURGB(AmdLo,BmdLo,bgZero,ndistExP);
    dist[0] = dist[1] = dist[2] = valor;
  }
  else
  {
    estima_dist_EURGB_IA
      ( Alo,Ahi,AmdLo,AmdHi,AsdLo,AsdHi,
        Blo,Bhi,BmdLo,BmdHi,BsdLo,BsdHi,
        res,res_max,bgZero,usa_IA,usa_MD_SD,dist,
        ndistIAP
      );
  }
}

void estima_dist_EURGB_IA
  ( 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,
    float dist[],
    int *ndistIAP
  )
{
  ia_init();
  int i=0,j=0,ch=0;
  Interval soma2 = (Interval){0.0,0.0};

  for (ch=0;ch< Alo->sz[0];ch++)
    {
      for (i=0;i<Alo->sz[1];i++)
	{
	  for (j=0;j<Alo->sz[2];j++)
	    {
              // Pega valores do pixel e canal:
              float alo = (Alo==NULL?0:float_image_get_sample(Alo,ch,i,j));
              float ahi = (Ahi==NULL?0:float_image_get_sample(Ahi,ch,i,j));
              float amdLo = (AmdLo==NULL?0:float_image_get_sample(AmdLo,ch,i,j));
              float amdHi = (AmdHi==NULL?0:float_image_get_sample(AmdHi,ch,i,j));
              float asdLo = (AsdLo==NULL?0:float_image_get_sample(AsdLo,ch,i,j));
              float asdHi = (AsdHi==NULL?0:float_image_get_sample(AsdHi,ch,i,j));

              float blo = (Blo==NULL?0:float_image_get_sample(Blo,ch,i,j));
              float bhi = (Bhi==NULL?0:float_image_get_sample(Bhi,ch,i,j));
              float bmdLo = (BmdLo==NULL?0:float_image_get_sample(BmdLo,ch,i,j));
              float bmdHi = (BmdHi==NULL?0:float_image_get_sample(BmdHi,ch,i,j));
              float bsdLo = (BsdLo==NULL?0:float_image_get_sample(BsdLo,ch,i,j));
              float bsdHi = (BsdHi==NULL?0:float_image_get_sample(BsdHi,ch,i,j));

              // Calcula estimativas da distancia para o pixel e canal:                          
              Interval dst = dstInterval(alo,ahi,amdLo,amdHi,asdLo,asdHi,blo,bhi,
                                        bmdLo,bmdHi,bsdLo,bsdHi,bgZero,usa_IA,usa_MD_SD);
              assert(dst.lo>=0);              

              // Garante que as estimativas sao consistentes:
              if (dst.lo > dst.hi) { float tmp = dst.hi; dst.hi = dst.lo; dst.lo = tmp; }

              float dMd = ia_mid(dst);

              if (dMd < dst.lo) { if (debug){fprintf(stderr, "dMd = %10.7f dLo = %10.7f\n", dMd, dst.lo);} dMd = dst.lo; }
              if (dMd > dst.hi) { if (debug){fprintf(stderr, "dMd = %10.7f dHi = %10.7f\n", dMd, dst.hi);} dMd = dst.hi; }

              // Acumula as estimativas:
              soma2 = ia_add(soma2,ia_sqr(dst));            
              if (debug){fprintf(stderr, "dLo:%f, dHi:%f, dMd:%f - soma2Lo:%f, soma2Hi:%f\n",dst.lo,dst.hi,dMd,soma2.lo,soma2.hi);}
	    }
	}
    }
  int namostras = Alo->sz[1]*Alo->sz[2]*Alo->sz[0];
  Interval distAB = ia_sqrt(ia_scale(soma2,1,namostras));
  dist[0] = distAB.lo;
  dist[1] = distAB.hi;
  dist[2] = ia_mid(distAB);
  (*ndistIAP)++;
}

//Aritmética intervalar
float dLoMono_IA(double aLo,double aHi,double bLo,double bHi,int bgZero)
{
  if (bgZero)
    { if ((aLo==0)&&(bLo==0))
        { /* Os dois tem fundo, e podem ser totalmente fundo: */ return 0; }
      else if ((aLo==0)&&(aHi==0))
        { /* {A} é totalmente fundo e {B} nao tem nada de fundo: */ return DFUNDO; }
      else if ((bLo==0)&&(bHi==0))
        { /* {B} é totalmente fundo e {A} nao tem nada de fundo: */ return DFUNDO; }
      else 
        { /* Nenhum dos dois tem fundo - tratamento normal: */ }
    }

  // Calcula um limite {dLo} usando aritmetica intervalar:
  float xHi,yLo;
  if (aLo>bLo)
    { xHi=bHi; yLo=aLo; }
  else
    { xHi=aHi; yLo=bLo; }
   
  if (xHi>=yLo)
    return 0;
  else if (aHi < bLo) 
    return (bLo-aHi);
  else if (aLo > bHi)
    return (aLo-bHi);
  else 
    assert(FALSE);
}

float dHiMono_IA(double aLo,double aHi,double bLo,double bHi,int bgZero)
{
  if (bgZero)
    { if ((aLo==0)&&(aHi==0)&&(bLo==0)&&(bHi==0))
        { /* Os dois sao totalmente fundo: */ return 0; }
      else if ((aLo==0) || (bLo==0))
        { /* Um deles pode ter fundo, e o outro pode ter nao-fundo: */ return DFUNDO; }
      else 
        { /* Nenhum dos dois tem fundo - tratamento normal: */ }
    }

  // Calcula um limite {dHi} usando aritmetica intervalar:
  double a = fabs(aHi-bLo);
  double b = fabs(bHi-aLo);
  if (a>b)
    return a;
  else
    return b;	
}


Interval distMdSd(double aMdLo,double aMdHi,double aSdLo,double aSdHi,double bMdLo,double bMdHi,double bSdLo,double bSdHi)
{
  //gera estruturas de intervalo para md e sd
  Interval amd,bmd,asd,bsd;
  amd.lo=aMdLo; amd.hi=aMdHi;
  bmd.lo=bMdLo; bmd.hi=bMdHi;
  asd.lo=aSdLo; asd.hi=aSdHi;
  bsd.lo=bSdLo; bsd.hi=bSdHi;
 /* printf("\namd: [%f %f] mid: %f rad: %f\n",amd.lo,amd.hi,ia_mid(amd),ia_rad(amd));
  printf("bmd: [%f %f] mid: %f rad: %f\n\n",bmd.lo,bmd.hi,ia_mid(bmd),ia_rad(bmd));
  printf("asd: [%f %f] mid: %f rad: %f \n",asd.lo,asd.hi,ia_mid(asd),ia_rad(asd));
  printf("bsd: [%f %f] mid: %f rad: %f \n\n",bsd.lo,bsd.hi,ia_mid(bsd),ia_rad(bsd));*/
  //calcula a diferença de dois intervalos de média
  Interval md_dif = ia_sub(amd,bmd);
 /* printf("\nmd: [%f %f] mid: %f rad: %f\n",md_dif.lo,md_dif.hi,ia_mid(md_dif),ia_rad(md_dif));*/
  //calcula a junção dos intervalos de desvio
  Interval sd_sum = ia_add(asd,bsd);
  Interval sd_dif = ia_abs(ia_sub(asd,bsd));
  assert(sd_dif.lo <= sd_sum.lo);
  assert(sd_dif.hi <= sd_sum.hi);
  Interval sd_any = (Interval){sd_dif.lo,sd_sum.hi};
/*  printf("sd1 -lo: %f -hi: %f\n",sd1.lo,sd1.hi);
  printf("sd2 -lo: %f -hi: %f\n",sd2.lo,sd2.hi);
  printf("sd3 -lo: %f -hi: %f\n",sd3.lo,sd3.hi);*/

  //calcula o valor da distância
  Interval md_dif2 = ia_sqr(md_dif);
  Interval sd_any2 = ia_sqr(sd_any);
  Interval dst=ia_sqrt(ia_add(md_dif2,sd_any2));
  return dst;
}

Interval dstInterval(double aLo,double aHi,double aMdLo,double aMdHi,double aSdLo,double aSdHi,double bLo,double bHi,double bMdLo,double bMdHi,double bSdLo,double bSdHi,int bgZero,int usa_IA,int usa_MD_SD)
{
  Interval dst;
  if ((usa_IA==1)&&(usa_MD_SD==0))
  {
    dst.lo=dLoMono_IA(aLo,aHi,bLo,bHi,bgZero);
    dst.hi=dHiMono_IA(aLo,aHi,bLo,bHi,bgZero);
  }
  else if ((usa_IA==0)&&(usa_MD_SD==1))
  {
    dst=distMdSd(aMdLo,aMdHi,aSdLo,aSdHi,bMdLo,bMdHi,bSdLo,bSdHi);
  }
  else
  {
    Interval ia;
    ia.lo=dLoMono_IA(aLo,aHi,bLo,bHi,bgZero);
    ia.hi=dHiMono_IA(aLo,aHi,bLo,bHi,bgZero);
    dst = distMdSd(aMdLo,aMdHi,aSdLo,aSdHi,bMdLo,bMdHi,bSdLo,bSdHi);
    if ((aLo < aHi) || (bLo < bHi))
    {
      if (ia.lo > dst.lo) { dst.lo = ia.lo; }
      if (ia.hi < dst.hi) { dst.hi = ia.hi; }
    }
  }

  if (dst.lo < 0) { dst.lo = 0;}
  if (dst.lo > 1) { dst.lo = 1;}

  if (dst.hi < 0) { dst.hi = 0;}
  if (dst.hi > 1) { dst.hi = 1;}

  return dst;
}