#ifndef CANNY_EDGE_MACHINE_C
#define CANNY_EDGE_MACHINE_C

#include <QtGlobal>
#include <QMainWindow>
#include <QColor>

#include <iostream>

#include <math.h>
#include "lazy_image.cpp"

class CannyEdgeMachine {
  
  private:
    LazyImage* original;
    LazyImage* working_copy;
     
    int width;
    int height;
    int pixels;
    double* gradient_magnitude;
    double* maximum_magnitude;
    int* binary_edge_pixels;
    int* gradient_x;
    int* gradient_y;
    
    // Params
    int filter_size;
    double filter_sigma;
    double t_low;
    double t_high;
    
    double gauss(int x, int y, double sigma) {
      // e^-((x^2 + y^2)/(sigma^2))
      double upper_part = pow(M_E, (-1.0) * ( (pow(x, 2)+pow(y, 2))/pow(sigma, 2) ) );
      return upper_part;
    };
    
  public:
    CannyEdgeMachine(LazyImage* original, LazyImage* working_copy){
      this->original = original;
      this->working_copy = working_copy;
      
      this->width = this->original->width();
      this->height = this->original->height();
      this->pixels = this->width * this->height;
      
      this->gradient_magnitude = (double*) malloc(sizeof(double) * this->pixels);
      this->maximum_magnitude = (double*) malloc(sizeof(double) * this->pixels);
      this->binary_edge_pixels = (int*) malloc(sizeof(int) * this->pixels);
      this->gradient_x = (int*) malloc(sizeof(int) * this->pixels);
      this->gradient_y = (int*) malloc(sizeof(int) * this->pixels);
    };
    
    ~CannyEdgeMachine() {
      free(this->gradient_magnitude);
      free(this->maximum_magnitude);
      free(this->binary_edge_pixels);
      free(this->gradient_x);
      free(this->gradient_y);
    };
    
    void setGaussFilterParams(int filter_size, double filter_sigma) {
      this->filter_size = filter_size;
      this->filter_sigma = filter_sigma;
    };
    
    void setThresholdValues(double t_low, double t_high) {
      this->t_low = t_low;
      this->t_high = t_high;
    };
    
    void reset(void) {
      for(int i=0; i<this->pixels; i++) {
	this->gradient_magnitude[i] = 0;
	this->maximum_magnitude[i] = 0;
	this->binary_edge_pixels[i] = 0;
	this->gradient_x[i] = 0;
	this->gradient_y[i] = 0;
      }
    };
    
    void doGaussBlur(void) {
      int filter_width = this->filter_size;
      double sigma = this->filter_sigma;
      // build the gauss filter
      double* filter = (double*) malloc(sizeof(double) * filter_width * filter_width);
      double sum_weights = 0;
      std::cout << "Using gauss filter: " << std::endl;
      for(int x=0; x<filter_width; x++) {
	for(int y=0; y<filter_width; y++) {
	  filter[x*filter_width + y] = this->gauss(x, y, sigma);
	  sum_weights += filter[x*filter_width + y];
	  std::cout << "(" << x << ", " << y << ") " << filter[x*filter_width + y] << " | ";
	}
        std::cout << std::endl;
      }
      std::cout << std::endl << "Sum weights: " << sum_weights << std::endl;
      // apply gauss filter
      int filter_offset = (filter_width+1)/2;
      for(int x=(0+filter_offset); x<(this->width-filter_offset); x++) {
        for(int y=(0+filter_offset); y<(this->height-filter_offset); y++) {
	  double sum_intensity = 0.0;
	  int h, s, l;
	  for(int fx=0; fx<filter_width; fx++) {
	    int dx = fx - (filter_width / 2);
	    for(int fy=0; fy<filter_width; fy++) {
	      int dy = fy - (filter_width / 2);
	      QColor color = QColor::fromRgb(this->original->getPixel(x+dx, y+dy, LazyImage::DEFAULT));
	      color.getHsl(&h, &s, &l);
	      sum_intensity += (l * filter[fy*filter_width + fx]);
            }	
          }
          QColor color = QColor::fromRgb(this->original->getPixel(x, y, LazyImage::DEFAULT));
          color.getHsl(&h, &s, &l);
          l = qRound(sum_intensity/sum_weights);
	  if(l > 255) l = 255;
	  if(l < 0) l = 0;
	  color.setHsl(h, s, l);
	  this->working_copy->getImage()->setPixel(x, y, color.rgb());
        }
      }
      free(filter);
    };
    
