#include <estimaDistancia.h>
#include <assert.h>

#define True 1
#define False 0

#define DFUNDO (1.0)
// DFUNDO deve ser pelo menos 1.0 senao calculo de dHiMono falha.

//#define USA_MD_SD TRUE   // este define é utilizado juntamente com o código anterior a mudança para testes com IA e 
                           // MD_SD separados



//****DEFINIÇÕES INTERNAS


float dLoMono_IA(double aLo,double aHi,double bLo,double bHi,int bgZero);
//calcula o limite inferior com aritmética intervalar
float dLoMono_MD_SD(double aMd,double aSd,double bMd,double bSd);
//calcula o limite inferior com média e desvio-padrão

float dHiMono_IA(double aLo,double aHi,double bLo,double bHi,int bgZero);
//calcula o limite superior com aritmética intervalar
float dHiMono_MD_SD(double aMd,double aSd,double bMd,double bSd);
//calcula o limite superior com média e desvio-padrão

float dMdMono_IA(double aLo,double aHi,double bLo,double bHi,int bgZero);
//calcula o valor esperado (no sentido rms) com aritmética intervalar
float dMdMono_MD_SD(double aMd,double aSd,double bMd,double bSd);
//calcula o valor esperado (no sentido rms) com média e desvio-padrão


//****IMPLEMENTAÇÕES

//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);
}

//Média e Desvio-Padrão
float dLoMono_MD_SD(double aMd,double aSd,double bMd,double bSd)
{
  // Ajusta o valor levando em conta media e desvio e Ajusta para levar em conta erros de arredondamento
  if (aSd==bSd){ return fabs(aMd-bMd);}
  else {return fmax(0.0,sqrt((aMd-bMd)*(aMd-bMd) + (aSd-bSd)*(aSd-bSd)) - 1.0e-6);}
}


float dLoMono(double aLo,double aHi,double aMd,double aSd, double bLo,double bHi,double bMd,double bSd,int bgZero,int usa_IA,int usa_MD_SD)
{
  float valor;
  if ((usa_IA==1)&&(usa_MD_SD==0))
  {
    valor=dLoMono_IA(aLo,aHi,bLo,bHi,bgZero);
  }
  else if ((usa_IA==0)&&(usa_MD_SD==1))
  {
    valor=dLoMono_MD_SD(aMd,aSd,bMd,bSd);
  }
  else
  {
     valor=dLoMono_IA(aLo,aHi,bLo,bHi,bgZero);
     if ((aLo < aHi) || (bLo < bHi))
     {
	float valor1=dLoMono_MD_SD(aMd,aSd,bMd,bSd);
        if (valor < valor1) { valor = valor1; }
     }
  }
  // Ajusta {dLo} para nao sair do intervalo [0_1]:
  if (valor < 0) { valor = 0; }
  if (valor > 1) { valor = 1; }
  return valor;
}

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;	
}

float dHiMono_MD_SD(double aMd,double aSd,double bMd,double bSd)
{
  // Ajusta valor levando em conta media e desvio e Ajusta para levar em conta erros de arredondamento
  if ((aSd==0)&&(bSd==0)){ return fabs(aMd-bMd);}
  else {return fmin(1.0,sqrt((aMd-bMd)*(aMd-bMd) + (aSd+bSd)*(aSd+bSd)) + 1.0e-6);}
}

float dHiMono(double aLo,double aHi,double aMd,double aSd, double bLo,double bHi,double bMd,double bSd, int bgZero,int usa_IA, int usa_MD_SD)
{
  float valor;
  if ((usa_IA==1)&&(usa_MD_SD==0))
  {
    valor=dHiMono_IA(aLo,aHi,bLo,bHi,bgZero);
  }
  else if ((usa_IA==0)&&(usa_MD_SD==1))
  {
    valor=dHiMono_MD_SD(aMd,aSd,bMd,bSd);
  }
  else
  {
    valor=dHiMono_IA(aLo,aHi,bLo,bHi,bgZero);
    if ((aLo < aHi) || (bLo < bHi))
    {
      float valor1=dHiMono_MD_SD(aMd,aSd,bMd,bSd);
      if (valor > valor1) { valor = valor1; }
    }
  }

  // Ajusta {dHi} para nao sair do intervalo [0_1]:
  if (valor < 0) { valor = 0; }
  if (valor > 1) { valor = 1; }
  
  return valor;
}

