#ifndef HOUGH_MACHINE_C
#define HOUGH_MACHINE_C

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

#include <iostream>

#include <math.h>

#include "lazy_image.cpp"

struct hparam {
  double theta;
  double r;
  int value;
};

class HoughMachine {
  
  private:
    LazyImage* original;
    LazyImage* working_copy;
    
    int x_center;
    int y_center;
    int n_ang;
    double d_ang;
    int n_rad;
    double d_rad;
    int* hough_arr;
    
    int matching_threshold;
    int bresenham_orientation_sector;

  public:
    HoughMachine(LazyImage* original, LazyImage* working_copy) {
      this->original = original;
      this->working_copy = working_copy;
      this->hough_arr = NULL;
    };
    
    ~HoughMachine() {
      this->reset();
    };
    
    void reset(void) {
      this->x_center = 0;
      this->y_center = 0;
      this->n_ang = 0;
      this->d_ang = 0;
      this->n_rad = 0;
      this->d_rad = 0;
      if(this->hough_arr != NULL) {
	free(this->hough_arr);
	this->hough_arr = NULL;
      }
      this->matching_threshold = 0;
      this->bresenham_orientation_sector = 0;
    };
    
    void setMatchingThreshold(int matching_threshold) {
      // If negative, pixels greater than the absolute value match.
      // If positive, pixels less than the absolute value match.
      this->matching_threshold = matching_threshold;
    };
        
    void run(int a_steps, int r_steps) {
      this->x_center = this->original->width()/2;
      this->y_center = this->original->height()/2;
      this->n_ang = a_steps;
      this->d_ang = M_PI / this->n_ang;
      this->n_rad = r_steps;
      double r_max = sqrt(pow(this->x_center, 2) + pow(this->y_center, 2));
      this->d_rad = (2*r_max)/this->n_rad;
      this->hough_arr = (int*) malloc(sizeof(int) * this->n_ang * this->n_rad);
      for(int i=0; i< this->n_ang*this->n_rad; i++) this->hough_arr[i] = 0;
      std::cout << "Hough params: x_center: " << this->x_center << ", y_center: " << this->y_center << std::endl;
      std::cout << "n_ang: " << this->n_ang << ", d_ang: " << this->d_ang << std::endl;
      std::cout << "n_rad: " << this->n_rad << ", d_rad: " << this->d_rad << ", r_max: " << r_max << std::endl;
      this->fillHoughAccumulator();
      this->debugShow();
    };
    
    void fillHoughAccumulator() {
      for(int v=0; v<this->original->height(); v++) {
	for(int u=0; u<this->original->width(); u++) {
	  QColor pixel = QColor::fromRgb(this->original->getPixel(u, v, LazyImage::DEFAULT));
	  int h, s, l;
	  pixel.getHsl(&h, &s, &l);
	  if(this->matching_threshold > 0) {
	    int threshold = this->matching_threshold;
	    if(l < threshold) this->doPixel(u, v);
	  } else {
	    int threshold = -this->matching_threshold;
	    if(l > threshold) this->doPixel(u, v);
	  }
	}
      }
    };
    
    void doPixel(int u, int v) {
      int x = u - this->x_center;
      int y = v - this->y_center;
      for(int a=0; a<this->n_ang; a++) {
	double theta = this->d_ang * a;
	int r = (int) qRound( (x*cos(theta) + y*sin(theta)) / this->d_rad ) + this->n_rad / 2;
	if(r >= 0 && r < this->n_rad) {
	  this->hough_arr[a*this->n_ang+r] += 1;
	}
      }
    };
    
