#include "rgbImage.h"

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

#include <math.h>

rgbImage::rgbImage( floatImage *srcR, floatImage *srcG, floatImage *srcB,
		    bool scaleFromZeroUp )
  : width_(srcR->getWidth()), height_(srcR->getHeight()),
    size_(srcR->getSize()), memSize_(size_*3*sizeof(unsigned char))
{
  if ( (width_!=srcR->getWidth()) ||
       (width_!=srcG->getWidth()) ||
       (height_!=srcR->getHeight()) ||
       (height_!=srcG->getHeight()) ) return;
  
  if ( (data_=(unsigned char*)malloc(memSize_))==0 ) return;
  
  float maxR=srcR->maxComponent();
  float maxG=srcG->maxComponent();
  float maxB=srcB->maxComponent();
  float max = (maxR>maxG) ? ((maxR>maxB)?maxR:maxB) : ((maxG>maxB)?maxG:maxB);
  
  if ( scaleFromZeroUp )
  {
    float scale = 255/max;
    for ( int i=size_-1; i>=0; i-- )
    {
      if ( srcR->rawData()[i]<0.0f )
	data_[i*3+R] = 0;
      else
	data_[i*3+R] = srcR->rawData()[i]*scale;
      
      if ( srcG->rawData()[i]<0.0f )
	data_[i*3+G] = 0;
      else
	data_[i*3+G] = srcG->rawData()[i]*scale;

      if ( srcB->rawData()[i]<0.0f )
	data_[i*3+B] = 0;
      else
	data_[i*3+B] = srcB->rawData()[i]*scale;
    }
  }
  else
  {
    float minR=srcR->minComponent();
    float minG=srcG->minComponent();
    float minB=srcB->minComponent();
    float min = (minR<minG) ? ((minR<minB)?minR:minB) : ((minG<minB)?minG:minB);
    float scale = 255/(max-min);
    for ( int i=size_-1; i>=0; i-- )
    {
      data_[i*3+R] = (srcR->rawData()[i]-min)*scale;
      data_[i*3+G] = (srcG->rawData()[i]-min)*scale;
      data_[i*3+B] = (srcB->rawData()[i]-min)*scale;
    }
  }
}

void 
rgbImage::extractRGBComponents( bwImage *&rImg, bwImage *&gImg, bwImage *&bImg )
{
  rImg = new bwImage( width_, height_ );
  gImg = new bwImage( width_, height_ );
  bImg = new bwImage( width_, height_ );
  if ( (rImg==0) || (bImg==0) || (gImg==0) ) return;
  for ( int i=size_-1; i>=0; i-- )
  {
    rImg->rawData()[i] = data_[i*3+R];
    gImg->rawData()[i] = data_[i*3+G];
    bImg->rawData()[i] = data_[i*3+B];
  }
}

void
rgbImage::addSquare( int x1, int y1, int x2, int y2,
		     char r, char g, char b, float trans,
		     bool checkInBounds )
{
  if ( checkInBounds )
  {
    clampInImage(x1,y1);
    clampInImage(x2,y2);
  }
  
  char preMultR = r*trans;
  char preMultG = g*trans;
  char preMultB = b*trans;
  for ( int i=x1; i<=x2; i++ )
  {
    pixel(i,y1)[R] = pixel(i,y1)[R]*(1.0f-trans) + preMultR;
    pixel(i,y1)[G] = pixel(i,y1)[G]*(1.0f-trans) + preMultG;
    pixel(i,y1)[B] = pixel(i,y1)[B]*(1.0f-trans) + preMultB;
    
    pixel(i,y2)[R] = pixel(i,y2)[R]*(1.0f-trans) + preMultR;
    pixel(i,y2)[G] = pixel(i,y2)[G]*(1.0f-trans) + preMultG;
    pixel(i,y2)[B] = pixel(i,y2)[B]*(1.0f-trans) + preMultB;
  }
  for ( i=y1+1; i<=y2-1; i++ )
  {
    pixel(x1,i)[R] = pixel(x1,i)[R]*(1.0f-trans) + preMultR;
    pixel(x1,i)[G] = pixel(x1,i)[G]*(1.0f-trans) + preMultG;
    pixel(x1,i)[B] = pixel(x1,i)[B]*(1.0f-trans) + preMultB;
    
    pixel(x2,i)[R] = pixel(x2,i)[R]*(1.0f-trans) + preMultR;
    pixel(x2,i)[G] = pixel(x2,i)[G]*(1.0f-trans) + preMultG;
    pixel(x2,i)[B] = pixel(x2,i)[B]*(1.0f-trans) + preMultB;
  }
}

