#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;

}