#include "floatImage.h"
#include <math.h>
#include <float.h>
#include "bwImage.h"
#include "rgbImage.h"


floatImage::floatImage( floatImage *src1, floatImage *src2, bool add )
  : width_(src1->getWidth()), height_(src1->getHeight()),
    size_(src1->getSize()), memSize_(size_*sizeof(float))
{
  if ( (src2->getWidth()!=width_) || (src2->getHeight()!=height_) ||
       (width_==0) || (height_==0) ) return;

  if ( (data_=(float*)malloc(memSize_))==0 ) return;

  if ( add )
  {
    for ( int i=size_-1; i>=0; i-- )
    {
      data_[i] = src1->rawData()[i] + src2->rawData()[i];
    }
  }
  else
  {
    float data1, data2;
    for ( int i=size_-1; i>=0; i-- )
    {
      data1 = src1->rawData()[i];
      data2 = src2->rawData()[i];
      data_[i] = sqrtf( data1*data1 + data2*data2 );
    }
  }
}

floatImage::floatImage( floatImage *src1, floatImage *src2,
			floatImage *src3, bool add )
  : width_(src1->getWidth()), height_(src1->getHeight()),
    size_(src1->getSize()), memSize_(size_*sizeof(float))
{
  if ( (src2->getWidth()!=width_) || (src2->getHeight()!=height_) ||
       (src3->getWidth()!=width_) || (src3->getHeight()!=height_) ||
       (width_==0) || (height_==0) ) return;

  if ( (data_=(float*)malloc(memSize_))==0 ) return;

  if ( add )
  {
    for ( int i=size_-1; i>=0; i-- )
    {
      data_[i] = src1->rawData()[i] + src2->rawData()[i] + src3->rawData()[i];
    }
  }
  else
  {
    float data1, data2, data3;
    for ( int i=size_-1; i>=0; i-- )
    {
      data1 = src1->rawData()[i];
      data2 = src2->rawData()[i];
      data3 = src3->rawData()[i];
      data_[i] = sqrtf( data1*data1 + data2*data2 + data3*data3 );
    }
  }
}

float
floatImage::maxComponent()
{
  float maxVal = -FLT_MAX;
  for ( int i=size_-1; i>=0; i-- )
  {
    if ( maxVal<data_[i] ) maxVal=data_[i];
  }
  return maxVal;
}

float
floatImage::minComponent()
{
  float minVal = FLT_MAX;
  for ( int i=size_-1; i>=0; i-- )
  {
    if ( minVal>data_[i] ) minVal=data_[i];
  }
  return minVal;
}

float 
floatImage::findMaxComponent( int &x, int &y )
{
  float maxVal = -FLT_MAX;
  for ( int i=size_-1; i>=0; i-- )
  {
    if ( maxVal<data_[i] )
    {
      maxVal=data_[i];
      x = i % width_;
      y = i / width_;
    }
  }
  return maxVal;
}

void 
floatImage::setBorder( int borderWidth, float intens )
{
  int x, y;
  for ( y=0; y<borderWidth; y++ )
    for ( x=0; x<width_; x++ )
      pixel(x,y)=intens;
  for ( ; y<height_-borderWidth-1; y++ )
  {
    for ( x=0; x<borderWidth; x++ )
      pixel(x,y)=intens;
    for ( x=width_-borderWidth-1; x<width_; x++ )
      pixel(x,y)=intens;
  }
  for ( ; y<height_; y++ )
    for ( x=0; x<width_; x++ )
      pixel(x,y)=intens;
}

void 
floatImage::drawSolidSquare( int x1, int y1, int x2, int y2, float intens,
			     bool checkInBounds )
{
  if ( checkInBounds )
  {
    clampInImage(x1,y1);
    clampInImage(x2,y2);
  }
  
  int x;
  for( ; y2>=y1; y2-- )
  {
    for( x=x2; x>=x1; x-- )
    {
      pixel(x,y2) = intens;
    }
  }
}

inline void
floatImage::gradToulouse( int x, int y, float &grdX, float &grdY )
{
#define cos45 0.70710678
#define twoRoot2 2.8284271
  float gx1, gy1, gx2, gy2;

  // calculate gradients diagonally
  gx2 = (pixel(x+1,y+1)-pixel(x-1,y-1))/twoRoot2;
  gy2 = (pixel(x-1,y+1)-pixel(x+1,y-1))/twoRoot2;

  // rotate the gradient to x and y axis
  gx1 = cos45*(gx2 - gy2);
  gy1 = cos45*(gx2 + gy2);

  // calculate the gradients vertically and horizontally
  gx2 = (pixel(x+1,y)-pixel(x-1,y))/2.0f;
  gy2 = (pixel(x,y+1)-pixel(x,y-1))/2.0f;

  // average the two gradient claculations
  grdX = (gx1 + gx2) / 2.0f;
  grdY = (gy1 + gy2) / 2.0f;

  // estimate the difference in gradients by the two calculations
  float len1 = sqrt(gx1*gx1+gy1*gy1);
  float len2 = sqrt(gx2*gx2+gy2*gy2);

  // prevent division by 0
  if ( (len1<0.000001f) && (len2<0.000001f) ) return;
  
  if ( len1>len2 )
  {
    gx1 /= len1;
    gy1 /= len1;
    gx2 /= len1;
    gy2 /= len1;
  }
  else
  {
    gx1 /= len2;
    gy1 /= len2;
    gx2 /= len2;
    gy2 /= len2;
  }
  
  gx1 -= gx2;
  gy1 -= gy2;
  len1 = sqrt(gx1*gx1+gy1*gy1) + 1.0f;
  
  // len1 of gr1 is in the range 1 to 3
  grdX /= len1;
  grdY /= len1;
}

void 
floatImage::imgGradToulouse( floatImage *&gradImgX, floatImage *&gradImgY )
{
  gradImgX = new floatImage( width_, height_ );
  gradImgY = new floatImage( width_, height_ );
  if ( (gradImgX==0) || (gradImgY==0) ) return;
  
  for ( int y=0; y<height_; y++ )
  {
    for ( int x=0; x<width_; x++ )
    {
      if ( (x==0) || (x==width_-1) || (y==0) || (y==height_-1) )
      {
	gradImgX->pixel(x,y)=0.0f;
	gradImgY->pixel(x,y)=0.0f;
      }
      else
      {
	gradToulouse(x,y,gradImgX->pixel(x,y),gradImgY->pixel(x,y));
      }
    }
  }
}

void 
floatImage::imgGrad( floatImage *&gradImgX, floatImage *&gradImgY )
{
  gradImgX = new floatImage( width_, height_ );
  gradImgY = new floatImage( width_, height_ );
  if ( (gradImgX==0) || (gradImgY==0) ) return;

  for ( int y=0; y<height_; y++ )
  {
    for ( int x=0; x<width_; x++ )
    {
      if ( (x==0) || (x==width_-1) || (y==0) || (y==height_-1) )
      {
	gradImgX->pixel(x,y)=0.0f;
	gradImgY->pixel(x,y)=0.0f;
      }
      else
      {
	gradImgX->pixel(x,y) = (pixel(x+1,y)-pixel(x-1,y))/2.0f;
	gradImgY->pixel(x,y) = (pixel(x,y+1)-pixel(x,y-1))/2.0f;
      }
    }
  }
}