    void doGradiants(void) {
      // build the gradiant vector
      int gradiant_size = 3;
      double* gradiant_vector = (double*) malloc(sizeof(double) * gradiant_size);
      gradiant_vector[0] = -0.5;
      gradiant_vector[1] = 0;
      gradiant_vector[2] = 0.5;
      int gradiant_offset = gradiant_size/2;
      // calculate gradiants
      double sum_intensity;
      for(int x=(0+gradiant_offset); x<(this->width-gradiant_offset); x++) {
        for(int y=(0+gradiant_offset); y<(this->height-gradiant_offset); y++) {
	  int h, s, l;
	  // x gradiant
          sum_intensity = 0;
	  for(int i=0; i<gradiant_size; i++) {
	    int dx = i - gradiant_offset;
	    QColor color = QColor::fromRgb(this->working_copy->getPixel(x+dx, y, LazyImage::DEFAULT));
	    color.getHsl(&h, &s, &l);
	    sum_intensity += l * gradiant_vector[i];
	  }
	  this->gradient_x[y*this->width + x] = sum_intensity;
	  // y gradiant
          sum_intensity = 0;
	  for(int i=0; i<gradiant_size; i++) {
	    int dy = i - gradiant_offset;
	    QColor color = QColor::fromRgb(this->working_copy->getPixel(x, y+dy, LazyImage::DEFAULT));
	    color.getHsl(&h, &s, &l);
	    sum_intensity += l * gradiant_vector[i];
	  }
	  this->gradient_y[y*this->width + x] = sum_intensity;
        }
      }
      free(gradiant_vector);
    };
    
    void doGradiantMagnitude(void) {
      for(int x=0; x<this->width; x++) {
	for(int y=0; y<this->height; y++) {
	  int gradiant_x = this->gradient_x[y*this->width + x];
	  int gradiant_y = this->gradient_y[y*this->width + x];
	  this->gradient_magnitude[y*this->width + x] = sqrt(pow(gradiant_x, 2) + pow(gradiant_y, 2));
	}
      }
    };
    
    int getOrientationSector(double dx, double dy) {
      // Matrix multiplication with rotation matrix pi/8
      //
      // cos(pi/8) -sin(pi/8)
      // sin(pi/8) cos(pi/8)
      // TODO: Move these somewhere else!
      double octangle = M_PI/8;
      double cosoct = cos(octangle);
      double sinoct = sin(octangle);
      double neg_sinoct = -sinoct;
      // Do matrix multiplication
      double new_dx = dx * cosoct + dy * neg_sinoct;
      double new_dy = dx * sinoct + dy * cosoct;
      if(new_dy < 0) {
	new_dx = -new_dx;
	new_dy = -new_dy;
      }
      int orientation_sector;
      if(new_dx >= 0 && new_dx >= new_dy) orientation_sector = 0;
      if(new_dx >= 0 && new_dx < new_dy) orientation_sector = 1;
      if(new_dx < 0 && -new_dx < new_dy) orientation_sector = 2;
      if(new_dx < 0 && -new_dy >= new_dy) orientation_sector = 3;
      return orientation_sector;
    };
    
