#include "bwImage.h"
#include "rgbImage.h"
#include "floatImage.h"

#include <math.h>
#include <stdio.h>

bwImage::bwImage( floatImage *src, bool scaleFromZeroUp )
  : width_(src->getWidth()), height_(src->getHeight()),
    size_(src->getSize()), memSize_(size_*3*sizeof(unsigned char))
{
  if ( (data_=(unsigned char*)malloc(memSize_))==0 ) return;

  if ( scaleFromZeroUp )
  {
    float scale = 255/src->maxComponent();
    for ( int i=size_-1; i>=0; i-- )
    {
      if ( src->rawData()[i]<0.0f )
	data_[i] = 0;
      else
	data_[i] = src->rawData()[i]*scale;
    }
  }
  else
  {
    float min = src->minComponent();
    float scale = 255/(src->maxComponent()-min);
    for ( int i=size_-1; i>=0; i-- )
    {
      data_[i] = (src->rawData()[i]-min) * scale;
    }
  }
}

bwImage::bwImage( rgbImage *src )
  : width_(src->getWidth()), height_(src->getHeight()),
    size_(src->getSize()), memSize_(size_*sizeof(unsigned char))
{
  if ( (data_=(unsigned char*)malloc(memSize_))==0 ) return;
  
  for ( int y=0; y<height_; y++ )
  {
    for ( int x=0; x<width_; x++ )
    {
      pixel(x,y) =
	(src->pixel(x,y)[R] + src->pixel(x,y)[G] + src->pixel(x,y)[B]) / 3;
    }
  }
}

inline void
bwImage::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 
bwImage::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 
bwImage::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;
      }
    }
  }
}

void 
bwImage::addSquare( int x1, int y1, int x2, int y2, char intens, float trans/*=1.0f*/,
		    bool checkInBounds )
{
  if ( checkInBounds )
  {
    clampInImage(x1,y1);
    clampInImage(x2,y2);
  }
  
  char preMultIntens = intens*trans;
  for ( int i=x1; i<=x2; i++ )
  {
    pixel(i,y1) = pixel(i,y1)*(1.0f-trans) + preMultIntens;
    pixel(i,y2) = pixel(i,y2)*(1.0f-trans) + preMultIntens;
  }
  for ( i=y1+1; i<=y2-1; i++ )
  {
    pixel(x1,i) = pixel(x1,i)*(1.0f-trans) + preMultIntens;
    pixel(x2,i) = pixel(x2,i)*(1.0f-trans) + preMultIntens;
  }
}

floatImage *
bwImage::harrisCornerDetect( floatImage *gradImgX, floatImage *gradImgY,
			     int responseWindow )
{
  if ( (gradImgX->getWidth()!=gradImgY->getWidth()) ||
       (gradImgX->getHeight()!=gradImgY->getHeight()) ||
       (gradImgX->getWidth()==0) ||
       (gradImgX->getHeight()==0) ) return 0;
  
  int width  = gradImgX->getWidth();
  int height = gradImgX->getHeight();
  
  int x, y, i, j;
  int halfWin = (responseWindow-1)/2;
  
  floatImage *cornerResponse = new floatImage( width, height );
  if ( cornerResponse==0 ) return 0;
  cornerResponse->setBorder( halfWin+1, 0.0f );

  float grdX, grdY;
  float imgGrdX2, imgGrdY2, imgGrdXY;
  for ( y=halfWin+1; y<height-halfWin-1; y++ )
  {
    for ( x=halfWin+1; x<width-halfWin-1; x++ )
    {
      imgGrdX2=0.0f;
      imgGrdY2=0.0f;
      imgGrdXY=0.0f;
      for( j=y-halfWin; j<=y+halfWin; j++ )
      {
	for( i=x-halfWin; i<=x+halfWin; i++ )
	{
	  grdX=gradImgX->pixel(i,j);
	  grdY=gradImgY->pixel(i,j);
	  imgGrdX2 += grdX*grdX;
	  imgGrdY2 += grdY*grdY;
	  imgGrdXY += grdX*grdY;
	}
      }
      cornerResponse->pixel(x,y) =
	(imgGrdX2*imgGrdY2 - imgGrdXY*imgGrdXY) -
	0.04f * (imgGrdX2+imgGrdY2)*(imgGrdX2+imgGrdY2);
    }
  }
  return cornerResponse;
}

floatImage *
bwImage::harrisCornerDetect( int responseWindow )
{
  floatImage *grdXImg, *grdYImg;
  imgGrad( grdXImg, grdYImg );
  floatImage *cornerResp =
    harrisCornerDetect( grdXImg, grdYImg, responseWindow );
  delete grdXImg;
  delete grdYImg;
  return cornerResp;
}

floatImage *
bwImage::harrisCornerDetect_T( int responseWindow )
{
  floatImage *grdXImg, *grdYImg;
  imgGradToulouse( grdXImg, grdYImg );
  floatImage *cornerResp =
    harrisCornerDetect( grdXImg, grdYImg, responseWindow );
  delete grdXImg;
  delete grdYImg;
  return cornerResp;
}