/*
   Copyright (c) 2010-2013, AMI RESEARCH GROUP <lalvarez@dis.ulpgc.es>
   License : CC Creative Commons "Attribution-NonCommercial-ShareAlike"
   see http://creativecommons.org/licenses/by-nc-sa/3.0/es/deed.en
 */


/**
 * @file lens_distortion_correction_division_model_1p.cpp
 * @brief distortion correction using the improved Hough transform with the 1 parameter division distortion model
 *
 * @author Luis Alvarez <lalvarez@dis.ulpgc.es> and Daniel Santana-Cedres <dsantana@ctim.es>
 */


//Included libraries
#include "ami_image/image.h"
#include "ami_filters/filters.h"
#include "ami_primitives/subpixel_image_contours.h"
#include "ami_primitives/line_extraction.h"
#include "ami_primitives/image_primitives.h"
#include "ami_lens_distortion/lens_distortion_procedures.h"
#include "ami_utilities/utilities.h"
#include <iostream>
#include <fstream>
using namespace std;

int main(int argc, char *argv[])
{
  if(argc!=11){
    print_function_syntax_lens_distortion_correction_division_model_1p();
    exit(EXIT_FAILURE);
  }

  if(check_params_lens_distortion_correction_division_model_1p(argv) != 0){
    manage_failure(argv);
    exit(EXIT_SUCCESS);
  }



  //We read the input image and some variables are initialized
  ami::image<unsigned char> input(argv[1]); // input image
  int width = input.width(), height = input.height();//input image dimensions
  int size_ = width*height; // image size
  ami::image<unsigned char> gray(width,height,1,0);//gray-level image to call canny
  ami::image<unsigned char> edges(width,height,1,0);//image to store edge information
  float canny_high_threshold = atof(argv[5]);// high threshold for canny detector
  float initial_distortion_parameter=atof(argv[6]);//left side of allowed distortion parameter interval
  float final_distortion_parameter = atof(argv[7]);//Hough parameter
  float distance_point_line_max_hough = atof(argv[8]);//Hough parameter
  //maximum difference allowed (in degrees) between edge and line orientation
  float angle_point_orientation_max_difference = atof(argv[9]);


  //Converting the input image to gray level
  for(int i=0; i<size_; i++)
    gray[i] = 0.3*input[i] + 0.59*input[i+size_] + 0.11*input[i+size_*2];

  //ALGORITHM STAGE 1 : Detecting edges with Canny
  float canny_low_threshold = 0.7; //default value for canny lower threshold
  ami::subpixel_image_contours contours=canny(gray,edges,canny_low_threshold,canny_high_threshold);

  //We create writable 3 channel images for edges and gray level
  ami::image<unsigned char> edges3c(width, height, 3),gray3c(width, height, 3);
  edges3c=edges;
  gray3c=gray;

  //Writing Canny detector output
  edges3c.write(argv[2]);
  cout << "...edges detected" << endl;

  //ALGORITHM STAGE 2 : Detecting lines with improved_hough_quotient
  cout << "Detecting lines with improved Hough and division model..." << endl;
  image_primitives i_primitives_quo;//object to store output edge line structure
  int max_lines=30; //maximun number of lines estimated
  float angle_resolution=0.1; // angle discretization step (in degrees)
  float distance_resolution=1.; // line distance discretization step
  float distortion_parameter_resolution=0.1;//distortion parameter discretization step
  //we call 3D Hough line extraction
  line_equation_distortion_extraction_improved_hough_quotient(
    contours,
    i_primitives_quo,
    distance_point_line_max_hough,
    max_lines,
    angle_resolution,
    distance_resolution,
    initial_distortion_parameter,
    final_distortion_parameter,
    distortion_parameter_resolution,
    angle_point_orientation_max_difference
  );

  // WE CHECK IF THE IMPROVED HOUGH PROCEDURE FINISHS PROPERLY
  if(i_primitives_quo.get_lines().size()==0){
    manage_failure(argv);
    exit(EXIT_SUCCESS);
  }
  cout << "...lines detected: " << i_primitives_quo.get_lines().size() <<
	        " with " << count_points(i_primitives_quo) << " points" << endl;

  //ALGORITHM STAGE 3 : Optimising the distortion parameter value by minimizing
  //the distance between the points and their corresponding lines
  cout << "Optimising the distortion model..." << endl;

  double dmi = i_primitives_quo.get_distortion().get_distortion_center().norm();
  dmi*=dmi;


  double error = model_estimation_1p_quotient(i_primitives_quo.get_distortion().get_distortion_center(),
                               i_primitives_quo.get_lines(),
                               i_primitives_quo.get_distortion());
  cout << "...distortion model optimised." << endl;



  //Drawing the detected lines on the original image to illustrate the results
  drawHoughLines(i_primitives_quo,gray3c);
  gray3c.write(argv[3]);

  //ALGORITHM STAGE 4 : Correcting the image distortion using the estimated model
  if(i_primitives_quo.get_distortion().get_d().size()>0){
    cout << "Correcting the distortion..." << endl;
    ami::image<unsigned char> undistorted =undistort_quotient_image_inverse(
      input, // input image
      i_primitives_quo.get_distortion(), // lens distortion model
      2.0 // integer index to fix the way the corrected image is scaled to fit input size image
    );
    //writing the distortion corrected image
    undistorted.write(argv[4]);
    cout << "...distortion corrected." << endl;
  }

  // WRITING OUTPUT TEXT DOCUMENTS
  // writing in a file the lens distortion model and the lines and associated points
  i_primitives_quo.write(argv[10]);
  // writing function parameters and basic outputs :
  ofstream fs("output.txt");//Output file
  fs << "Selected parameters:" << endl;
  fs << "\t High Canny's threshold: " << argv[5] << endl;
  fs << "\t Initial normalized distortion parameter: " << argv[6] << endl;
  fs << "\t Final normalized distortion parameter: " << argv[7] << endl;
  fs << "\t Maximum distance between points and line: " << argv[8] << endl;
  fs << "\t Maximum differente between edge point and line orientations: "
     << argv[9]	<< endl;
  fs << "-------------------------" << endl;
  fs << "Results: " << endl;
  fs << "\t Number of detected lines: " << i_primitives_quo.get_lines().size()
	   << endl;
  int count = count_points(i_primitives_quo);
  fs << "\t Total amount of line points: " << count << endl;
  fs << "\t Estimated normalized distortion parameter: " <<
	   (-i_primitives_quo.get_distortion().get_d()[1]*dmi) /
		 (1+i_primitives_quo.get_distortion().get_d()[1]*dmi)
		 << endl;
  fs << "\t Average squared error distance between line and associated points = " << error/count << endl;

  fs.close();
  exit(EXIT_SUCCESS);
}