    bool isLocalMax(int x, int y, int orientation_sector) {
      double local_magnitude = this->gradient_magnitude[y * this->width + x];
      if(local_magnitude < this->t_low) {
	return false;
      } else {
	int magnitude_l, magnitude_r;
	switch(orientation_sector) {
	  case 0:
	    magnitude_l = this->gradient_magnitude[y * this->width + (x-1)];
	    magnitude_r = this->gradient_magnitude[y * this->width + (x+1)];
	    break;
	  case 1:
	    magnitude_l = this->gradient_magnitude[(y-1) * this->width + (x-1)];
	    magnitude_r = this->gradient_magnitude[(y+1) * this->width + (x+1)];
	    break;
	  case 2:
	    magnitude_l = this->gradient_magnitude[(y-1) * this->width + x];
	    magnitude_r = this->gradient_magnitude[(y+1) * this->width + x];
	    break;
	  case 3:
	    magnitude_l = this->gradient_magnitude[(y-1) * this->width + (x+1)];
	    magnitude_r = this->gradient_magnitude[(y+1) * this->width + (x-1)];
	    break;
	}
	return ((magnitude_l <= local_magnitude) && (local_magnitude > magnitude_r));
      }
    };
    
    void filterLocalMaxima(void) {
      for(int x=1; x<this->width-2; x++) {
	for(int y=1; y<this->height-2; y++) {
	  double dx = this->gradient_x[y*this->width + x];
	  double dy = this->gradient_y[y*this->width + x];
	  // get orientation sector
	  int orientation_sector = this->getOrientationSector(dx, dy);
	  if(this->isLocalMax(x, y, orientation_sector)) {
	    this->maximum_magnitude[y*this->width + x] = this->gradient_magnitude[y*this->width + x];
	  }
	}
      }
    };
    
    void traceAndThreshold(int x, int y) {
      this->binary_edge_pixels[y*this->width + x] = 1;
      int x_l = std::max(x-1, 0);
      int x_r = std::min(x+1, this->width-1);
      int y_l = std::max(y-1, 0);
      int y_r = std::min(y+1, this->height-1);
      for(int x=x_l; x<=x_r; x++) {
	for(int y=y_l; y<y_r; y++) {
	  if((this->maximum_magnitude[y*this->width + x] >= this->t_high)
	    && (this->binary_edge_pixels[y*this->width + x] == 0)) {
	    this->traceAndThreshold(x, y);
	  }
	}
      }
    };
    
    void workLocalMaxima(void) {
      for(int x=1; x<this->width-2; x++) {
	for(int y=1; y<this->height-2; y++) {
	  if((this->maximum_magnitude[y*this->width + x] >= this->t_high)
	    && (this->binary_edge_pixels[y*this->width + x] == 0)) {
	    this->traceAndThreshold(x, y);
	  }
	}
      }
    };
    
    void showEdges(void) {
      QRgb black = QColor::fromRgb(0, 0, 0).rgb();
      QRgb white = QColor::fromRgb(255, 255, 255).rgb();
      for(int x=0; x<this->width; x++) {
	for(int y=0; y<this->height; y++) {
	  int pixel = this->binary_edge_pixels[y*this->width + x];
	  if(pixel > 0) {
	    this->working_copy->getImage()->setPixel(x, y, black);
	  } else {
	    this->working_copy->getImage()->setPixel(x, y, white);
	  }
	}
      }
    };
    
    void work() {
      this->reset();
      this->doGaussBlur(); // uses filter_size, filter_sigma
      this->doGradiants();
      this->doGradiantMagnitude();
      // TODO: Checkpoint for 'before t_low changed'
      this->filterLocalMaxima(); // uses t_low only
      // TODO: Checkpoint for 'before t_high changed'
      this->workLocalMaxima(); // uses t_low & t_high
      this->showEdges();
    };
    
    void doFirstStep(void) {
      this->reset();
      this->doGaussBlur(); // uses filter_size, filter_sigma
      this->doGradiants();
      this->doGradiantMagnitude();      
    };
    
    void doSecondStep(void) {
      // Reset maximum_magnitude since it will be rebuilt here
      for(int i=0; i<this->pixels; i++) this->maximum_magnitude[i] = 0;
      this->filterLocalMaxima(); // uses t_low only
    };
    
    void doThirdStep(void) {
      // Reset binary_edge_pixels since they will be rebuilt here
      for(int i=0; i<this->pixels; i++) this->binary_edge_pixels[i] = 0;
      this->workLocalMaxima(); // uses t_low & t_high
      this->showEdges();      
    };
    
    int* getBinaryEdgePixels(void) {
      return this->binary_edge_pixels;
    };
    
};

#endif