void
rgbImage::addSquare( const subWindow &src,
		     char r, char g, char b, float trans,
		     bool checkInBounds )
{
  addSquare( src.upperLeftX(), src.upperLeftY(),
	     src.lowerRightX(), src.lowerRightY(),
	     r, g, b, trans, checkInBounds );
}

void 
rgbImage::addLine( float ax, float by, float c,
		   char r, char g, char b, float trans,
		   int clipXmin, int clipYmin, int clipXmax, int clipYmax )
{
  float m, d;
  int x, y;
  bool doClip = (clipXmin!=clipXmax) && (clipYmin!=clipYmax);
  
  char preMultR = r*trans;
  char preMultG = g*trans;
  char preMultB = b*trans;
  trans = 1.0f-trans;
  
  if ( fabs(ax)<fabs(by) )
  { // line closer to horizontal
    // y = mx+d = (-ax/by)*x + (-c/by)
    m = -ax/by;
    d = -c/by;
    for ( x=width_-1; x>=0; x-- )
    {
      y=rint(m*x+d);
      if ( (y<0) || (y>=height_) ) continue;
      if ( (doClip) &&
	   (x<clipXmin) || (x>=clipXmax) || (y<clipYmin) || (y>=clipYmax) ) continue;
      pixel(x,y)[R] = pixel(x,y)[R]*trans + preMultR;
      pixel(x,y)[G] = pixel(x,y)[G]*trans + preMultG;
      pixel(x,y)[B] = pixel(x,y)[B]*trans + preMultB;
    }
  }
  else
  { // line closer to vertical
    // x = ym+d = (-by/ax)*y + (-c/ax)
    m = -by/ax;
    d = -c/ax;
    for ( y=height_-1; y>=0; y-- )
    {
      x=rint(m*y+d);
      if ( (x<0) || (x>=width_) ) continue;
      if ( (doClip) &&
	   (x<clipXmin) || (x>=clipXmax) || (y<clipYmin) || (y>=clipYmax) ) continue;
      pixel(x,y)[R] = pixel(x,y)[R]*trans + preMultR;
      pixel(x,y)[G] = pixel(x,y)[G]*trans + preMultG;
      pixel(x,y)[B] = pixel(x,y)[B]*trans + preMultB;
    }
  }
}

void 
rgbImage::addLineSegment( int x1, int y1, int x2, int y2,
			  char r, char g, char b, float trans )
{
  int minX = x1<x2 ? x1 : x2;
  int minY = y1<y2 ? y1 : y2;
  int maxX = (x1>x2 ? x1 : x2)+1;
  int maxY = (y1>y2 ? y1 : y2)+1;
  float ax,bx,c;
  if ( x2-x1 == 0 )
  {
    ax = 1.0f;
    bx = (float)(x1-x2) / (float)(y2-y1);
  }
  else
  {
    ax = (float)(y2-y1) / (float)(x1-x2);
    bx = 1.0f;
  }
  c = -ax*(float)(x1) - bx*(float)(y1);
  addLine( ax, bx, c, r, g, b, trans, minX, minY, maxX, maxY );
    
}

void 
rgbImage::paste( int x, int y, rgbImage *src )
{
  int sx = x+src->getWidth()>=width_ ? width_-x : src->getWidth();
  int sy = y+src->getHeight()>=height_ ? height_-y : src->getHeight();
  
  for ( int i=sy-1; i>=0; i-- )
    memcpy( pixel(x,y+i), src->pixel(0,i), 3*sx*sizeof(char) );
//     for ( int j=sx-1; j>=0; j-- )
//     {
//       pixel(i+x,j+y)[R] = src->pixel(i,j)[R];
//       pixel(i+x,j+y)[G] = src->pixel(i,j)[G];
//       pixel(i+x,j+y)[B] = src->pixel(i,j)[B];
//     }
}

void 
rgbImage::imgGrad( floatImage *&gradImgX, floatImage *&gradImgY )
{
  bwImage intensImg( this );
  intensImg.imgGrad( gradImgX, gradImgY );
}

