//***************************************************************************
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "image.h"
#include "matrix.h"
#include "gaussian.h"
//***************************************************************************
#define DOOG_OFFSET_FACTOR 4.0
//***************************************************************************
void FilteredImg(Matrix *Out, Matrix &Inp, FilterType filter, 
        double sigma, double theta, int side)
{
int border = side;
int height = Inp.nx;
int width = Inp.ny;

/* FFT2 */
int xs = (int) pow(2.0, ceil(log2((double)(height+2.0*border))));
int ys = (int) pow(2.0, ceil(log2((double)(width+2.0*border))));

Matrix IMG(xs,ys);

int r1, r2, r3, r4;
r1 = width+border;
r2 = width+2*border;
for (int h = 0; h < border; h++)
        {
         for (int w = 0; w < border; w++)
                IMG.p[h][w] = Inp.p[border-1-h][border-1-w];
         for (int w = border; w < r1; w++)
                IMG.p[h][w] = Inp.p[border-1-h][w-border];
         for (int w = r1; w < r2; w++)
                IMG.p[h][w] = Inp.p[border-1-h][2*width-w+border-1];
        }

r1 = height+border;
r2 = width+border;
r3 = width+2*border;
for (int h = border; h < r1; h++)
        {
         for (int w = 0; w < border; w++)
                IMG.p[h][w] = Inp.p[h-border][border-1-w];
         for (int w = border; w < r2; w++)
                IMG.p[h][w] = Inp.p[h-border][w-border];
         for (int w = r2; w < r3; w++)
                IMG.p[h][w] = Inp.p[h-border][2*width-w+border-1];
        }

r1 = height+border;
r2 = height+2*border;
r3 = width+border;
r4 = width+2*border;
for (int h = r1; h < r2; h++)
        {
         for (int w = 0; w < border; w++)
                IMG.p[h][w] = Inp.p[2*height-h+border-1][border-1-w];
         for (int w = border; w < r3; w++)
                IMG.p[h][w] = Inp.p[2*height-h+border-1][w-border];
         for (int w = r3; w < r4; w++)
                IMG.p[h][w] = Inp.p[2*height-h+border-1][2*width-w+border-1];
        }
//IMG.WriteImage("PaddedInp.dbg", "gif");

Matrix F_real(xs,ys), F_imag(xs,ys), IMG_imag(xs,ys);

MatrixFFT2D(&F_real, &F_imag, &IMG, &IMG_imag);

// ----------- compute the GD filtered output -------------
Matrix Mask(2*side+1, 2*side+1);
Matrix Padded(xs,ys), Padded_i(xs, ys);
Matrix Tmp_1(xs,ys), Tmp_2(xs,ys);
Matrix G_real(xs,ys), G_imag(xs,ys);

switch(filter)
        {
         case GDmask:
                GD(&Mask, sigma, theta);
         break;

         case DOOGmask:
                DOOG(&Mask, sigma, theta);
         break;

         case GAUSSmask:
                Gaussian(&Mask, sigma, theta);
         break;

         default:
                cerr << "FilteredImg: Invalid filter type!!" << endl;
                exit(-1);
         break;
        }

MatCopy(&Padded, &Mask, 0, 0, 0, 0, 2*side, 2*side);
MatrixFFT2D(&G_real, &G_imag, &Padded, &Padded_i);
//F_real.WriteFFTImage("PaddedOut.dbg", "gif");

//Padded.WriteImage("h.dbg", "gif");
//G_real.WriteImage("H.dbg", "gif");

// ^ stands for pixel by pixel multiplication
Tmp_1 = G_real ^ F_real;
Tmp_2 = G_imag ^ F_imag;
IMG = Tmp_1 - Tmp_2;

// ^ stands for pixel by pixel multiplication
Tmp_1 = G_real ^ F_imag;
Tmp_2 = G_imag ^ F_real;
IMG_imag = Tmp_1 + Tmp_2;

MatrixIFFT2D(&Tmp_1, &Tmp_2, &IMG, &IMG_imag);

//Tmp_1.WriteImage("PaddedOut.dat");

MatCopy(Out, &Tmp_1, 0, 0, 2*border, 2*border, height+2*border-1, width+2*border-1);
}
//***************************************************************************
void Gaussian(Matrix *M, double sigma, double theta)
{
// a square mask (2*side+1)x(2*side+1)
int side = (M->nx-1)/2;

// Note: theta is meaningless in Gaussian() due to presence of
// the term (x^2+y^2) which is rotation independent!
for (int x = 0; x < 2*side+1; x++)
        {
         for (int y = 0; y < 2*side+1; y++)
                {
                 M->p[x][y] = Gaussian((double) (x-side), (double) (y-side), sigma);
                }
        }
}
//***************************************************************************
void GD(Matrix *M, double sigma, double theta)
{
// a square mask (2*side+1)x(2*side+1)
int side = (M->nx-1)/2;

for (int x = 0; x < 2*side+1; x++)
        {
         for (int y = 0; y < 2*side+1; y++)
                {
                 M->p[x][y] = GD((double) (x-side), (double) (y-side), sigma, theta);
                }
        }
}
//***************************************************************************
void DOOG(Matrix *M, double sigma, double theta)
{
// a square mask (2*side+1)x(2*side+1)
int side = (M->nx-1)/2;

for (int x = 0; x < 2*side+1; x++)
        {
         for (int y = 0; y < 2*side+1; y++)
                {
                 M->p[x][y] = DOOG((double) (x-side), (double) (y-side), sigma, theta);
                }
        }
}
//***************************************************************************
// Derivative of Gaussian
double GD(double x, double y, double sigma, double theta)
{
double xn =  x*cos(theta)+y*sin(theta);
double yn = -x*sin(theta)+y*cos(theta);

return -xn/(sigma*sigma)*Gaussian(xn,yn,sigma);
}
//***************************************************************************
// Derivative of offset Gaussian
double DOOG(double x, double y, double sigma, double theta)
{
double xn =  x*cos(theta)+y*sin(theta);
double yn = -x*sin(theta)+y*cos(theta);
// d is the offset
double d = DOOG_OFFSET_FACTOR*sigma;

return (Gaussian(xn,yn,sigma) - Gaussian(xn+d,yn,sigma));
}
//***************************************************************************
double Gaussian(double x, double y, double sigma)
{
// 0.39894228040143267794 = 1/sqrt(2*PI)
return 0.39894228040143267794/sigma*exp(-(x*x+y*y)/(2.0*sigma*sigma));
}
//***************************************************************************