    void drawLines(int n) {
      // for each value: if == 0 ignore
      // else check if bigger than all neighbours
      // if so, put it in the list
      struct hparam* lines = (struct hparam*) malloc(sizeof(struct hparam) * this->n_ang * this->n_rad);
      int line = 0;
      for(int i=0; i<this->n_ang*this->n_rad; i++) {
	int value = this->hough_arr[i];
	if(value <= 5) continue; // Ignore small values
	// TODO: Check neighbours and compare!
	// Store value if bigger than neighbours
	int a = i / this->n_ang;
	int r = i - (a*this->n_ang);
	double theta = this->d_ang * a;
	//std::cout << "[" << theta << ", " << r << "] " << value << std::endl;
	lines[line].r = r;
	lines[line].theta = theta;
	lines[line].value = value;
	line++;
      }
      // Now sort it
      qsort((void*) lines, line, sizeof(struct hparam), HoughMachine::comp);
      // Draw them
      double pi_half = M_PI / 2.0;
      for(int i=0; i<n; i++) {
	double theta = lines[i].theta;
	int r = lines[i].r;
	int quadrant = floor(theta / pi_half);
	std::cout << "Top " << i << ": " << lines[i].value << ", r: " << r << ", theta: " << theta << " quadrand: " << quadrant;

	// cos() -sin()
        // sin() cos()
        // Do matrix multiplication
	// Turn r into a x/y vector
        double vec_x = r * cos(theta);// + 0 * -sin(theta);
        double vec_y = r * sin(theta);// + 0 * cos(theta);
	// find point in image
	double x = vec_x + this->x_center;
	double y = vec_y + this->y_center;
	std::cout << " vec x: " << x << ", vec y: " << y << std::endl;
	// create vector for line
	double vec_target_x = vec_y;
	double vec_target_y = -vec_x;
	// normalize it
	double vec_target_len = sqrt(pow(vec_target_x, 2) + pow(vec_target_y, 2));
	vec_target_x = vec_target_x / vec_target_len;
	vec_target_y = vec_target_y / vec_target_len;
	// find first border
	double i = x;
	double j = y;
	while(i < this->working_copy->width() && i > 0 && j < this->working_copy->height() && j > 0) {
	  i += vec_target_x;
	  j += vec_target_y;
	}
	// find second border
	int x1 = i;
	int y1 = j;
	i = x;
	j = y;
	while(i < this->working_copy->width() && i > 0 && j < this->working_copy->height() && j > 0) {
	  i -= vec_target_x;
	  j -= vec_target_y;
	}
	int x2 = i;
	int y2 = j;
	// TODO: Draw the lines
	this->otherBresenham(x1, y1, x2, y2);
      }
      
    };
    
    static int comp(const void* a, const void* b) {
      struct hparam param_a = *((struct hparam*) a);
      struct hparam param_b = *((struct hparam*) b);
      int f = param_a.value;
      int s = param_b.value;
      if (f > s) return -1;
      if (f < s) return 1;
      return 0;
    }
        
    void setPixelDebug(int x, int y, int color=0) {
      int our_color = QColor::fromRgb(0, 255, 0).rgb();
      if(color == 0) color = our_color;
      //std::cout << " --> SET (" << x << ", " << y << ")" << std::endl;
      if(x < 0 || y < 0 || x > this->working_copy->width() || y > this->working_copy->height()) return;
      this->working_copy->getImage()->setPixel(x, y, color);
    };
    
    void otherBresenham(int x, int y, int x2, int y2) {
      int width = x2 - x;
      int height = y2 - y;
      int dx1 = 0, dy1 = 0, dx2 = 0, dy2 = 0;
      dx1 = (width < 0) ? -1 : 1;
      dy1 = (height < 0) ? -1 : 1;
      dx2 = (width < 0) ? -1 : 1;
      int longest = abs(width);
      int shortest = abs(height);
      if(longest <= shortest) {
        longest = abs(height);
        shortest = abs(width);
        dy2 = (height < 0) ? -1 : 1;
        dx2 = 0;            
      }
      int numerator = longest / 2;
      for(int i=0; i<=longest; i++) {
        this->setPixelDebug(x, y);
        numerator += shortest;
        if(numerator >= longest) {
          numerator -= longest;
          x += dx1;
          y += dy1;
        } else {
          x += dx2;
          y += dy2;
        }
      }      
    };
    
