#include "get_skew.h"
#include <stdio.h>
#include <time.h>
#include <sys/time.h>
#include "tcl_interface.h"

int docommand(char* fmt, ...);

struct angle_and_sd
{
  double angle;
  double sd;
};

#define DEBUG_GET_SKEW 1

double get_skew(RLEMap* r)
/*--------------------------------------------------------------
Primary Purpose: Determine the angle of rotation of the RLEMap r
Arguments: pointer to an RLEMap
Return Value: detected angle of rotation
Rev: AR
---------------------------------------------------------------*/
{

  int i,j,max_index;
  double max_sd = 0;
  double test_angle, angular_skip, angle_increment;
  Histogram* temp_hist;
  angle_and_sd Angles_and_Sds[20];

  struct timeval tv;
  double start_time;
  gettimeofday(&tv, NULL);
  start_time = (double)tv.tv_sec;
  printf("Starting deskew process: time = %lf\n", ((double) tv.tv_sec) - start_time);
  if(ENABLE_USER_INTERFACE)
    set_status("Finding Skew. . .");
/* first run, angular spacing = 1 degree */
  
  test_angle = -1;
  angular_skip = 1;  
  double loop_start_angle = 0;
  double loop_step = 1;
  for(i = 0; i < 2; i++)
    {
      temp_hist = project_histogram(r, loop_start_angle);
      Angles_and_Sds[0].angle = loop_start_angle;
      Angles_and_Sds[0].sd = temp_hist->get_standard_dev();
  
      temp_hist = project_histogram(r, loop_start_angle + loop_step);
      Angles_and_Sds[1].angle = loop_start_angle + loop_step;
      Angles_and_Sds[1].sd = temp_hist->get_standard_dev();
      
      if(Angles_and_Sds[1].sd > Angles_and_Sds[0].sd) 
	{
	  angle_increment = loop_step;
	  max_index = 1;
	  test_angle = Angles_and_Sds[1].angle + angle_increment;
	}
      else
	{
	  max_index = 0;
	  angle_increment = -loop_step;
	  test_angle = Angles_and_Sds[0].angle + angle_increment;
	}
      for(j = 2; j < 10; j++)
	{
	  temp_hist = project_histogram(r, test_angle);
	  Angles_and_Sds[j].angle = test_angle; 
	  Angles_and_Sds[j].sd = temp_hist->get_standard_dev(); 
	  if(Angles_and_Sds[j].sd > Angles_and_Sds[max_index].sd)
	    {
	      max_index = j;
	      test_angle += angle_increment;
	    }
	  else
	    break;
	}      
      loop_start_angle = Angles_and_Sds[max_index].angle;
      loop_step = 0.1;
    }
  gettimeofday(&tv, NULL);
  /*  printf("Done findinge skew.  Elapsed time = %lf\n", ((double) tv.tv_sec) - start_time); */
  if(ENABLE_USER_INTERFACE)
      {
	set_status("Skew angle = %.3lf", Angles_and_Sds[max_index].angle);
	update();
      }
  return Angles_and_Sds[max_index].angle;
}