// file: $USFS/util/error_eval/v6.1/error_proc_03.cc
//

// system include files
//
#include <math.h>

// isip include files
//
#include "error_eval.h"
#include "error_eval_constants.h"

// function: compu_param_cc
//
// arguments:
//  int_4* line_ref: (input) pointer to the reference line data array
//  int_4 num_coord_ref: (input) number of coordinate data in the reference
//                       line data array
//  int_4* line_res: (input) pointer to the detected line data array
//  int_4 num_coord_res: (input) number of coordinate data in the detected
//                       line data array
//  float_8& dist: (output) distance from the middle point of the detected
//                 line to the reference line
//  float_8& angle: (output) angle between the detected line and the
//                  reference line
//  float_8& len_dif: (output) length difference between the detected line
//                    and the reference line
//
// return:
//  a logical_1 value indicating status
//
// this function computes parameters needed in line detector evaluation
//
logical_1 compu_param_cc(int_4* line_ref_a, int_4 num_coord_ref_a,
			 int_4* line_res_a, int_4 num_coord_res_a,
			 float_8& dist_a, float_8& angle_a,
			 float_8& len_dif_a) {

  // declare local vaiables
  //  
  int_4 ref_end_x0 = line_ref_a[0];
  int_4 ref_end_y0 = line_ref_a[1];
  int_4 ref_end_x1 = line_ref_a[num_coord_ref_a - 2];
  int_4 ref_end_y1 = line_ref_a[num_coord_ref_a - 1];

  int_4 res_end_x0 = line_res_a[0];
  int_4 res_end_y0 = line_res_a[1];
  int_4 res_end_x1 = line_res_a[num_coord_res_a - 2];
  int_4 res_end_y1 = line_res_a[num_coord_res_a - 1];

  float_8 ref_k = 0, res_k = 0;
  float_8 ref_angle = 0, res_angle = 0;
  
  float_8 res_mid_x = (float_8)line_res_a[num_coord_res_a / 2 - 1];
  float_8 res_mid_y = (float_8)line_res_a[num_coord_res_a / 2];

  if ((num_coord_res_a / 2) % 2 == 0) {

    // the points number of the detected line is even
    //
    res_mid_x = (float_8)(line_res_a[num_coord_res_a / 2 - 2] +
		line_res_a[num_coord_res_a / 2]) / 2.0;
    res_mid_y = (float_8)(line_res_a[num_coord_res_a / 2 - 1] +
		line_res_a[num_coord_res_a / 2 + 1]) / 2.0;
  }
  
  // check if the middle point of the detected line falls inside the range
  // of the reference line
  //
  if ((res_mid_y <= min(ref_end_y0, ref_end_y1)) ||
      (res_mid_y >= max(ref_end_y0, ref_end_y1))) {

    // not a match line
    //
    dist_a = -1;
    angle_a = -1;
    len_dif_a = -1;
    return ISIP_TRUE;
  }

  // compute the distance from the detected line to the reference line
  //
  if (ref_end_x1 == ref_end_x0) {
    ref_angle = ISIP_TWOPI / 4;
    dist_a = (float_8)(fabs(res_mid_x - ref_end_x1));
  }
  else { 
    ref_k = ((float_8)(ref_end_y1 - ref_end_y0)) /
            ((float_8)(ref_end_x1 - ref_end_x0));
    ref_angle = atan(ref_k);
    
    float_8 x = (ref_k * (res_mid_y - ref_end_y0) + ref_k * ref_k *
		 ref_end_x0 + res_mid_x) / (ref_k * ref_k + 1);
    float_8 y = ref_end_y0 + ref_k * (x - ref_end_x0);

    dist_a = sqrt((res_mid_y - y) * (res_mid_y - y) +
           (res_mid_x - x) * (res_mid_x - x));
  }
  
  // compute the angle between the detected line and the reference line
  //
  if (res_end_x1 == res_end_x0) {
    res_angle = ISIP_TWOPI / 4;
  }
  else { 
    res_k = ((float_8)(res_end_y1 - res_end_y0)) /
            ((float_8)(res_end_x1 - res_end_x0));
    res_angle = atan(res_k);
  }
  angle_a = fabs(res_angle - ref_angle);
  if ((res_angle * ref_angle) < 0) {
    angle_a = ISIP_TWOPI / 2 - angle_a;
  }
  
  // compute the length difference between the two lines
  //
  float_8 ref_len = sqrt((ref_end_y1 - ref_end_y0) * (ref_end_y1 - ref_end_y0)
		    + (ref_end_x1 - ref_end_x0) * (ref_end_x1 - ref_end_x0));
  float_8 res_len = sqrt((res_end_y1 - res_end_y0) * (res_end_y1 - res_end_y0)
		    + (res_end_x1 - res_end_x0) * (res_end_x1 - res_end_x0));

  // check for data validality
  //
  if (ref_len == 0) {
    fprintf(stdout, "compu_param_cc: zero length\n");
    return ISIP_FALSE;
  }
  len_dif_a = fabs(ref_len - res_len) / ref_len;
  
  // exit gracefully
  //
  return ISIP_TRUE;
}
