#include <vpu/circlefit.hh>
#include <csv_reader.hh>
#include <cmath>
#include <cli_args.hh>
#include <string>
#include <sstream>
#include <iostream>
#include <iomanip>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

using namespace std;
using namespace sw;

namespace sw { namespace math {
template <typename vt>
vt random(vt min, double max) throw()
{
  static bool is_init = false;
  if(!is_init) { ::srand(::time(0)); is_init=true; }
  double mn = min, mx = max;
  if(mn>mx) { double t=mn; mn=mx; mx=t; }
  mn += ((mx-mn)/(double)RAND_MAX * ::rand());
  return (vt)mn;
}
}}

// CSV reader for floating point numbers:
typedef sw::csv_reader<double> csv_t;
csv_t csv;

// Fit result circle
sw::circle_fit::circle_t circle;

/**
 * Perform fit from csv data.
 * @return bool
 */
bool fit()
{
  bool ok = true;
  unsigned n = csv.rows();
  std::vector<double> x,y;
  x.reserve(n); y.reserve(n);
  for(unsigned i=0; i<n; ++i) {
    x.push_back(csv.data()[i][0]);
    y.push_back(csv.data()[i][1]);
  }
  if(!cli_args["g"]) {
    // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< ALGEBRAIC CIRCLE FIT CALL IS HERE
    circle = sw::circle_fit::fit(x, y);
    // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  } else {
    // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< GEOMETRIC CIRCLE FIT CALL IS HERE
    circle = sw::circle_fit::fit_geometric(x, y);
    // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  }
  return ok;
}

/**
 * Create test circle
 */
bool make_test_circle(std::string &result, double *result_x, double *result_y, double *result_r,
    double *result_a0, double *result_a1, int *result_n, double min_x, double max_x, double min_y,
    double max_y, double min_r, double max_r, double min_a, double max_a, double noise, int min_n,
    int max_n)
{
  double n0 = sw::math::random(min_n, max_n); // Number of circumference points
  double x0 = sw::math::random(min_x, max_x); // Centre x
  double y0 = sw::math::random(min_y, max_y); // Centre y
  double r0 = sw::math::random(min_r, max_r); // Radius
  double a0, a1; // Start / end angle

  if(min_a > max_a) min_a = 0; // Input error override

  if(max_a != 0) {
    // Random arc
    a0 = sw::math::random(0.0, 360.0);
    a1 = sw::math::random(min_a, (max_a==0.0 ? 360.0 : max_a)) + a0;
    for(int i=0; i<5 && a0 == a1; i++) {
      a1 = a0 + sw::math::random((double)min_a, (double)(max_a==0 ? 360 : max_a));
    }
    // Arc modulo
    if(a1 > 360.0) { a0 -= 360.0; a1-=360.0; }
  } else {
    a0 = 0;
    a1 = 360;
  }

  // Resulting "input" circle description
  if(result_x) *result_x = x0;
  if(result_y) *result_y = y0;
  if(result_r) *result_r = r0;
  if(result_n) *result_n = n0;
  if(result_a0) *result_a0 = a0;
  if(result_a1) *result_a1 = a1;

  // CSV header
  std::stringstream ss;
  ss    << "# Test circle" << endl
        << "# cfg: x-min=" << min_x << ",x-max=" << max_x << endl
        << "# cfg: y-min=" << min_y << ",y-max=" << max_y << endl
        << "# cfg: r-min=" << min_r << ",r-max=" << max_r << endl
        << "# cfg: n-min=" << min_n << ",n-max=" << max_n << endl
        << "# cfg: r-noise=" << noise << endl
        << "#" << endl
        << "# x=" << x0 << endl
        << "# y=" << y0 << endl
        << "# r=" << r0 << endl
        << "# a=" << a0 << endl
        << "# b=" << a1 << endl
        << "# N=" << n0 << endl
        << "# arc=" << (a1-a0) << endl
        << "# " << endl
        << "x, y" << endl
        ;

  // CSV data
  a0 *= M_PI/180.0;
  a1 *= M_PI/180.0;
  double as = (a1-a0) / n0;
  for(double ka = a0; ka < a1; ka += as) {
    double a = ka;
    double r = r0 * (1.0 + sw::math::random(-noise/2, noise/2));
    double x = x0 + cos(a) * r;
    double y = y0 + sin(a) * r;
    ss << x << ", " << y << endl;
  }
  result = ss.str();
  return true;
}

/**
 * Generate test circle (wrapper - command line args)
 */
bool mk_test(std::string &result, double *x=0, double *y=0, double *r=0, double *a0=0,
    double *a1=0, int *n=0, double *rnoise=0)
{
  double min_x = (double) sw::cli_args["x-min"];
  double max_x = (double) sw::cli_args["x-max"];
  double min_y = (double) sw::cli_args["y-min"];
  double max_y = (double) sw::cli_args["y-max"];
  double min_r = (double) sw::cli_args["r-min"];
  double max_r = (double) sw::cli_args["r-max"];
  double min_a = (double) sw::cli_args["a-min"];
  double max_a = (double) sw::cli_args["a-max"];
  int min_n    = (double) sw::cli_args["n-min"];
  int max_n    = (double) sw::cli_args["n-max"];
  double max_noise = (double) sw::cli_args["noise-max"];
  if(rnoise) *rnoise = max_noise;
  make_test_circle(
    result, x, y, r, a0, a1, n,
    min_x, max_x, min_y, max_y, min_r, max_r,
    min_a, max_a, max_noise, min_n, max_n
  );
  return true;
}

/**
 * Main
 */
int main(int argc, char** argv)
{
  cli_args(argc, argv) // Global CLI argument object.
    ("g", "geometric", "b", "" , "1" , "Do a Geometric fit (with algebraic initial guess)")
    ("i", "max-iterations", "i", "100" , "" , "Maximum number of fit iterations (algebraic and geometric fit)")
    ("l", "lambda", "n", "0.2" , "" , "Geometric fit: Initial lambda (\"scaled iteration step size\")")
    ("d", "down-factor", "n", "0.04" , "" , "Geometric fit: Lambda scaling for improving iteration "\
                                            "(less deviation)")
    ("u", "up-factor", "n", "10" , "" , "Geometric fit: Lambda scaling for worse deviation than in "\
                                        "the previous iteration")
    ("e", "epsilon", "n", "3.0e-8" , "" , "Geometric fit: Success break condition (aborts when "\
          "position and radius differences with respect to the radius are smaller than epsilon)")
    ("L", "limit", "n", "1.0e6" , "" , "Geometric fit: Abort condition (aborts when x/y centre "\
          "coordinates exceed this limit)")
    ("v", "verbose", "b", "" , "1" , "Add verbosity output to stdout")
    ("x", "x-min", "n", "-100.0" , "" , "Test: Minimum random X position")
    ("X", "x-max", "n", "100.0" , "" , "Test: Maximum random X position")
    ("y", "y-min", "n", "-100.0" , "" , "Test: Minimum random Y position")
    ("Y", "y-max", "n", "100.0" , "" , "Test: Maximum random Y position")
    ("r", "r-min", "n", "2.5" , "" , "Test: Minimum random radius")
    ("R", "r-max", "n", "100" , "" , "Test: Maximum random radius")
    ("a", "a-min", "n", "180"  , "" , "Test: Minimum random arc in DEG")
    ("A", "a-max", "n", "360" , "" , "Test: Maximum random arc in DEG (0==full circle)")
    ("n", "n-min", "n", "10"   , "" , "Test: Minimum random number of points")
    ("N", "n-max", "n", "1000", "" , "Test: Maximum random number of points")
    ("s", "noise-max", "n", "0.05", "" , "Test: Maximum point position noise")
    ;
  cli_args
    .allow_unknown_options(true)
    .parse()
    .handle_help_and_errors(
      "Circle algorithm test.\n\n"
      "  circlefit [opts] <FILE> : Fit a CSV file point cloud.\n"
      "  circlefit [opts] -      : Fit a CSV from stdin.\n"
      "  circlefit [opts] test   : Make a random circle fit test.\n"
      "  circlefit [opts] mktest : Generate CSV data to stdout to double check\n"
      "                            with other programs.\n",
      "v1.0, stfwi, 2011",
      "[mktest | test | <csv file path> | - ] ",
      " 0: successful\n>0: error", 1, cerr
    );

  // First positional argument is the test command or the file path
  string file;
  if(cli_args.positional_arguments().size()) {
    file = cli_args.positional_arguments().front().value();
    if(file == "-") {
      file = "/dev/stdin";
    }
  }
  if(file.empty()) {
    cerr << "No file or test command given (use '-' for stdin)" << endl;
    return 1;
  } else if(file == "mktest") {
    // Generate a random CSV x/y data table
    std::string s;
    if(!mk_test(s)) {
      cerr << "Failed to create test data." << endl;
      return 1;
    }
    cout << s << endl;
    return 0;
  } else if(file == "test") {
    // This is a simple test where we generate random data, fit it, and check the
    // differences between the original circle data and the fitted one.
    std::string csv_txt;
    // Some data we add to the CSV header to see where the point cloud comes from.
    double x, y, r, rnoise, a0, a1;
    int n;
    if(!mk_test(csv_txt, &x, &y, &r, &a0, &a1, &n, &rnoise)) {
      cerr << "Failed to create test data." << endl;
      return 1;
    }
    // That text is simply read back via the csv reader
    csv.abort_on_error(true);
    csv.with_header(csv_t::header_auto);
    csv.comment_chars("#");
    std::stringstream ss(csv_txt);
    ss >> csv;
    if(csv.error() != csv_t::ok) {
      cerr << "Failed to read generated test data." << endl;
    }

    // Fit the data
    fit();

    // Evaluate errors
    double dxy = sqrt((x-circle.x)*(x-circle.x) + (y-circle.y)*(y-circle.y));
    double dr  = (r-circle.r);

    // Print result
    cerr
      << "\"err_xy\"," << "\"err_r\","
      << "  "
      << "\"ref_x\"," << "\"ref_y\"," << "\"ref_r\"," << "\"num_points\","
      << "\"ref_arc_start\"," << "\"ref_arc_len\"," << "\"r_noise\","
      << "  "
      << "\"fit_x\"," << "\"fit_y\"," << "\"fit_r\","
      << "  "
      << "\"fit_diff_x\"," << "\"fit_diff_y\"," << "\"fit_diff_r\""
      << endl;
    cout
      << dxy << "," << dr << ","
      << "  "
      << x << "," << y << "," << r << "," << n << "," << a0 << "," << (a1-a0) << "," << rnoise << ","
      << "  "
      << circle.x << "," << circle.y << "," << circle.r << ","
      << "  "
      << (x-circle.x) << "," << (y-circle.y) << "," << (r-circle.r)
      << endl;
  } else {
    // We assume the positional argument is a file.
    // csv is a global object, it shall stop on read errors or inconsistencies,
    // auto detect a header and ignore lines starting with "#" === coments.
    // csv.load() returns true on success.
    csv.abort_on_error(true);
    csv.with_header(csv_t::header_auto);
    csv.comment_chars("#");
    if(!csv.load(file)) {
      cerr << "failed to load " << file << endl;
      if(cli_args["v"] != false) {
        // This here is only debugging to see what it actually did read.
        if(csv.has_header()) {
          for(unsigned i=0; i<csv.header().size(); ++i) {
            cerr << csv.header()[i] << " ";
          }
          cerr << endl;
        }
        for(unsigned i=0; i<csv.data().size(); ++i) {
          for(unsigned j=0; j<csv.data()[i].size(); ++j) {
            cerr << csv.data()[i][j] << " ";
          }
          cerr << endl;
        }
      }
      return 1;
    }

    // Double check the read data
    if(csv.cols() != 2) {
      cerr << "File data must have 2 columns, column1=x, column2=y'" << endl;
      return 1;
    } else if(csv.rows() < 3) {
      cerr << "File must have at least 3 data rows." << endl;
      return 1;
    } else if(!csv.data_size_consistent()) {
      cerr << "The row sizes of the CSV file are not consistent." << endl;
      return 1;
    }

    // Fit the data
    fit();

    // Print the result, optionally with additional verbose output
    cout << "x=" << circle.x << ", y=" << circle.y << ", r=" << circle.r;
    cout.flush();
    if(cli_args["v"] == true) {
      cerr << ", s=" << circle.s;
      if(cli_args["g"] == true) {
        cerr << ", g=" << circle.g << ", iterations=" << circle.i
             << ", inner_iterations=" << circle.j;
      }
    }
    cout << endl;
  }
  return 0;
}