    void debugShow() {
      std::cout << "Dimensions: " << this->n_ang << " x " << this->n_rad << " | " << std::endl;
      int max_value = 0;
      for(int i=0; i<this->n_ang*this->n_rad; i++) {
	int current_value = this->hough_arr[i];
	if(current_value > max_value) max_value = current_value;
      }
      
      for(int x=0; x<this->n_ang; x++) {
	for(int y=0; y<this->n_rad; y++) {
	  int value = qRound(255.0*this->hough_arr[x*this->n_ang+y] / max_value);
	  if(value < 0) {
	    std::cout << value << std::endl;
	    value = 0;
	  }    
	  if(value > 255) {
	    std::cout << value << std::endl;
	    value = 255;
	  }
	  int color = QColor::fromRgb(value, value, value).rgb();
	  this->working_copy->getImage()->setPixel(x,y,color);
	}
      }
    };
    
    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;
      int orientation_sector = 0;
      if(new_dy < 0) {
	new_dx = -new_dx;
	new_dy = -new_dy;
	orientation_sector += 4;
      }
      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_dx >= new_dy) orientation_sector += 3;
      return orientation_sector;
    };

    void drawLineBresenham(int start_x, int start_y, int end_x, int end_y) {
      this->bresenham_orientation_sector = this->getOrientationSector(end_x - start_x, end_y - start_y);
      std::cout << "START LINE: (" << start_x << ", " << start_y << ") -> (" << end_x << ", " << end_y << ") - O: " << this->bresenham_orientation_sector << std::endl;
      // other implementation for now.
      this->otherBresenham(start_x, end_x, start_y, end_y);
      // Now adapt input
      // Output will be adapted in setPixel()
      /*switch(this->bresenham_orientation_sector) {
	case 0:
	  // Going right, nothing to do here
	  this->bresenham(start_x, start_y, end_x, end_y);
	  break; 
	case 1:
	  // Going right, but more up
	  this->bresenham(start_y, start_x, end_y, end_x);
	  break;
	case 2:
	  // Going left, but more up
	  this->bresenham(start_y, end_x, end_y, start_x);
	  break;
	case 3:
	  // Going left
	  this->bresenham(end_x, start_y, start_x, end_y);
	  break;
	case 4:
	  // Going left
	  this->bresenham(end_x, end_y, start_x, start_y);
	  break; 
	case 5:
	  // Going left, but more down
	  this->bresenham(end_y, end_x, start_y, start_x);
	  break;
	case 6:
	  // Going right, but more down
	  this->bresenham(start_x, end_y, end_x, start_y);
	  break;
	case 7:
	  // Going right
	  this->bresenham(start_x, end_y, end_x, start_y);
	  break;
      }*/
    };
    
    /*void bresenham(int start_x, int start_y, int end_x, int end_y) {
      std::cout << "BRESENHAM START LINE: (" << start_x << ", " << start_y << ") -> (" << end_x << ", " << end_y << ")" << std::endl;
      int x, y;
      int delta_x, delta_y;
      int d;
      int delta_ne, delta_e;
      x = start_x;
      y = start_y;
      delta_x = end_x - start_x;
      delta_y = end_y - start_y;
      delta_ne = 2 * (delta_y - delta_x);
      delta_e = 2 * delta_y;
      d = 2 * delta_y - delta_x;
      this->setPixel(start_x, start_y, x, y);
      while(x < end_x) {
	if(d >= 0) {
	  d += delta_ne; x++; y++;
	} else {
	  d += delta_e; x++;
	}
        this->setPixel(start_x, start_y, x, y);
      }
      std::cout << std::endl;
    };*/
    
    /*void setPixel(int start_x, int start_y, int dx, int dy) {
      std::cout << "setPixel(" << start_x << ", " << start_y << ", " << dx << ", " << dy << ")" << std::endl;
      if(dx > start_x) dx = dx - start_x;
      else dx = start_x - dx;
      if(dy > start_y) dy = dy - start_y;
      else dy = start_y - dy;
      std::cout << " --> dx: " << dx << ", dy: " << dy << "" << std::endl; 
      
      int color = QColor::fromRgb(0, 255, 0).rgb();
      //TODO: Fix this! This has to happen relative to the center point! :|
      switch(this->bresenham_orientation_sector) {
	case 0:
	  // Going right, nothing to do here
	  this->setPixelDebug(start_x + dx, start_y + dy, color);
	  break; 
	case 1:
	  // Going right, but more up
	  this->setPixelDebug(start_x + dy, start_y + dx, color);
	  break;
	case 2:
	  // Going left, but more up
	  this->setPixelDebug(start_x - dy, start_y + dx, color);
	  break;
	case 3:
	  // Going left
	  this->setPixelDebug(start_x - dx, start_y + dy, color);
	  break;
	case 4:
	  // Going left
	  this->setPixelDebug(start_x - dx, start_y - dy, color);
	  break; 
	case 5:
	  // Going left, but more down
	  this->setPixelDebug(start_x + dy, start_y + dx, color); // ok
	  break;
	case 6:
	  // Going right, but more down
	  this->setPixelDebug(start_x + dy, start_y - dx, color); // ?
	  break;
	case 7:
	  // Going right
	  this->setPixelDebug(start_x + dx, start_y - dy, color);
	  break;
      }

    };*/

};


#endif