//**************************************************************************
#include <iostream.h>
#include <fstream.h>
#include <math.h>
#include "matrix.h"
#include "gabor.h"
#include "gaussian.h"
#include "edgeflow.h"
//**************************************************************************
// Filter mask size is 2*NUM_BOUNDARY_PIXELS_FOR_PADDING+1 squared!
#define NUM_BOUNDARY_PIXELS_FOR_PADDING 60
//**************************************************************************
extern int debugflag;
extern int textureflag;
extern int intensityflag;
//**************************************************************************
void ComputeEdgeFlowVector(Matrix *Fr, Matrix *Fi, 
        Matrix &imgR, Matrix &imgG, Matrix &imgB, 
        int numorient, double sigma)
{
int height = imgR.nx;
int width = imgR.ny;

int N = numorient;      // number of different theta values

Matrix *E, *P;
E = allocateMatrixArray(N, height, width);
P = allocateMatrixArray(N, height, width);

for (int i = 0; i < N; i++)
        {
         int side = NUM_BOUNDARY_PIXELS_FOR_PADDING;
         double theta = (i*2.0/N)*Pi;
         cout << endl << "Calculating Total Edge Flow for theta = " 
                << theta*RAD2DEG << " deg" << endl;
         TotalEdgeFlowRGB(&E[i], &P[i], imgR, imgG, imgB, sigma, theta, numorient, side);
        }

if (debugflag)
        {
         printMatrixArray(E, "TotalE.dbg", N, height, width);
         printMatrixArray(P, "TotalP.dbg", N, height, width);
        }

// Compute edge flow vector
cout << endl << "Computing Edge Flow Vector from above data...";
cout.flush();

Fr->Zero();
Fi->Zero();
for (int h = 0; h < height; h++)
        for (int w = 0; w < width; w++)
                {
                 int index = -1;
                 double maxP = -1.0e+300;
                 for (int k = 0; k < N; k++)
                        {
                         double sumP = 0.0;
                         for (int i = 0; i < N/2; i++)
                                {
                                 int m = (i + k)%N;
                                 sumP += P[m].p[h][w];          // P(s,theta)
                                }
                         if (sumP > maxP)
                                {
                                 maxP = sumP;
                                 index = k;
                                }
                        }
                 for (int k = 0; k < N/2; k++)
                        {
                         int m = (index + k)%N;
                         double theta = (m*2.0/N)*Pi;
                         Fr->p[h][w] += E[m].p[h][w]*cos(theta);
                         Fi->p[h][w] += E[m].p[h][w]*sin(theta);
                        }
                }

double norm_r = Fr->Norm();
double norm_i = Fi->Norm();
double norm = sqrt(norm_r*norm_r + norm_i*norm_i);
if (norm > 0.0)
        {
         Fr->scale(1.0/norm);
         Fi->scale(1.0/norm);
        }

#ifdef FREEMEM
deallocateMatrixArray(E, N);
deallocateMatrixArray(P, N);
#endif

cout << "done!" << endl;
cout.flush();
}
//**************************************************************************
void TotalEdgeFlowRGB(Matrix *E, Matrix *P, 
        Matrix &imgR, Matrix &imgG, Matrix &imgB, 
        double sigma, double theta, int numorient, int side)
{
cout << "  Calling TotalEdgeFlow..." << endl;

int height = imgR.nx;
int width = imgR.ny;

// Intensity Edge Flow
Matrix TmpE(height, width);
Matrix TmpP(height, width);

E->Zero();
P->Zero();

if (intensityflag) {
if (imgR == imgG)
        {
         cout << "    GRAY:  ";
         IntensityEdgeFlow(E, P, imgR, sigma, theta, side);
        }
else
        {
         // Red
         cout << "    RED:   ";
         IntensityEdgeFlow(&TmpE, &TmpP, imgR, sigma, theta, side);
         *E += TmpE;
         *P += TmpP;

         // Green
         cout << "    GREEN: ";
         IntensityEdgeFlow(&TmpE, &TmpP, imgG, sigma, theta, side);
         *E += TmpE;
         *P += TmpP;

         // Blue
         cout << "    BLUE:  ";
         IntensityEdgeFlow(&TmpE, &TmpP, imgB, sigma, theta, side);
         *E += TmpE;
         *P += TmpP;

         E->scale(1.0/3.0);
         P->scale(1.0/3.0);
        }

if (debugflag)
        {
         E->WriteImage("IntensityE.dbg", "gif");
         P->WriteImage("IntensityP.dbg", "gif");
        }

//E->Normalize();
E->scale(0.6);
P->scale(0.6);
        } // end intensityflag

if (textureflag) {
// Texture Edge Flow
int scale = 4;
int orientation = numorient;
double Ul = 0.25/sigma; //0.1;
double Uh = 0.45;       //0.4;
int flag = 0;           // Remove DC component

Matrix img(height, width);
img += imgR;
img += imgG;
img += imgB;

TextureEdgeFlow(&TmpE, &TmpP, img, 
        theta, side, Ul, Uh, sigma, scale, 6, flag);

if (debugflag)
        {
         TmpE.WriteImage("TextureE.dbg", "gif");
         TmpP.WriteImage("TextureP.dbg", "gif");
        }

//TmpE.Normalize();
TmpE.scale(0.4);
TmpP.scale(0.4);

*E += TmpE;
*P += TmpP;
        } // end textureflag

cout << "  Exiting TotalEdgeFlow." << endl;
cout.flush();
}
//**************************************************************************
void IntensityEdgeFlow(Matrix *E, Matrix *P, Matrix &img, 
        double sigma, double theta, int side)
{
cout << "Calling IntensityEdgeFlow...";
cout.flush();

int height = img.nx;
int width = img.ny;

FilteredImg(E, img, GDmask, sigma, theta, side);
E->Abs();
if (debugflag)
        {
         E->WriteImage("GDafter.dbg", "gif");
        }

Matrix Err1(height, width), Err2(height, width), Tmp(height, width);

FilteredImg(&Err1, img, DOOGmask, sigma, theta, side);
Err1.Abs();
if (debugflag)
        {
         Err1.WriteImage("DOOGafter.dbg", "gif");
        }

//Err1.WriteImage("Err1.dbg", "gif");

FilteredImg(&Err2, img, DOOGmask, sigma, theta+Pi, side);
Err2.Abs();
//Err2.WriteImage("Err2.dbg", "gif");

Tmp = Err1 + Err2;
//Tmp.WriteImage("ErrSum.dbg", "gif");

// P(s,theta)
*P = Err1/Tmp;

cout << "done!" << endl;
cout.flush();
}
//**************************************************************************
void TextureEdgeFlow(Matrix *E, Matrix *P, Matrix &img, 
        double theta, int side, double Ul, double Uh, double sigma, 
        int scale, int orientation, int flag)
{
static int firstcall = 1;
static Matrix **F_r, **F_i;

cout << "    Calling TextureEdgeFlow." << endl;

int height = img.nx;
int width = img.ny;

if (firstcall)
        {
         F_r = allocateMatrixArray(scale, orientation, height, width);
         F_i = allocateMatrixArray(scale, orientation, height, width);

         GaborFilteredImg(F_r, F_i, img, side, Ul, Uh, scale, orientation, flag);

         firstcall = 0;
        }

Matrix Err1(height, width), Err2(height, width), Tmp(height, width);
E->Zero();

cout << "      TextureEdgeFlow: Processing Image 0/" << scale*orientation;
for (int i = 0; i < scale; i++)
        for (int j = 0; j < orientation; j++)
                {
                 cout << "\r      TextureEdgeFlow: Processing Image " << i*orientation+j+1 << "/" << scale*orientation;
                 cout.flush();

                 // Converts: F_r -> mag, F_i -> phase
                 Complex2Polar(&F_r[i][j], &F_i[i][j]);

                 // Calculate E(s,theta)
                 FilteredImg(&Tmp, F_r[i][j], GDmask, sigma, theta, side);
                 Tmp.Abs();
                 Tmp.NormalizeEnergy();
                 *E += Tmp;

                 // Calculate Error(s,theta)
                 FilteredImg(&Tmp, F_r[i][j], DOOGmask, sigma, theta, side);
                 Tmp.Abs();
                 Tmp.NormalizeEnergy();
                 Err1 += Tmp;

                 // Calculate Error(s,theta+Pi)
                 FilteredImg(&Tmp, F_r[i][j], DOOGmask, sigma, theta+Pi, side);
                 Tmp.Abs();
                 Tmp.NormalizeEnergy();
                 Err2 += Tmp;
                }
cout << "...done!" << endl;
cout.flush();

if (debugflag)
        {
         printMatrixArray(F_r, "TextureMatR.dbg", scale, orientation, height, width);
         printMatrixArray(F_i, "TextureMatI.dbg", scale, orientation, height, width);
        }

Tmp = Err1 + Err2;
*P = Err1/Tmp;  // P(s, theta)

/*
#ifdef FREEMEM
deallocateMatrixArray(F_r, scale, orientation);
deallocateMatrixArray(F_i, scale, orientation);
#endif
*/

cout << "    Exiting TextureEdgeFlow." << endl;
cout.flush();
}
//**************************************************************************