float dMdMono_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 ((aHi==0) || (bHi==0))
          { /* Um deles eh totalmente fundo, e o outro pode ter nao-fundo: */ return DFUNDO; }
        else 
          { /* Nenhum dos dois eh totalmente fundo - tratamento normal: */ }
      }
      return fabs((aHi+aLo)/2 - (bHi+bLo)/2);
}

float dMdMono_MD_SD(double aMd,double aSd,double bMd,double bSd)
{
  double abMd = aMd-bMd;
  return sqrt(aSd*aSd + bSd*bSd + abMd*abMd);
}

float dMdMono(double aLo,double aHi,double aMd,double aSd, double bLo,double bHi,double bMd,double bSd,int bgZero,int usa_IA,int usa_MD_SD)
{
  double dMd;
  
  if ((usa_IA==1)&&(usa_MD_SD==0))
  {
    dMd=dMdMono_IA(aLo,aHi,bLo,bHi,bgZero);
  }
  else if ((usa_IA==0)&&(usa_MD_SD==1))
  {
    dMd=dMdMono_MD_SD(aMd,aSd,bMd,bSd);
  }
  else
  {
      dMd=dMdMono_MD_SD(aMd,aSd,bMd,bSd);
  }

  // Ajusta {dMd} para nao sair do intervalo [0_1]:
  if (dMd < 0) { dMd = 0; }
  if (dMd > 1) { dMd = 1; }
  
  return dMd;
}

void distRGB
  ( img_t *Alo, 
    img_t *Ahi,
    img_t *Amd,
    img_t *Asd, 
    img_t *Blo,
    img_t *Bhi,
    img_t *Bmd,
    img_t *Bsd, 
    int bgZero,
    float *distLo,
    float *distHi,
    float *distMd,
    int usa_IA,
    int usa_MD_SD
  )
{
  int i=0,j=0,ch=0;
  double somaLo2 = 0;
  double somaHi2 = 0;
  double somaMd2 = 0;
  bool_t debug=FALSE; 
 
  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 amd = (Amd==NULL?0:float_image_get_sample(Amd,ch,i,j));
              float asd = (Asd==NULL?0:float_image_get_sample(Asd,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 bmd = (Bmd==NULL?0:float_image_get_sample(Bmd,ch,i,j));
              float bsd = (Bsd==NULL?0:float_image_get_sample(Bsd,ch,i,j));

              // Calcula estimativas da distancia para o pixel e canal:
              float dLo = dLoMono(alo,ahi,amd,asd, blo,bhi,bmd,bsd, bgZero,usa_IA,usa_MD_SD);
              float dHi = dHiMono(alo,ahi,amd,asd, blo,bhi,bmd,bsd, bgZero,usa_IA,usa_MD_SD);
              float dMd = dMdMono(alo,ahi,amd,asd, blo,bhi,bmd,bsd, bgZero,usa_IA,usa_MD_SD);

              // Garante que as estimativas sao consistentes:
              if (dLo > dHi) { float tmp = dHi; dHi = dLo; dLo = tmp; }
              if (dMd < dLo) { dMd = dLo; }
              if (dMd > dHi) { dMd = dHi; }

              // Acumula as estimativas:
              somaLo2 += dLo*dLo;
              somaHi2 += dHi*dHi;
              somaMd2 += dMd*dMd;
              if (debug){fprintf(stderr, "dLo:%f, dHi:%f, dMd:%f - somaLo:%f, somaHi:%f, somaMd:%f\n",dLo,dHi,dMd,somaLo2,somaHi2,somaMd2);}
	    }
	}
    }
  int namostras = Alo->sz[1]*Alo->sz[2]*Alo->sz[0];
  (*distLo) = sqrt(somaLo2/namostras);
  (*distHi) = sqrt(somaHi2/namostras);
  (*distMd) = sqrt(somaMd2/namostras);
}

//distância (euclidiana) multiescala