import java.util.*;
import java.awt.*;

public class HoughLines
{
  static final boolean verbose = true;

  int[][] pspace;

  int rhodiv;
  double deltarho;

  int thetadiv;
  double deltatheta;

  public boolean findLines(int[][] img, int thetadiv, int rhodiv)
  {
    int ysize, xsize;
    int x, y;
    int i, j;
    double rho, theta;

    ysize = img.length;

    if (ysize <= 0) {
      if (verbose) System.out.println("findLines: ysize <= 0");
      return false;
    }

    xsize = img[0].length;

    if (xsize <= 0) {
      if (verbose) System.out.println("findLines: xsize <= 0");
      return false;
    }

    if (rhodiv < 2 || thetadiv < 2) {
      if (verbose) System.out.println("findLines: rhodiv or thetadiv too small");
      return false;
    }

    if ((rhodiv % 3) != 0)
      rhodiv += 3 - (rhodiv % 3);

    this.rhodiv = rhodiv;
    this.thetadiv = thetadiv;

    deltarho = Math.sqrt(xsize*xsize + ysize*ysize) / (double)((rhodiv - 1)/2);
    deltatheta = Math.PI / (double)(thetadiv - 1);

    pspace = new int[thetadiv][rhodiv];

    for (y = 0; y < ysize; y++) {
      for (x = 0; x < xsize; x++) {
        if (img[y][x] == 0)
          continue;
        for (i = 0, theta = 0; i < thetadiv; i++, theta += deltatheta) {
          rho = x*Math.cos(theta) + y*Math.sin(theta);
          j = Math.round((float)(rho / deltarho));
          j += (rhodiv-1)/2;
          if (j < 0 || j >= rhodiv) {
            if (verbose) {
              System.out.println("Broken rho computation!");
              System.out.println("theta = " + theta);
              System.out.println("rho = " + rho);
              System.out.println("j = " + j);
              System.out.println("max |j| = " + (rhodiv-1)/2);
              System.out.println("deltarho = " + deltarho);
              System.out.println("rho/deltarho = " + (float)(rho / deltarho));
            }
            return false;
          }
          pspace[i][j]++;
        }
      }
    }

    return true;
  }

  public boolean findBestLine(double[] outparams)
  {
    int maxi = 0, maxj = 0, max = 0;
    int i, j;

    if (pspace == null) {
      if (verbose) System.out.println("findBestLine called before findLines");
      return false;
    }

    if (outparams.length != 2) {
      if (verbose) System.out.println("findBestLine: outparams wrong size");
      return false;
    }

    for (i = 0; i < thetadiv; i++) {
      for (j = 0; j < rhodiv; j++) {
        if (pspace[i][j] > max) {
          max = pspace[i][j];
          maxi = i;
          maxj = j;
        }
      }
    }

    if (max == 0)
      return false;

    System.out.println("maxj = " + maxj);

    outparams[0] = maxi*deltatheta;  //theta
    outparams[1] = (maxj-(rhodiv-1)/2)*deltarho;  //rho

    return true;
  }

  public static void main(String args[])
  {
    HoughLines hl = new HoughLines();
    int[][] data;
    double[] params;
    int i;
    boolean ret;
    long start, end;
    int lx, rx;

    data = new int[100][200];
    params = new double[2];

    for (i = 0; i < 30; i++) {
//      data[i+12][2*i+15] = 1;
      data[50-i][2*i+15] = 1;
    }
    lx = 15;
    rx = 75;

    MapGraphics mg = new MapGraphics(200, 100);
    Frame f = mg.createFrame("test");
    mg.loadMap(data);

    start = System.currentTimeMillis();

    ret = hl.findLines(data, 200, 200);
    if (!ret)
      System.out.println("findBestLine returned " + ret);

    ret = hl.findBestLine(params);
    if (!ret)
      System.out.println("findBestLine returned " + ret);

    end = System.currentTimeMillis();

    System.out.println("line finding took " + (end-start) + "ms");

    System.out.println("theta = " + params[0]);
    System.out.println("rho = " + params[1]);

    double yint = params[1] / Math.sin(params[0]);
    double xint = params[1] / Math.cos(params[0]);
    double m = -1.0 / Math.tan(params[0]);
    System.out.println("yint = " + yint);
    System.out.println("xint = " + xint);
    System.out.println("m = " + m);
/*
    if (xint < 0) {
      if (yint > 0)
        mg.placeLine(0, (int)yint, 75, (int)(yint+m*75.0), Color.RED);
      //else, can't display!
    }
    else {
      mg.placeLine((int)xint, 0, 75, (int)(yint+m*75.0), Color.RED);
    }
*/
    mg.placeLine(lx, (int)(m*(lx-xint)), rx, (int)(m*(rx-xint)), Color.RED);
    System.out.println("lx = " + lx + " ly = " + (int)(m*(lx-xint)) + " rx = " + rx + " ry = " + (int)(m*(rx-xint)));
    mg.repaint();
  }
}