void 
rgbImage::imgGrad( floatImage *&compXR, floatImage *&compXG, floatImage *&compXB,
	      floatImage *&compYR, floatImage *&compYG, floatImage *&compYB )
{
  compXR = new floatImage( width_, height_ );
  compXG = new floatImage( width_, height_ );
  compXB = new floatImage( width_, height_ );
  compYR = new floatImage( width_, height_ );
  compYG = new floatImage( width_, height_ );
  compYB = new floatImage( width_, height_ );
  if ( (compXR==0) || (compXG==0) || (compXB==0) ||
       (compYR==0) || (compYG==0) || (compYB==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) )
      {
	compXR->pixel(x,y)=0.0f;
	compXG->pixel(x,y)=0.0f;
	compXB->pixel(x,y)=0.0f;
	compYR->pixel(x,y)=0.0f;
	compYG->pixel(x,y)=0.0f;
	compYB->pixel(x,y)=0.0f;
      }
      else
      {
	compXR->pixel(x,y) = (pixel(x+1,y)[R]-pixel(x-1,y)[R])/2.0f;
	compXG->pixel(x,y) = (pixel(x+1,y)[G]-pixel(x-1,y)[G])/2.0f;
	compXB->pixel(x,y) = (pixel(x+1,y)[B]-pixel(x-1,y)[B])/2.0f;
	compYR->pixel(x,y) = (pixel(x,y+1)[R]-pixel(x,y-1)[R])/2.0f;
	compYG->pixel(x,y) = (pixel(x,y+1)[G]-pixel(x,y-1)[G])/2.0f;
	compYB->pixel(x,y) = (pixel(x,y+1)[B]-pixel(x,y-1)[B])/2.0f;
      }
    }
  }
}

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

  // calculate gradients diagonally
  gx2 = (pixel(x+1,y+1)[comp]-pixel(x-1,y-1)[comp])/twoRoot2;
  gy2 = (pixel(x-1,y+1)[comp]-pixel(x+1,y-1)[comp])/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)[comp]-pixel(x-1,y)[comp])/2.0f;
  gy2 = (pixel(x,y+1)[comp]-pixel(x,y-1)[comp])/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 
rgbImage::imgGradToulouse( floatImage *&gradImgX, floatImage *&gradImgY )
{
  bwImage intensImg( this );
  intensImg.imgGradToulouse( gradImgX, gradImgY );
}

void 
rgbImage::imgGradToulouse(
  floatImage *&compXR, floatImage *&compXG, floatImage *&compXB,
  floatImage *&compYR, floatImage *&compYG, floatImage *&compYB )
{
  compXR = new floatImage( width_, height_ );
  compXG = new floatImage( width_, height_ );
  compXB = new floatImage( width_, height_ );
  compYR = new floatImage( width_, height_ );
  compYG = new floatImage( width_, height_ );
  compYB = new floatImage( width_, height_ );
  if ( (compXR==0) || (compXG==0) || (compXB==0) ||
       (compYR==0) || (compYG==0) || (compYB==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) )
      {
	compXR->pixel(x,y)=0.0f;
	compXG->pixel(x,y)=0.0f;
	compXB->pixel(x,y)=0.0f;
	compYR->pixel(x,y)=0.0f;
	compYG->pixel(x,y)=0.0f;
	compYB->pixel(x,y)=0.0f;
      }
      else
      {
	gradToulouse(x,y,compXR->pixel(x,y),compYR->pixel(x,y),R);
	gradToulouse(x,y,compXG->pixel(x,y),compYG->pixel(x,y),G);
	gradToulouse(x,y,compXB->pixel(x,y),compYB->pixel(x,y),B);
      }
    }
  }
}

floatImage *
rgbImage::harrisCornerDetect(
  floatImage *&compXR, floatImage *&compXG, floatImage *&compXB,
  floatImage *&compYR, floatImage *&compYG, floatImage *&compYB,
  int responseWindow )
{
  floatImage *cornerRespR =
    bwImage::harrisCornerDetect( compXR, compYR, responseWindow );
  floatImage *cornerRespG =
    bwImage::harrisCornerDetect( compXG, compYG, responseWindow );
  floatImage *cornerRespB =
    bwImage::harrisCornerDetect( compXB, compYB, responseWindow );

  floatImage *cornerResponse =
    new floatImage( cornerRespR, cornerRespG, cornerRespB, true );
  
  return cornerResponse;
}

floatImage *
rgbImage::harrisCornerDetect( int responseWindow )
{
  floatImage *compXR, *compXG, *compXB;
  floatImage *compYR, *compYG, *compYB;
  imgGrad( compXR, compXG, compXB,
	   compYR, compYG, compYB );
  floatImage *cornerResp =
    harrisCornerDetect( compXR, compXG, compXB,
			compYR, compYG, compYB, responseWindow );
  delete compXR;
  delete compXG;
  delete compXB;
  delete compYR;
  delete compYG;
  delete compYB;
  return cornerResp;
}

floatImage *
rgbImage::harrisCornerDetect_T( int responseWindow )
{
  floatImage *compXR, *compXG, *compXB;
  floatImage *compYR, *compYG, *compYB;
  imgGradToulouse( compXR, compXG, compXB,
		   compYR, compYG, compYB );
  floatImage *cornerResp =
    harrisCornerDetect( compXR, compXG, compXB,
			compYR, compYG, compYB, responseWindow );
  delete compXR;
  delete compXG;
  delete compXB;
  delete compYR;
  delete compYG;
  delete compYB;
  return cornerResp;
}