/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

//- Class:       NonlinearCGOptimizer
//- Description: Implementation code for the NonlinearCGOptimizer class
//- Owner:       Brian Adams
//- Checked by:

#include "NonlinearCGOptimizer.hpp"
#include "ProblemDescDB.hpp"
#include <boost/math/tools/minima.hpp>

// uncomment to use the Boost Brent's algorithm
//#define BOOST_BRENT 1

using namespace std;

namespace Dakota {

// wrapper around linesearch evaluator needed for use in Brent's algorithm
class boost_ls_eval {

public:

  // constructor
  boost_ls_eval(NonlinearCGOptimizer& pimpl_): pImpl(pimpl_)
  { /* empty constructor */ }

  // evaluator
  Real operator()(const Real& trial_step)
  { return ( pImpl.linesearch_eval(trial_step, 1) ); }

private:

  // pointer to class instance so we have a context when performing evaluations
  NonlinearCGOptimizer& pImpl;

};

NonlinearCGOptimizer::
NonlinearCGOptimizer(ProblemDescDB& problem_db, Model& model):
  Optimizer(problem_db, model, std::shared_ptr<TraitsBase>(new NonlinearCGTraits())),
  initialStep(0.01), linesearchTolerance(1.0e-2),
  linesearchType(CG_LS_SIMPLE), maxLinesearchIters(10), relFunctionTol(0.0),
  relGradientTol(0.0), resetStep(true), restartIter(1000000),
  updateType(CG_FLETCHER_REEVES)
{
  if (numFunctions > 1 || numConstraints > 0 || boundConstraintFlag) {
    Cerr << "ERROR: NonlinearCG only supports unconstrainted single objective "
	 << "problems!" << endl;
    abort_handler(-1);
  }
  // some of the defaults may be overridden by user-supplied options
  parse_options();

  stepLength = initialStep;
}

NonlinearCGOptimizer::~NonlinearCGOptimizer()
{ }

void NonlinearCGOptimizer::core_run()
{

  // TODO: Once DAKOTA moves to Teuchos for its numerical data type,
  // remove the various copies below.  (Also, could use std::copy for some.)
  copy_data(iteratedModel.continuous_variables(), designVars); // view->copy
  searchDirection.sizeUninitialized(numContinuousVars);

  if (linesearchType > CG_FIXED_STEP)
    trialVars.sizeUninitialized(numContinuousVars);

  for (iterCurr = 0; iterCurr < maxIterations; iterCurr++) {

    // get function and gradient -- should discern whether linesearch
    // is in use and only request gradient if appropriate
    activeSet.request_values(3);
    iteratedModel.evaluate(activeSet);
    const Response&   response  = iteratedModel.current_response();
    const RealVector& functions = response.function_values();

    functionCurr = functions[0];
    gradCurr = response.function_gradient_view(0);

    // always store ||g||^2
    gradDotGrad_curr = gradCurr.dot(gradCurr);
    if (iterCurr == 0)
      gradDotGrad_init = gradDotGrad_curr;

    // TODO: accumulate stats
    if (outputLevel >= NORMAL_OUTPUT) {
      Cout << "INFO (NonlinearCG): iteration statistics\n";
      Cout << "iter J                norm(g)          steplen\n";
      Cout << "---- ---------------- ---------------- ----------------\n";
      Cout << setw(4) << iterCurr << " " << functionCurr << " "
	   << sqrt(gradDotGrad_curr) << " " << stepLength << "\n" << endl;
    }
    // check convergence: two-norm of gradient
    if (sqrt(gradDotGrad_curr) < convergenceTol) {
      Cout << "INFO (NonlinearCG): hard convergence reached (gradient norm "
	   << "within tolerance)." << endl;
      break;
    }

    if ( iterCurr > 0)
      // could do as CTK and check this in conjunction with test above...
      if (sqrt(gradDotGrad_curr) < relGradientTol*gradDotGrad_init) {
	Cout << "INFO (NonlinearCG): convergence reached (gradient reduction "
	     << "within tolerance)." << endl;
	break;
      }
    if ( std::fabs(functionCurr - functionPrev) /
	 std::max(1.0,std::fabs(functionPrev)) <
	   relFunctionTol ) {
	Cout << "INFO (NonlinearCG): convergence reached (function change "
	     << "within tolerance)." << endl;
	break;
      }

    // will set searchDirection to steepest descent, or CG direction
    // TODO: recourse to steepest descent
    compute_direction();
    if (sqrt(searchDirection.dot(searchDirection)) < 1e-16) {
      Cout << "INFO (NonlinearCG): degenerate search direction. Exiting."
	   << endl;
      break;
    }

    // will set stepLength if successful, return false if failed
    if (!compute_step()) {
      Cout << "INFO (NonlinearCG): failure computing step length. Exiting."
	   << endl;
      break;
    }

    // update design vars here, gradient evaluated on loop continuation
    // compute designVars <- stepLength * searchDirection + designVars
    designVars.AXPY(designVars.length(), stepLength, searchDirection.values(),
		    1, designVars.values(), 1);

    // TODO: this is a duplicate copy in the linesearch case -- rework to avoid
    iteratedModel.continuous_variables(designVars);

    // archive gradient and derived metric for next iteration
    functionPrev = functionCurr;
    gradPrev = response.function_gradient_copy(0);
    gradDotGrad_prev = gradDotGrad_curr;

  }

  if (iterCurr == maxIterations)
    Cout << "INFO (NonlinearCG): maxIterations = " << maxIterations
	 << " reached. Exiting." << endl;

  // return the optimal point in best*; should do DB lookup?
  bestVariablesArray.front().continuous_variables(designVars);
  if (!localObjectiveRecast) // else local_objective_recast_retrieve() used
    bestResponseArray.front().function_value(functionCurr, 0);
}

void NonlinearCGOptimizer::compute_direction()
{

  if (iterCurr == 0 || (iterCurr % restartIter == 0) ||
      updateType == CG_STEEPEST) {

    // choose steepest descent on first iteration or on reset
    if (outputLevel >= VERBOSE_OUTPUT && iterCurr > 0)
      Cout << "INFO (NonlinearCG): Iteration = " << iterCurr
	   << ", resetting to steepest descent." << endl;
    searchDirection.putScalar(0.0);
    searchDirection -= gradCurr;

  }
  else {

    double beta = 0.0;
    if (updateType == CG_FLETCHER_REEVES)
      beta = gradDotGrad_curr/gradDotGrad_prev;
    else {

      // Polak-Ribiere or Hestenes-Stiefel
      gradDiff = iteratedModel.current_response().function_gradient_copy(0);
      gradDiff -= gradPrev;
      Real gradCurr_dot_gradDiff =
	gradCurr.dot(gradDiff);

      if (updateType == CG_POLAK_RIBIERE)
	beta = gradCurr_dot_gradDiff / gradDotGrad_prev;
      else if (updateType == CG_POLAK_RIBIERE_PLUS)
	// use max(B_PR, 0.0))
	beta = std::max(gradCurr_dot_gradDiff / gradDotGrad_prev, 0.0);
      else if (updateType == CG_HESTENES_STIEFEL)
	// use (gradCurr - gradPrev)'*searchDirection in denominator
	beta = gradCurr_dot_gradDiff /
	  gradDiff.dot(searchDirection);
    }

    if (outputLevel >= DEBUG_OUTPUT)
      Cout << "DEBUG (NonlinearCG): beta = " << beta << endl;
    // searchDirection <-- beta*searchDirection - gradCurr
    searchDirection.scale(beta);
    searchDirection.AXPY(searchDirection.length(), -1.0,
			 gradCurr.values(), 1,
			 searchDirection.values(), 1);
  }
  if (outputLevel >= DEBUG_OUTPUT)
    Cout << "DEBUG (NonlinearCG): new search direction is:\n"
	 << searchDirection << endl;

}

// returns whether a valid step was found
bool NonlinearCGOptimizer::compute_step()
{

  // TODO: stepLength should be chosen based on descent direction and
  // previous successful step
  if (resetStep)
    stepLength = initialStep;

  switch (linesearchType) {

  case CG_FIXED_STEP:
    // do nothing (leave initial stepLength )
    return(true);
    break;

  case CG_LS_SIMPLE: {

    // value-based line search with simple decrease
    bool decrease = false;
    unsigned bt_iter = 0;
    while (!decrease && bt_iter < maxLinesearchIters) {

      if (linesearch_eval(stepLength) < functionCurr) {
	if (outputLevel > NORMAL_OUTPUT)
	  Cout << "INFO (NonlinearCG_LS): Simple decrease achieved; step = "
	       << stepLength << "." << endl;
	decrease = true;
      }
      else {
	if (outputLevel > NORMAL_OUTPUT) {
	  if (bt_iter == 0)
	    Cout << "INFO (NonlinearCG_LS): Initiating simple linesearch.\n";
	  Cout << "INFO (NonlinearCG_LS): Backtracking." << endl;
	}
	stepLength /= 2;
	bt_iter++;
      }

    }
    if (bt_iter == maxLinesearchIters && outputLevel > NORMAL_OUTPUT) {
      Cout << "INFO (NonlinearCG_LS): Could not find step yielding simple "
	   << "decrease." << endl;
    }
    return(decrease);

    break;
  }

  case CG_LS_BRENT: {

    if (outputLevel > NORMAL_OUTPUT)
      Cout << "INFO (NonlinearCG_LS): Initiating Brent linesearch." << endl;

    // first bracket the minimum, then use Brent's algorithm from Boost to
    // hone in on the minimum
    Real xa, xb, xc, fa, fb, fc;
    xa = 0.0;
    fa = functionCurr;

    if (outputLevel > NORMAL_OUTPUT)
      Cout << "INFO (NonlinearCG_LS): Evaluating at initial step = "
	   << stepLength << "." << endl;
    xb = stepLength;
    fb = linesearch_eval(xb);

    if (outputLevel > NORMAL_OUTPUT)
      Cout << "INFO (NonlinearCG_LS): Initiating bracketing procedure." << endl;
    bracket_min(xa, xb, xc, fa, fb, fc);
    if (outputLevel > NORMAL_OUTPUT) {
      Cout << "INFO (NonlinearCG_LS): Bracketing complete:\n";
      Cout << "Bracket:   [" << xa << ", " << xb << ", " << xc << "]" << endl;
      Cout << "Functions: [" << fa << ", " << fb << ", " << fc << "]" << endl;
      Cout << "INFO (NonlinearCG_LS): Initiating 1-D minimization." << endl;
    }

#ifdef BOOST_BRENT
    if (outputLevel >= DEBUG_OUTPUT)
      Cout << "INFO (NonlinearCG_LS): Using Boost Brent." << endl;
    int exponent;
    frexp(linesearchTolerance, &exponent);
    int bits = 1 - exponent;
    boost::uintmax_t max_it = maxLinesearchIters;

    pair<Real,Real> opt_len =
      boost::math::tools::brent_find_minima<boost_ls_eval,Real>
      (boost_ls_eval(*this), xa, xc, bits, max_it);
    stepLength = opt_len.first;
#else
    if (outputLevel >= DEBUG_OUTPUT)
      Cout << "INFO (NonlinearCG_LS): Using native Brent." << endl;
    stepLength = brent_minimize(xa, xc, linesearchTolerance);
#endif

    if (outputLevel > NORMAL_OUTPUT)
      Cout << "INFO (NonlinearCG_LS): Linesearch complete; step = "
	   << stepLength << "." << endl;

    return(true);

    break;

  }

  default:
    Cerr << "ERROR (NonlinearCG_LS): Requested linesearch type not available."
	 << endl;
    abort_handler(-1);
    break;

  }

  return(false);

}

void NonlinearCGOptimizer::bracket_min(Real& xa, Real& xb, Real& xc,
				       Real& fa, Real& fb, Real& fc)
{
  // Repeatedly evaluate the function along the search direction until
  // we know we've bracketed a minimum.

  const Real GOLDEN_RATIO = 1.618033988749895, SMALL_DIV = 1e-16,
    MAX_EXTRAP_FACTOR = 100.0;
  Real tmp, q, r, xm, xlim, fm = 0.0;

  // TODO: implement option to do simple backtracking to get a lower
  // function value in the current search direction; for now, just
  // swap the values of a and b if necessary
  // also, need to bound the search >= 0 when changing sign...
  if (fb > fa) {
    Cout << "swapping points, fa = " << fa << "fb = " << fb << "diff "
	 << fb-fa << endl;
    tmp = xa;
    xa = xb;
    xb = tmp;
    tmp = fa;
    fa = fb;
    fb = tmp;
  }

  xc = xb + GOLDEN_RATIO*(xb-xa);
  fc = linesearch_eval(xc);

  if (outputLevel >= DEBUG_OUTPUT) {
    Cout << "Bracket:   [" << xa << ", " << xb << ", " << xc << "]" << endl;
    Cout << "Functions: [" << fa << ", " << fb << ", " << fc << "]" << endl;
  }

  while (fb >= fc) {

    if (outputLevel >= DEBUG_OUTPUT) {
      Cout << "Bracket:   [" << xa << ", " << xb << ", " << xc << "]" << endl;
      Cout << "Functions: [" << fa << ", " << fb << ", " << fc << "]" << endl;
    }

    // find the extremum xm of a quadratic model interpolating a, b, c
    q = (fb-fa)*(xb-xc);
    r = (fb-fc)*(xb-xa);
    // avoid division by small (q-r) by bounding with signed minimum
    tmp = std::fabs(q-r);
    tmp = (tmp < SMALL_DIV) ? SMALL_DIV : tmp;
    tmp = (q-r < 0 ) ? -tmp : tmp;
    xm = xb - (q*(xb-xc) -r*(xb-xa))/2.0/tmp;
    // maximum point for which we trust the interpolation
    xlim = xb + MAX_EXTRAP_FACTOR * (xc-xb);

    // now detect which interval xm is in and act accordingly
    // [xb, xc]
    if ( (xb-xm)*(xm-xc) > 0.0 ) {

      fm = linesearch_eval(xm);
      if (fm < fc) {
	// use points [b, xm, c]
	xa = xb;
	fa = fb;
        xb = xm;
	fb = fm;
	return;
      }
      else if (fm > fb) {
	// use points [a, b, xm]
	xc = xm;
	fc = fm;
	return;
      }
      xm = xc + GOLDEN_RATIO*(xc-xb);
      fm = linesearch_eval(xm);

    }
    // [xc, xlim]
    else if ((xc-xm)*(xm-xlim) > 0.0) {
      if (fm < fc) {
	xb = xc;
	fb = fc;
	xc = xm;
	fc = fm;
	xm = xc+GOLDEN_RATIO*(xc-xb);
	fm = linesearch_eval(xm);
      }
    }
    // [xlim, inf]
    else if ((xm-xlim)*(xlim-xc) >= 0.0 ) {
      xm = xlim;
      fm = linesearch_eval(xm);
    }
    // [0,xb]
    else {
      xm = xc + GOLDEN_RATIO*(xc-xb);
      fm = linesearch_eval(xm);
    }

    // shift to newest 3 points before loop
    xa = xb;
    fa = fb;
    xb = xc;
    fb = fc;
    xc = xm;
    fc = fm;

  }

}

/** Perform 1-D minimization for the stepLength using Brent's method.
    This is a C translation of fmin.f from Netlib.
*/
Real NonlinearCGOptimizer::brent_minimize(Real a, Real b, Real tol)
{

  // c is the squared inverse of the golden ratio
  Real c = 0.5*(3.0-sqrt(5.0));

  // eps is approximately the square root of the relative machine
  // precision.

  // initialization
  Real eps = std::numeric_limits<double>::epsilon();
  Real tol1 = eps+1.0;
  eps = sqrt(eps);

  Real v = a+c*(b-a);
  Real w = v;
  Real x = v;
  Real e = 0.0;
  Real fx = linesearch_eval(x);
  Real fv = fx;
  Real fw = fx;
  Real tol3 = tol/3.0;

  Real xm = 0.5*(a+b);
  tol1 = eps*std::fabs(x)+tol3;
  Real t2 = 2.0*tol1;

  Real d, p, q, r, u, fu;  // temp variables

  unsigned iter = 1;
  while ( iter < maxLinesearchIters && std::fabs(x-xm) > (t2-0.5*(b-a)) ) {

    d = 0.0;
    p = 0.0;
    q = 0.0;
    r = 0.0;
    if ( std::fabs(e) > tol1 ) {

      // fit parabola
      r = (x-w)*(fx-fv);
      q = (x-v)*(fx-fw);
      p = (x-v)*q-(x-w)*r;
      q = 2.0*(q-r);

      if (q <= 0.0)
	q = -q;
      else
	p = -p;
      r = e;
      e = d;

    }

    if ( (std::fabs(p) < std::fabs(0.5*q*r)) && (p > q*(a-x)) && (p < q*(b-x)) ) {

      // a parabolic-interpolation step
      d = p/q;
      u = x+d;
      // f must not be evaluated too close to ax or bx
      if ( ((u-a) < t2) || ((b-u) < t2) ) {
	d = tol1;
	if (x >= xm)
	  d = -d;
      }
    }
    else {

      // a golden-section step
      if (x >= xm)
	e = a-x;
      else
	e = b-x;
      d = c*e;

    }

    // f must not be evaluated too close to x
    if ( std::fabs(d) < tol1 )
      if ( d <= 0.0 )
	u = x-tol1;
      else
	u = x+tol1;
    else
      u = x+d;

    fu = linesearch_eval(u);

    // update  a, b, v, w, and x
    if (fx <= fu)
      if (u >= x)
	b = u;
      else
	a = u;

    if (fu <= fx) {

      if (u >= x)
	a = x;
      else
	b = x;
      v = w;
      fv = fw;
      w = x;
      fw = fx;
      x = u;
      fx = fu;

    }
    else if ( (fu > fw) && (w != x) ) {

      if ((fu<=fv) || (v == x) || (v == w)) {
	v = u;
	fv = fu;
      }

    }
    else {

      v = w;
      fv = fw;
      w = u;
      fw = fu;

    }

    xm = 0.5*(a+b);
    tol1 = eps*std::fabs(x)+tol3;
    t2 = 2.0*tol1;
    iter++;

  } // end while

  if (iter > maxLinesearchIters && outputLevel >= NORMAL_OUTPUT) {
    Cout << "WARN (NonlinearCG_LS): Step length not found within "
	 << "maxLinesearchIters; using best known." << endl;
  }

  return(x);

}

// Function evaluator to use in linesearches
// Uses the current designVars and searchDirection
// TODO: Support gradient-based evals, likely returning a const Response&
Real NonlinearCGOptimizer::linesearch_eval(const Real& trial_step,
					   short req_val)
{
  // evaluate function only
  for (size_t i=0; i<numContinuousVars; i++)
    trialVars[i] = designVars[i] + trial_step * searchDirection[i];
  iteratedModel.continuous_variables(trialVars);
  activeSet.request_values(req_val);
  iteratedModel.evaluate(activeSet);
  const Response& response = iteratedModel.current_response();
  const RealVector& functions = response.function_values();

  return(functions[0]);
}

void NonlinearCGOptimizer::parse_options()
{
  // Allowed update options
  map<string, int> update_type;
  update_type["steepest"]              = 0;
  update_type["fletcher_reeves"]       = 1;
  update_type["polak_ribiere"]         = 2;
  update_type["polak_ribiere_plus"]    = 3;
  update_type["hestenes_stiefel"]      = 4;

  // Allowed linesearch options
  map<string, int> search_type;
  search_type["fixed_step"] = 0;
  search_type["ls_simple"]  = 1;
  search_type["ls_brent"]   = 2;
  search_type["ls_wolfe"]   = 3;

  map<string,string> opts;
  const StringArray& db_opts = probDescDB.get_sa("method.coliny.misc_options");
  StringArray::const_iterator db_it = db_opts.begin();
  StringArray::const_iterator db_end = db_opts.end();
  String::const_iterator delim;

  for ( ; db_it != db_end; ++db_it)
    if ( (delim = find(db_it->begin(), db_it->end(), '=')) != db_it->end()) {

      String opt(*db_it, 0, distance(db_it->begin(), delim));
      String val(*db_it, distance(db_it->begin(), delim+1),
		 distance(delim, db_it->end()));

      if (opt == "initial_step")
	initialStep = atof(val.c_str());
      else if (opt == "linesearch_tolerance")
	linesearchTolerance = atof(val.c_str());
      else if (opt == "linesearch_type") {
	map<string, int>::const_iterator cit;
	if ( (cit = search_type.find(val)) != search_type.end())
	  linesearchType = cit->second;
	else {
	  Cerr << "ERROR (NonlinearCG): Invalid linesearch_type." << endl;
	  abort_handler(-1);
	}
      }
      else if (opt == "max_linesearch_iters")
	maxLinesearchIters = atoi(val.c_str());
      else if (opt == "rel_function_tol")
	relFunctionTol = atof(val.c_str());
      else if (opt == "rel_gradient_tol")
	relGradientTol = atof(val.c_str());
      else if (opt == "restart_iter")
	restartIter = atoi(val.c_str());
      else if (opt == "reset_step")
	resetStep = (val == "true") ? true : false;
      else if (opt == "update_type") {
	map<string, int>::const_iterator cit;
	if ( (cit = update_type.find(val)) != update_type.end())
	  updateType = cit->second;
	else {
	  Cerr << "ERROR (NonlinearCG): Invalid update_type." << endl;
	  abort_handler(-1);
	}
      }
      else {
	Cerr << "ERROR (NonlinearCG): Unknown misc_option." << endl;
	abort_handler(-1);
      }

      if (outputLevel > NORMAL_OUTPUT)
	Cout << "INFO (NonlinearCG): User parameter '" << opt << "': " << val
	     << endl;

    }
    else {
      Cerr << "ERROR (NonlinearCG): Invalid misc_options format." << endl;
      abort_handler(-1);
    }

}

} // namespace Dakota


#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/make_shared.hpp>
#include <sm/assert_macros.hpp>
#include <sm/logging.hpp>
#include <aslam/cameras/GridCalibrationTargetAprilgrid.hpp>

namespace aslam {
namespace cameras {

/// \brief Construct an Aprilgrid calibration target
///        tagRows:    number of tags in y-dir (gridRows = 2*tagRows)
///        tagCols:    number of tags in x-dir (gridCols = 2*tagCols)
///        tagSize:    size of a tag [m]
///        tagSpacing: space between tags (in tagSpacing [m] = tagSpacing*tagSize)
///
///        corner ordering in _points :
///          12-----13  14-----15
///          | TAG 3 |  | TAG 4 |
///          8-------9  10-----11
///          4-------5  6-------7
///    y     | TAG 1 |  | TAG 2 |
///   ^      0-------1  2-------3
///   |-->x
GridCalibrationTargetAprilgrid::GridCalibrationTargetAprilgrid(
    size_t tagRows, size_t tagCols, double tagSize, double tagSpacing,
    const AprilgridOptions &options)
    : GridCalibrationTargetBase(2 * tagRows, 2 * tagCols),  //4 points per tag
      _tagSize(tagSize),
      _tagSpacing(tagSpacing),
      _options(options),
      _tagCodes(AprilTags::tagCodes36h11) {
  SM_ASSERT_GT(Exception, tagSize, 0.0, "tagSize has to be positive");
  SM_ASSERT_GT(Exception, tagSpacing, 0.0, "tagSpacing has to be positive");

  // allocate memory for the grid points
  _points.resize(size(), 3);

  //initialize a normal grid (checkerboard and circlegrids)
  createGridPoints();

  //start the output window if requested
  initialize();
}

//protected ctor for serialization
GridCalibrationTargetAprilgrid::GridCalibrationTargetAprilgrid() :
      _tagCodes(AprilTags::tagCodes36h11)
{}

/// \brief initialize the object
void GridCalibrationTargetAprilgrid::initialize()
{
  if (_options.showExtractionVideo) {
    cv::namedWindow("Aprilgrid: Tag detection");
    cv::namedWindow("Aprilgrid: Tag corners");
    cvStartWindowThread();
  }

  //create the tag detector
  _tagDetector = boost::make_shared<AprilTags::TagDetector>(_tagCodes, _options.blackTagBorder);
}

/// \brief initialize an april grid
///   point ordering: (e.g. 2x2 grid)
///          12-----13  14-----15
///          | TAG 3 |  | TAG 4 |
///          8-------9  10-----11
///          4-------5  6-------7
///    y     | TAG 1 |  | TAG 2 |
///   ^      0-------1  2-------3
///   |-->x
void GridCalibrationTargetAprilgrid::createGridPoints() {
  //each tag has 4 corners
  //unsigned int numTags = size()/4;
  //unsigned int colsTags = _cols/2;

  for (unsigned r = 0; r < _rows; r++) {
    for (unsigned c = 0; c < _cols; c++) {
      Eigen::Matrix<double, 1, 3> point;

      point(0) = (int) (c / 2) * (1 + _tagSpacing) * _tagSize
          + (c % 2) * _tagSize;
      point(1) = (int) (r / 2) * (1 + _tagSpacing) * _tagSize
          + (r % 2) * _tagSize;
      point(2) = 0.0;

      _points.row(r * _cols + c) = point;
    }
  }
}

/// \brief extract the calibration target points from an image and write to an observation
bool GridCalibrationTargetAprilgrid::computeObservation(
    const cv::Mat & image, Eigen::MatrixXd & outImagePoints,
    std::vector<bool> &outCornerObserved) const {

  bool success = true;

  // detect the tags
  std::vector<AprilTags::TagDetection> detections = _tagDetector->extractTags(image);

  /* handle the case in which a tag is identified but not all tag
   * corners are in the image (all data bits in image but border
   * outside). tagCorners should still be okay as apriltag-lib
   * extrapolates them, only the subpix refinement will fail
   */

  //min. distance [px] of tag corners from image border (tag is not used if violated)
  std::vector<AprilTags::TagDetection>::iterator iter = detections.begin();
  for (iter = detections.begin(); iter != detections.end();) {
    // check all four corners for violation
    bool remove = false;

    for (int j = 0; j < 4; j++) {
      remove |= iter->p[j].first < _options.minBorderDistance;
      remove |= iter->p[j].first > (float) (image.cols) - _options.minBorderDistance;  //width
      remove |= iter->p[j].second < _options.minBorderDistance;
      remove |= iter->p[j].second > (float) (image.rows) - _options.minBorderDistance;  //height
    }

    //also remove tags that are flagged as bad
    if (iter->good != 1)
      remove |= true;

    //also remove if the tag ID is out-of-range for this grid (faulty detection)
    if (iter->id >= (int) size() / 4)
      remove |= true;

    // delete flagged tags
    if (remove) {
      SM_DEBUG_STREAM("Tag with ID " << iter->id << " is only partially in image (corners outside) and will be removed from the TargetObservation.\n");

      // delete the tag and advance in list
      iter = detections.erase(iter);
    } else {
      //advance in list
      ++iter;
    }
  }

  //did we find enough tags?
  if (detections.size() < _options.minTagsForValidObs) {
    success = false;

    //immediate exit if we dont need to show video for debugging...
    //if video is shown, exit after drawing video...
    if (!_options.showExtractionVideo)
      return success;
  }

  //sort detections by tagId
  std::sort(detections.begin(), detections.end(),
            AprilTags::TagDetection::sortByIdCompare);

  // check for duplicate tagIds (--> if found: wild Apriltags in image not belonging to calibration target)
  // (only if we have more than 1 tag...)
  if (detections.size() > 1) {
    for (unsigned i = 0; i < detections.size() - 1; i++)
      if (detections[i].id == detections[i + 1].id) {

        // Do not throw a fatal error if the tag is seen twice.
        // Some frames are just corrupted. Hence ignore bad
        // frames (with a warning) rather than die.
        std::cout << "Wild April tag detected." << std::endl;
        return false;

        //show the duplicate tags in the image
        cv::destroyAllWindows();
        cv::namedWindow("Wild Apriltag detected. Hide them!");
        cvStartWindowThread();

        cv::Mat imageCopy = image.clone();
        cv::cvtColor(imageCopy, imageCopy, CV_GRAY2RGB);

        //mark all duplicate tags in image
        for (int j = 0; i < detections.size() - 1; i++) {
          if (detections[j].id == detections[j + 1].id) {
            detections[j].draw(imageCopy);
            detections[j + 1].draw(imageCopy);
          }
        }

        cv::putText(imageCopy, "Duplicate Apriltags detected. Hide them.",
                    cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8,
                    CV_RGB(255,0,0), 2, 8, false);
        cv::putText(imageCopy, "Press enter to exit...", cv::Point(50, 80),
                    CV_FONT_HERSHEY_SIMPLEX, 0.8, CV_RGB(255,0,0), 2, 8, false);
        cv::imshow("Duplicate Apriltags detected. Hide them", imageCopy);  // OpenCV call

        // and exit
        SM_FATAL_STREAM("\n[ERROR]: Found apriltag not belonging to calibration board. Check the image for the tag and hide it.\n");

        cv::waitKey();
        exit(0);
      }
  }

  // convert corners to cv::Mat (4 consecutive corners form one tag)
  /// point ordering here
  ///          11-----10  15-----14
  ///          | TAG 2 |  | TAG 3 |
  ///          8-------9  12-----13
  ///          3-------2  7-------6
  ///    y     | TAG 0 |  | TAG 1 |
  ///   ^      0-------1  4-------5
  ///   |-->x
  cv::Mat tagCorners(4 * detections.size(), 2, CV_32F);

  for (unsigned i = 0; i < detections.size(); i++) {
    for (unsigned j = 0; j < 4; j++) {
      tagCorners.at<float>(4 * i + j, 0) = detections[i].p[j].first;
      tagCorners.at<float>(4 * i + j, 1) = detections[i].p[j].second;
    }
  }

  //store a copy of the corner list before subpix refinement
  cv::Mat tagCornersRaw = tagCorners.clone();

  //optional subpixel refinement on all tag corners (four corners each tag)
  if (_options.doSubpixRefinement && success)
    cv::cornerSubPix(
        image, tagCorners, cv::Size(2, 2), cv::Size(-1, -1),
        cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

  if (_options.showExtractionVideo) {
    //image with refined (blue) and raw corners (red)
    cv::Mat imageCopy1 = image.clone();
    cv::cvtColor(imageCopy1, imageCopy1, CV_GRAY2RGB);
    for (unsigned i = 0; i < detections.size(); i++)
      for (unsigned j = 0; j < 4; j++) {
        //raw apriltag corners
        //cv::circle(imageCopy1, cv::Point2f(detections[i].p[j].first, detections[i].p[j].second), 2, CV_RGB(255,0,0), 1);

        //subpixel refined corners
        cv::circle(
            imageCopy1,
            cv::Point2f(tagCorners.at<float>(4 * i + j, 0),
                        tagCorners.at<float>(4 * i + j, 1)),
            3, CV_RGB(0,0,255), 1);

        if (!success)
          cv::putText(imageCopy1, "Detection failed! (frame not used)",
                      cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8,
                      CV_RGB(255,0,0), 3, 8, false);
      }

    cv::imshow("Aprilgrid: Tag corners", imageCopy1);  // OpenCV call
    cv::waitKey(1);

    /* copy image for modification */
    cv::Mat imageCopy2 = image.clone();
    cv::cvtColor(imageCopy2, imageCopy2, CV_GRAY2RGB);
    /* highlight detected tags in image */
    for (unsigned i = 0; i < detections.size(); i++) {
      detections[i].draw(imageCopy2);

      if (!success)
        cv::putText(imageCopy2, "Detection failed! (frame not used)",
                    cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8,
                    CV_RGB(255,0,0), 3, 8, false);
    }

    cv::imshow("Aprilgrid: Tag detection", imageCopy2);  // OpenCV call
    cv::waitKey(1);

    //if success is false exit here (delayed exit if _options.showExtractionVideo=true for debugging)
    if (!success)
      return success;
  }

  //insert the observed points into the correct location of the grid point array
  /// point ordering
  ///          12-----13  14-----15
  ///          | TAG 2 |  | TAG 3 |
  ///          8-------9  10-----11
  ///          4-------5  6-------7
  ///    y     | TAG 0 |  | TAG 1 |
  ///   ^      0-------1  2-------3
  ///   |-->x

  outCornerObserved.resize(size(), false);
  outImagePoints.resize(size(), 2);

  for (unsigned int i = 0; i < detections.size(); i++) {
    // get the tag id
    unsigned int tagId = detections[i].id;

    // calculate the grid idx for all four tag corners given the tagId and cols
    unsigned int baseId = (int) (tagId / (_cols / 2)) * _cols * 2
        + (tagId % (_cols / 2)) * 2;
    unsigned int pIdx[] = { baseId, baseId + 1, baseId + (unsigned int) _cols
        + 1, baseId + (unsigned int) _cols };

    // add four points per tag
    for (int j = 0; j < 4; j++) {
      //refined corners
      double corner_x = tagCorners.row(4 * i + j).at<float>(0);
      double corner_y = tagCorners.row(4 * i + j).at<float>(1);

      //raw corners
      double cornerRaw_x = tagCornersRaw.row(4 * i + j).at<float>(0);
      double cornerRaw_y = tagCornersRaw.row(4 * i + j).at<float>(1);

      //only add point if the displacement in the subpixel refinement is below a given threshold
      double subpix_displacement_squarred = (corner_x - cornerRaw_x)
          * (corner_x - cornerRaw_x)
          + (corner_y - cornerRaw_y) * (corner_y - cornerRaw_y);

      //add all points, but only set active if the point has not moved to far in the subpix refinement
      outImagePoints.row(pIdx[j]) = Eigen::Matrix<double, 1, 2>(corner_x,
                                                                corner_y);

      if (subpix_displacement_squarred <= _options.maxSubpixDisplacement2) {
        outCornerObserved[pIdx[j]] = true;
      } else {
        SM_DEBUG_STREAM("Subpix refinement failed for point: " << pIdx[j] << " with displacement: " << sqrt(subpix_displacement_squarred) << "(point removed) \n");
        outCornerObserved[pIdx[j]] = false;
      }
    }
  }

  //succesful observation
  return success;
}

}  // namespace cameras
}  // namespace aslam

//export explicit instantions for all included archives
#include <sm/boost/serialization.hpp>
#include <boost/serialization/export.hpp>
BOOST_CLASS_EXPORT_IMPLEMENT(aslam::cameras::GridCalibrationTargetAprilgrid);
BOOST_CLASS_EXPORT_IMPLEMENT(aslam::cameras::GridCalibrationTargetAprilgrid::AprilgridOptions);


//------------------------------------------------------------------------------
// Copyright (c) 2018 Hsienchi Kuo (Allen Institute, Hanchuan Peng's team)
// All rights reserved.
//------------------------------------------------------------------------------

/*******************************************************************************
*
*  This library intends to provide functionalities for neuron struct analysis, including graph and geometry analysis, etc.
*  Typically NeuronStructExplorer class methods need a profiledTree struct as part of the input arguments. A profiledTree can be assigned when the class is initiated or later.
*
*  profiledTree is the core data type in NeuronStructExplorer class. It profiles the NeuronTree and carries crucial information of it.
*  Particularly profiledTree provides node-location, child-location, and detailed segment information of a NeuronTree.
*  Each segment of a NeuronTree is represented as a segUnit struct. A segUnit struct carries within-segment node-location, child-location, head, and tails information.
*  All segments are stored and sorted in profiledTree's map<int, segUnit> data member.

*  The class can be initiated with or without a profiledTree being initiated at the same time. A profiledTree can be stored and indexed in NeuronStructExplorer's treeDatabe.
*
********************************************************************************/

#include <iostream>
#include <algorithm>
#include <cmath>

#include <boost/container/flat_map.hpp>

#include "basic_4dimage.h"
#include "basic_landmark.h"
#include "neuron_format_converter.h"

#include "NeuronStructExplorer.h"

using namespace std;

profiledTree::profiledTree(const NeuronTree& inputTree, float segTileLength)
{
	this->tree = inputTree;

	NeuronStructUtil::nodeTileMapGen(this->tree, this->nodeTileMap);

	NeuronStructUtil::node2loc_node2childLocMap(this->tree.listNeuron, this->node2LocMap, this->node2childLocMap);

	this->segs = NeuronStructExplorer::findSegs(this->tree.listNeuron, this->node2childLocMap);
	cout << "segs num: " << this->segs.size() << endl;
	vector<segUnit> allSegs;
	for (map<int, segUnit>::iterator it = this->segs.begin(); it != this->segs.end(); ++it)
	{
		//if (it->second.tails.size() > 1) cout << " branching seg: " << it->first << endl;
		allSegs.push_back(it->second);
	}
	this->segHeadMap = NeuronStructExplorer::segTileMap(allSegs, segTileLength);
	this->segTailMap = NeuronStructExplorer::segTileMap(allSegs, segTileLength, false);
}

NeuronStructExplorer::NeuronStructExplorer(QString inputFileName)
{
    cout<<NODE_TILE_LENGTH<<endl;
	QStringList fileNameParse1 = inputFileName.split(".");
	if (fileNameParse1.back() == "swc")
	{
        printf("%s", qPrintable(inputFileName));
		NeuronTree inputSWC = readSWC_file(inputFileName);
		QStringList fileNameParse2 = fileNameParse1.at(0).split("\\");
		string treeName = fileNameParse2.back().toStdString();
		this->treeEntry(inputSWC, treeName);
	}
}

void NeuronStructExplorer::treeEntry(const NeuronTree& inputTree, string treeName, float segTileLength)
{
	if (this->treeDataBase.find(treeName) == this->treeDataBase.end())
	{
		profiledTree registeredTree(inputTree, segTileLength);
		this->treeDataBase.insert(pair<string, profiledTree>(treeName, registeredTree));
	}
	else
	{
		cerr << "This tree name has already existed. The tree will not be registered for further operations." << endl;
		return;
	}
}

void NeuronStructExplorer::profiledTreeReInit(profiledTree& inputProfiledTree)
{
	profiledTree tempTree(inputProfiledTree.tree);
	inputProfiledTree = tempTree;
}

map<int, segUnit> NeuronStructExplorer::findSegs(const QList<NeuronSWC>& inputNodeList, const map<int, vector<size_t>>& node2childLocMap)
{
	// -- This method profiles all segments in a given input tree.

	map<int, segUnit> segs;
	vector<NeuronSWC> inputNodes;
	for (QList<NeuronSWC>::const_iterator nodeIt = inputNodeList.begin(); nodeIt != inputNodeList.end(); ++nodeIt)
		inputNodes.push_back(*nodeIt);

	int segCount = 0;
	for (vector<NeuronSWC>::iterator nodeIt = inputNodes.begin(); nodeIt != inputNodes.end(); ++nodeIt)
	{
		if (nodeIt->parent == -1)
		{
			// ------------ Identify segments ------------
			++segCount;
			//cout << " processing segment " << segCount << "..  head node ID: " << nodeIt->n << endl << "  ";
			segUnit newSeg;
			newSeg.segID = segCount;
			newSeg.head = nodeIt->n;
			newSeg.nodes.push_back(*nodeIt);
			vector<size_t> childLocs = node2childLocMap.find(nodeIt->n)->second;
				// In NeuronStructUtilities::node2loc_node2childLocMap,
				// if a segment is only with a head, its childLocs will also be assigned as empty set.
			if (childLocs.empty())
			{
				newSeg.tails.push_back(nodeIt->n);
				newSeg.seg_nodeLocMap.insert(pair<int, size_t>(nodeIt->n, 0));
				segs.insert(pair<int, segUnit>(newSeg.segID, newSeg));
				continue;
			}

			vector<size_t> grandChildLocs;
			vector<size_t> thisGrandChildLocs;
			bool childNum = true;
			while (childNum)
			{
				grandChildLocs.clear();
				for (vector<size_t>::iterator childIt = childLocs.begin(); childIt != childLocs.end(); ++childIt)
				{
					//cout << inputNodes.at(*childIt).n << " ";
					thisGrandChildLocs.clear();
					for (QList<NeuronSWC>::iterator checkIt = newSeg.nodes.begin(); checkIt != newSeg.nodes.end(); ++checkIt)
						if (checkIt->n == inputNodes.at(*childIt).n) goto LOOP;

					newSeg.nodes.push_back(inputNodes.at(*childIt));
					if (node2childLocMap.find(inputNodes.at(*childIt).n) == node2childLocMap.end())
					{
							newSeg.tails.push_back(inputNodeList[*childIt].n);
							//cout << "<-tail ";
					}
					else
					{
						thisGrandChildLocs = node2childLocMap.at(inputNodes.at(*childIt).n);
						grandChildLocs.insert(grandChildLocs.end(), thisGrandChildLocs.begin(), thisGrandChildLocs.end());
					}

				LOOP:
					continue;
				}
				if (grandChildLocs.size() == 0) childNum = false;

				childLocs = grandChildLocs;
			}
			//cout << endl;
			// --------- END of [Identify segments] ---------

			// ------------- Identify node/child location in the segment -------------
			for (QList<NeuronSWC>::iterator it = newSeg.nodes.begin(); it != newSeg.nodes.end(); ++it)
				newSeg.seg_nodeLocMap.insert(pair<int, size_t>(it->n, size_t(it - newSeg.nodes.begin())));

			for (QList<NeuronSWC>::iterator it = newSeg.nodes.begin(); it != newSeg.nodes.end(); ++it)
			{
				vector<size_t> childSegLocs;
				childSegLocs.clear();
				if (newSeg.seg_childLocMap.find(it->n) == newSeg.seg_childLocMap.end())
					newSeg.seg_childLocMap.insert(pair<int, vector<size_t>>(it->n, childSegLocs));
				// This ensures that tail node still has its own pair in seg_chileLocMap, and its childSegLocs is empty.

				if (node2childLocMap.find(it->n) != node2childLocMap.end())
				{
					vector<size_t> childLocs = node2childLocMap.at(it->n);
					for (vector<size_t>::iterator globalChildLocIt = childLocs.begin(); globalChildLocIt != childLocs.end(); ++globalChildLocIt)
					{
						size_t childSegLoc = newSeg.seg_nodeLocMap[inputNodeList.at(*globalChildLocIt).n];
						newSeg.seg_childLocMap[it->n].push_back(childSegLoc);
					}
				}
			}
			segs.insert(pair<int, segUnit>(newSeg.segID, newSeg));
			// --------- END of [Identify node/child location in the segment] ---------
		}
	}

	return segs;
}

map<string, vector<int>> NeuronStructExplorer::segTileMap(const vector<segUnit>& inputSegs, float xyLength, bool head)
{
	if (head)
	{
		map<string, vector<int>> outputSegTileMap;
		for (vector<segUnit>::const_iterator it = inputSegs.begin(); it != inputSegs.end(); ++it)
		{
			string xLabel = to_string(int(it->nodes.begin()->x / xyLength));
			string yLabel = to_string(int(it->nodes.begin()->y / xyLength));
			string zLabel = to_string(int(it->nodes.begin()->z / (xyLength / zRATIO)));
			string keyLabel = xLabel + "_" + yLabel + "_" + zLabel;
			if (outputSegTileMap.find(keyLabel) != outputSegTileMap.end()) outputSegTileMap[keyLabel].push_back(it->segID);
			else
			{
				vector<int> newSet;
				newSet.push_back(it->segID);
				outputSegTileMap.insert(pair<string, vector<int>>(keyLabel, newSet));
			}
		}
		return outputSegTileMap;
	}
	else
	{
		map<string, vector<int>> outputSegTileMap;
		for (vector<segUnit>::const_iterator it = inputSegs.begin(); it != inputSegs.end(); ++it)
		{
			for (vector<int>::const_iterator tailIt = it->tails.begin(); tailIt != it->tails.end(); ++tailIt)
			{
				NeuronSWC tailNode = it->nodes.at(it->seg_nodeLocMap.at(*tailIt));
				string xLabel = to_string(int(tailNode.x / xyLength));
				string yLabel = to_string(int(tailNode.y / xyLength));
				string zLabel = to_string(int(tailNode.z / (xyLength / zRATIO)));
				string keyLabel = xLabel + "_" + yLabel + "_" + zLabel;
				if (outputSegTileMap.find(keyLabel) != outputSegTileMap.end()) outputSegTileMap[keyLabel].push_back(it->segID);
				else
				{
					vector<int> newSet;
					newSet.push_back(it->segID);
					outputSegTileMap.insert(pair<string, vector<int>>(keyLabel, newSet));
				}
			}
		}
		return outputSegTileMap;
	}
}

void NeuronStructExplorer::treeUpSample(const profiledTree& inputProfiledTree, profiledTree& outputProfiledTree, float intervalLength)
{
	size_t maxNodeID = 0;
	for (QList<NeuronSWC>::const_iterator it = inputProfiledTree.tree.listNeuron.begin(); it != inputProfiledTree.tree.listNeuron.end(); ++it)
		if (it->n > maxNodeID) maxNodeID = it->n;

	for (map<int, segUnit>::const_iterator segIt = inputProfiledTree.segs.begin(); segIt != inputProfiledTree.segs.end(); ++segIt)
	{
		QList<NeuronSWC> newSegNodes;
		map<int, vector<QList<NeuronSWC>>> interpolatedNodeMap;
		for (QList<NeuronSWC>::const_iterator nodeIt = segIt->second.nodes.begin(); nodeIt != segIt->second.nodes.end() - 1; ++nodeIt)
		{
			vector<size_t> childLocs = segIt->second.seg_childLocMap.at(nodeIt->n);
			for (vector<size_t>::iterator childLocIt = childLocs.begin(); childLocIt != childLocs.end(); ++childLocIt)
			{
				float dist = sqrt((segIt->second.nodes.at(*childLocIt).x - nodeIt->x) * (segIt->second.nodes.at(*childLocIt).x - nodeIt->x) +
								  (segIt->second.nodes.at(*childLocIt).y - nodeIt->y) * (segIt->second.nodes.at(*childLocIt).y - nodeIt->y) +
								  (segIt->second.nodes.at(*childLocIt).z - nodeIt->z) * (segIt->second.nodes.at(*childLocIt).z - nodeIt->z));
				int intervals = int(dist / intervalLength);
				float intervalX = (segIt->second.nodes.at(*childLocIt).x - nodeIt->x) / float(intervals);
				float intervalY = (segIt->second.nodes.at(*childLocIt).y - nodeIt->y) / float(intervals);
				float intervalZ = (segIt->second.nodes.at(*childLocIt).z - nodeIt->z) / float(intervals);

				QList<NeuronSWC> interpolatedNodes;
				for (int i = 1; i < intervals; ++i)
				{
					NeuronSWC newNode;
					newNode.x = nodeIt->x + intervalX * float(i);
					newNode.y = nodeIt->y + intervalY * float(i);
					newNode.z = nodeIt->z + intervalZ * float(i);
					newNode.type = nodeIt->type;
					++maxNodeID;
					newNode.n = maxNodeID;
					newNode.parent = maxNodeID - 1;
					interpolatedNodes.push_back(newNode);
				}
				interpolatedNodes.push_back(segIt->second.nodes.at(*childLocIt));
				if (interpolatedNodes.size() >= 2)
				{
					interpolatedNodes.back().parent = (interpolatedNodes.end() - 2)->n;
					interpolatedNodes.begin()->parent = nodeIt->n;
				}

				interpolatedNodeMap[nodeIt->n].push_back(interpolatedNodes);
			}
		}
		for (map<int, vector<QList<NeuronSWC>>>::iterator mapIt = interpolatedNodeMap.begin(); mapIt != interpolatedNodeMap.end(); ++mapIt)
			for (vector<QList<NeuronSWC>>::iterator qlistIt = mapIt->second.begin(); qlistIt != mapIt->second.end(); ++qlistIt) newSegNodes.append(*qlistIt);
		newSegNodes.push_front(segIt->second.nodes.at(segIt->second.seg_nodeLocMap.at(segIt->second.head)));

		segUnit newSegUnit;
		newSegUnit.nodes = newSegNodes;
		outputProfiledTree.segs.insert(pair<int, segUnit>(segIt->first, newSegUnit));
		outputProfiledTree.tree.listNeuron.append(newSegNodes);
	}
}

// ========================================= Segment Analysis / Operations =========================================
segUnit NeuronStructExplorer::segUnitConnect_executer(const segUnit& segUnit1, const segUnit& segUnit2, connectOrientation connOrt, NeuronSWC* tailNodePtr1, NeuronSWC* tailNodePtr2)
{
	if (segUnit1.tails.size() > 1 || segUnit2.tails.size() > 1)
		throw invalid_argument("Currently forked segment connection is not supported. Do nothing and return");

	segUnit newSeg;
	QList<NeuronSWC> newSegNodes;
	QList<NeuronSWC> endEditedNodes;

	switch (connOrt)
	{
		case head_tail:
		{
			int connTailID = *segUnit2.tails.cbegin();
			endEditedNodes = segUnit1.nodes;
			endEditedNodes.begin()->parent = connTailID; // In current implementation, the 1st element of a seg must be a root.
			newSegNodes.append(segUnit2.nodes);
			newSegNodes.append(endEditedNodes);
			newSeg.nodes = newSegNodes;
			break;
		}
		case tail_head:
		{
			int connTailID = *segUnit1.tails.cbegin();
			endEditedNodes = segUnit2.nodes;
			endEditedNodes.begin()->parent = connTailID; // In current implementation, the 1st element of a seg must be a root.
			newSegNodes.append(segUnit1.nodes);
			newSegNodes.append(endEditedNodes);
			newSeg.nodes = newSegNodes;
			break;
		}
		case head_head:
		{
			int connTailID = segUnit2.head;
			for (map<int, vector<size_t>>::const_iterator it = segUnit2.seg_childLocMap.cbegin(); it != segUnit2.seg_childLocMap.cend(); ++it)
			{
				size_t nodeLoc = segUnit2.seg_nodeLocMap.at(it->first);
				NeuronSWC newNode = segUnit2.nodes.at(nodeLoc);
				if (it->second.empty())
				{
					//cout << newNode.x << " " << newNode.y << " " << newNode.z << endl;
					newNode.parent = -1;
				}
				else newNode.parent = segUnit2.nodes.at(*(it->second.cbegin())).n;
				endEditedNodes.push_back(newNode);
			}
			newSegNodes.append(segUnit1.nodes);
			newSegNodes.begin()->parent = connTailID;
			newSegNodes.append(endEditedNodes);
			newSeg.nodes = newSegNodes;
			break;
		}
		case tail_tail:
		{
			int connTailID = *segUnit2.tails.cbegin();
			for (map<int, vector<size_t>>::const_iterator it = segUnit1.seg_childLocMap.cbegin(); it != segUnit1.seg_childLocMap.cend(); ++it)
			{
				size_t nodeLoc = segUnit1.seg_nodeLocMap.at(it->first);
				NeuronSWC newNode = segUnit1.nodes.at(nodeLoc);
				if (it->second.empty()) newNode.parent = connTailID;
				else newNode.parent = segUnit1.nodes.at(*(it->second.cbegin())).n;
				endEditedNodes.push_back(newNode);
			}
			newSegNodes.append(segUnit2.nodes);
			newSegNodes.append(endEditedNodes);
			newSeg.nodes = newSegNodes;
			break;
		}
		default:
			break;
	}

	return newSeg;
}

map<int, segUnit> NeuronStructExplorer::segRegionConnector_angle(const vector<int>& currTileHeadSegIDs, const vector<int>& currTileTailSegIDs, profiledTree& currProfiledTree, double angleThre, bool length)
{
	set<int> connectedSegs;
	map<int, int> elongConnMap;
	map<string, double> segAngleMap;
	map<int, segUnit> newSegs;

	//__________________________________________________________________________________________________________________________

	//cout << "------- head-tail:" << endl;
	for (vector<int>::const_iterator headIt = currTileHeadSegIDs.begin(); headIt != currTileHeadSegIDs.end(); ++headIt)
	{
		for (vector<int>::const_iterator tailIt = currTileTailSegIDs.begin(); tailIt != currTileTailSegIDs.end(); ++tailIt)
		{
			if (*headIt == *tailIt) continue;
			else
			{
				if (currProfiledTree.segs.at(*headIt).to_be_deted || currProfiledTree.segs.at(*tailIt).to_be_deted) continue;

				double pointingRadAngle = this->segPointingCompare(currProfiledTree.segs.at(*headIt), currProfiledTree.segs.at(*tailIt), head_tail);
				double turningRadAngle = this->segTurningAngle(currProfiledTree.segs.at(*headIt), currProfiledTree.segs.at(*tailIt), head_tail);
				if (pointingRadAngle == -1 || turningRadAngle == -1) continue;

				if (pointingRadAngle < angleThre && turningRadAngle < angleThre)
				{
					//cout << *headIt << "_" << *tailIt << "->(" << pointingRadAngle << "," << turningRadAngle << ") ";
					string segAngleKey = to_string(*headIt) + "_" + to_string(*tailIt);
					segAngleMap.insert(pair<string, double>(segAngleKey, (pointingRadAngle + turningRadAngle)));
				}
			}
		}
	}
	this->tileSegConnOrganizer_angle(segAngleMap, connectedSegs, elongConnMap);
	for (map<int, int>::iterator it = elongConnMap.begin(); it != elongConnMap.end(); ++it)
	{
		currProfiledTree.segs[it->first].to_be_deted = true;
		currProfiledTree.segs[it->second].to_be_deted = true;
		segUnit newSeg = this->segUnitConnect_executer(currProfiledTree.segs[it->first], currProfiledTree.segs[it->second], head_tail);
		newSeg.segID = currProfiledTree.segs.size() + 1;
		while (currProfiledTree.segs.find(newSeg.segID) != currProfiledTree.segs.end()) ++newSeg.segID;
		currProfiledTree.segs.insert(pair<int, segUnit>(newSeg.segID, newSeg));
		newSegs.insert(pair<int, segUnit>(newSeg.segID, newSeg));
	}
	segAngleMap.clear();
	elongConnMap.clear();
	//cout << endl;
	//__________________________________________________________________________________________________________________________

	//cout << "------- tail-head:" << endl;
	for (vector<int>::const_iterator tailIt = currTileHeadSegIDs.begin(); tailIt != currTileHeadSegIDs.end(); ++tailIt)
	{
		for (vector<int>::const_iterator headIt = currTileTailSegIDs.begin(); headIt != currTileTailSegIDs.end(); ++headIt)
		{
			if (*headIt == *tailIt) continue;
			else
			{
				if (currProfiledTree.segs.at(*tailIt).to_be_deted || currProfiledTree.segs.at(*headIt).to_be_deted) continue;

				double pointingRadAngle = this->segPointingCompare(currProfiledTree.segs.at(*tailIt), currProfiledTree.segs.at(*headIt), tail_head);
				double turningRadAngle = this->segTurningAngle(currProfiledTree.segs.at(*tailIt), currProfiledTree.segs.at(*headIt), tail_head);
				if (pointingRadAngle == -1 || turningRadAngle == -1) continue;

				if (pointingRadAngle < angleThre && turningRadAngle < angleThre)
				{
					//cout << *tailIt << "_" << *headIt << "->(" << pointingRadAngle << "," << turningRadAngle << ") ";
					string segAngleKey = to_string(*tailIt) + "_" + to_string(*headIt);
					segAngleMap.insert(pair<string, double>(segAngleKey, (pointingRadAngle + turningRadAngle)));
				}
			}
		}
	}
	this->tileSegConnOrganizer_angle(segAngleMap, connectedSegs, elongConnMap);
	for (map<int, int>::iterator it = elongConnMap.begin(); it != elongConnMap.end(); ++it)
	{
		currProfiledTree.segs[it->first].to_be_deted = true;
		currProfiledTree.segs[it->second].to_be_deted = true;
		segUnit newSeg = this->segUnitConnect_executer(currProfiledTree.segs[it->first], currProfiledTree.segs[it->second], tail_head);
		newSeg.segID = currProfiledTree.segs.size() + 1;
		while (currProfiledTree.segs.find(newSeg.segID) != currProfiledTree.segs.end()) ++newSeg.segID;
		currProfiledTree.segs.insert(pair<int, segUnit>(newSeg.segID, newSeg));
		newSegs.insert(pair<int, segUnit>(newSeg.segID, newSeg));
	}
	segAngleMap.clear();
	elongConnMap.clear();
	//cout << endl;
	//__________________________________________________________________________________________________________________________

	//cout << "------- head-head:" << endl;
	for (vector<int>::const_iterator headIt1 = currTileHeadSegIDs.begin(); headIt1 != currTileHeadSegIDs.end(); ++headIt1)
	{
		for (vector<int>::const_iterator headIt2 = currTileHeadSegIDs.begin(); headIt2 != currTileHeadSegIDs.end(); ++headIt2)
		{
			if (*headIt1 == *headIt2) continue;
			else
			{
				if (currProfiledTree.segs.at(*headIt1).to_be_deted || currProfiledTree.segs.at(*headIt2).to_be_deted) continue;

				double pointingRadAngle = this->segPointingCompare(currProfiledTree.segs.at(*headIt1), currProfiledTree.segs.at(*headIt2), head_head);
				double turningRadAngle = this->segTurningAngle(currProfiledTree.segs.at(*headIt1), currProfiledTree.segs.at(*headIt2), head_head);
				if (pointingRadAngle == -1 || turningRadAngle == -1) continue;

				if (pointingRadAngle < angleThre && turningRadAngle < angleThre)
				{
					//cout << *headIt1 << "_" << *headIt2 << "->(" << pointingRadAngle << "," << turningRadAngle << ") ";
					string segAngleKey = to_string(*headIt1) + "_" + to_string(*headIt2);
					segAngleMap.insert(pair<string, double>(segAngleKey, (pointingRadAngle + turningRadAngle)));
				}
			}
		}
	}
	this->tileSegConnOrganizer_angle(segAngleMap, connectedSegs, elongConnMap);
	for (map<int, int>::iterator it = elongConnMap.begin(); it != elongConnMap.end(); ++it)
	{
		currProfiledTree.segs[it->first].to_be_deted = true;
		currProfiledTree.segs[it->second].to_be_deted = true;
		segUnit newSeg = this->segUnitConnect_executer(currProfiledTree.segs[it->first], currProfiledTree.segs[it->second], head_head);
		newSeg.segID = currProfiledTree.segs.size() + 1;
		while (currProfiledTree.segs.find(newSeg.segID) != currProfiledTree.segs.end()) ++newSeg.segID;
		currProfiledTree.segs.insert(pair<int, segUnit>(newSeg.segID, newSeg));
		newSegs.insert(pair<int, segUnit>(newSeg.segID, newSeg));
	}
	segAngleMap.clear();
	elongConnMap.clear();
	//cout << endl;
	//__________________________________________________________________________________________________________________________

	//cout << "------- tail-tail:" << endl;
	for (vector<int>::const_iterator tailIt1 = currTileHeadSegIDs.begin(); tailIt1 != currTileHeadSegIDs.end(); ++tailIt1)
	{
		for (vector<int>::const_iterator tailIt2 = currTileTailSegIDs.begin(); tailIt2 != currTileTailSegIDs.end(); ++tailIt2)
		{
			if (*tailIt1 == *tailIt2) continue;
			else
			{
				if (currProfiledTree.segs.at(*tailIt1).to_be_deted || currProfiledTree.segs.at(*tailIt2).to_be_deted) continue;

				double pointingRadAngle = this->segPointingCompare(currProfiledTree.segs.at(*tailIt1), currProfiledTree.segs.at(*tailIt2), tail_tail);
				double turningRadAngle = this->segTurningAngle(currProfiledTree.segs.at(*tailIt1), currProfiledTree.segs.at(*tailIt2), tail_tail);
				if (pointingRadAngle == -1 || turningRadAngle == -1) continue;

				if (pointingRadAngle < angleThre && turningRadAngle < angleThre)
				{
					//cout << *tailIt1 << "_" << *tailIt2 << "->(" << pointingRadAngle << "," << turningRadAngle << ") ";
					string segAngleKey = to_string(*tailIt1) + "_" + to_string(*tailIt2);
					segAngleMap.insert(pair<string, double>(segAngleKey, (pointingRadAngle + turningRadAngle)));
				}
			}
		}
	}
	this->tileSegConnOrganizer_angle(segAngleMap, connectedSegs, elongConnMap);
	for (map<int, int>::iterator it = elongConnMap.begin(); it != elongConnMap.end(); ++it)
	{
		currProfiledTree.segs[it->first].to_be_deted = true;
		currProfiledTree.segs[it->second].to_be_deted = true;
		segUnit newSeg = this->segUnitConnect_executer(currProfiledTree.segs[it->first], currProfiledTree.segs[it->second], tail_tail);
		newSeg.segID = currProfiledTree.segs.size() + 1;
		while (currProfiledTree.segs.find(newSeg.segID) != currProfiledTree.segs.end()) ++newSeg.segID;
		currProfiledTree.segs.insert(pair<int, segUnit>(newSeg.segID, newSeg));
		newSegs.insert(pair<int, segUnit>(newSeg.segID, newSeg));
	}
	segAngleMap.clear();
	elongConnMap.clear();
	//cout << endl << endl;
	//__________________________________________________________________________________________________________________________

	return newSegs;
}

profiledTree NeuronStructExplorer::segElongate(const profiledTree& inputProfiledTree, double angleThre)
{
	//cout << inputProfiledTree.segHeadMap.size() << " " << inputProfiledTree.segTailMap.size() << endl;

	profiledTree outputProfiledTree = inputProfiledTree;
	QList<NeuronSWC> newNodeList = inputProfiledTree.tree.listNeuron;
	map<int, segUnit> allNewSegs;

	set<string> allTileKeys;
	for (map<string, vector<int>>::const_iterator headTileIt = inputProfiledTree.segHeadMap.begin(); headTileIt != inputProfiledTree.segHeadMap.end(); ++headTileIt)
		allTileKeys.insert(headTileIt->first);
	for (map<string, vector<int>>::const_iterator tailTileIt = inputProfiledTree.segTailMap.begin(); tailTileIt != inputProfiledTree.segTailMap.end(); ++tailTileIt)
		allTileKeys.insert(tailTileIt->first);

	vector<int> currTileHeadSegIDs;
	vector<int> currTileTailSegIDs;
	set<int> connectedSegs;
	for (set<string>::iterator keyIt = allTileKeys.begin(); keyIt != allTileKeys.end(); ++keyIt)
	{
		//cout << "TILE " << *keyIt << " =======" << endl;
		//cout << " - Heads: ";
		if (inputProfiledTree.segHeadMap.find(*keyIt) != inputProfiledTree.segHeadMap.end())
		{
			for (vector<int>::const_iterator headIt = inputProfiledTree.segHeadMap.at(*keyIt).begin(); headIt != inputProfiledTree.segHeadMap.at(*keyIt).end(); ++headIt)
			{
				//cout << *headIt << " ";
				currTileHeadSegIDs.push_back(*headIt);
			}
		}
		//cout << endl;
		//cout << " - Tails: ";
		if (inputProfiledTree.segTailMap.find(*keyIt) != inputProfiledTree.segTailMap.end())
		{
			for (vector<int>::const_iterator tailIt = inputProfiledTree.segTailMap.at(*keyIt).begin(); tailIt != inputProfiledTree.segTailMap.at(*keyIt).end(); ++tailIt)
			{
				//cout << *tailIt << " ";
				currTileTailSegIDs.push_back(*tailIt);
			}
		}
		//cout << endl;
		if (currTileHeadSegIDs.size() + currTileTailSegIDs.size() <= 1) // If there is <= 1 head or tail (only 1 terminal) in the tile, no further process is needed.
		{
			currTileHeadSegIDs.clear();
			currTileTailSegIDs.clear();
			//cout << endl;
			continue;
		}

		map<int, segUnit> newSegs = this->segRegionConnector_angle(currTileHeadSegIDs, currTileTailSegIDs, outputProfiledTree, angleThre);
		for (map<int, segUnit>::iterator newSegIt = newSegs.begin(); newSegIt != newSegs.end(); ++newSegIt)
			allNewSegs.insert(pair<int, segUnit>(newSegIt->first, newSegIt->second));

		currTileHeadSegIDs.clear();
		currTileTailSegIDs.clear();
	}

	vector<size_t> nodeDeleteLocs;
	bool segDeleted = true;
	while (segDeleted)
	{
		for (map<int, segUnit>::iterator it = outputProfiledTree.segs.begin(); it != outputProfiledTree.segs.end(); ++it)
		{
			if (it->second.to_be_deted)
			{
				//cout << it->first << ": " << it->second.nodes.size() << endl;
				for (QList<NeuronSWC>::iterator nodeIt = it->second.nodes.begin(); nodeIt != it->second.nodes.end(); ++nodeIt)
					nodeDeleteLocs.push_back(outputProfiledTree.node2LocMap.at(nodeIt->n));
				outputProfiledTree.segs.erase(it);

				goto SEG_DELETED;
			}
		}
		segDeleted = false;

	SEG_DELETED:
		continue;
	}
	//cout << endl;

	sort(nodeDeleteLocs.rbegin(), nodeDeleteLocs.rend());
	for (vector<size_t>::iterator it = nodeDeleteLocs.begin(); it != nodeDeleteLocs.end(); ++it)
		outputProfiledTree.tree.listNeuron.erase(outputProfiledTree.tree.listNeuron.begin() + ptrdiff_t(*it));

	//cout << endl << "NEW SEGMENTS ------------- " << endl;
	for (map<int, segUnit>::iterator it = allNewSegs.begin(); it != allNewSegs.end(); ++it)
	{
		//cout << it->first << ": " << it->second.nodes.size() << endl;
		outputProfiledTree.tree.listNeuron.append(it->second.nodes);
	}

	this->profiledTreeReInit(outputProfiledTree);
	return outputProfiledTree;
}

profiledTree NeuronStructExplorer::treeUnion_MSTbased(const profiledTree& expandingPart, const profiledTree& baseTree)
{
	set<int> connectedSegs;
	profiledTree outputProfiledTree = baseTree;

	for (map<int, segUnit>::const_iterator segIt = expandingPart.segs.begin(); segIt != expandingPart.segs.end(); ++segIt)
	{
		NeuronSWC headNode = *segIt->second.nodes.begin();
		string xLabel = to_string(int(headNode.x / tileXY_LENGTH));
		string yLabel = to_string(int(headNode.y / tileXY_LENGTH));
		string zLabel = to_string(int(headNode.z / (tileXY_LENGTH / zRATIO)));
		string keyLabel = xLabel + "_" + yLabel + "_" + zLabel;
		//cout << keyLabel << ": ";

		vector<int> baseSegHeads;
		vector<int> baseSegTails;
		vector<int> baseSegs;
		if (baseTree.segHeadMap.find(keyLabel) != baseTree.segHeadMap.end()) baseSegHeads = baseTree.segHeadMap.at(keyLabel);
		if (baseTree.segTailMap.find(keyLabel) != baseTree.segTailMap.end()) baseSegTails = baseTree.segTailMap.at(keyLabel);
		baseSegs = baseSegHeads;
		for (vector<int>::iterator tileSegIt = baseSegTails.begin(); tileSegIt != baseSegTails.end(); ++tileSegIt)
		{
			if (find(baseSegHeads.begin(), baseSegHeads.end(), *tileSegIt) == baseSegs.end())
				baseSegs.push_back(*tileSegIt);
		}
		//for (vector<int>::iterator checkSegIt = baseSegs.begin(); checkSegIt != baseSegs.end(); ++checkSegIt) cout << *checkSegIt << " ";
		//cout << endl;

		boost::container::flat_map<int, connectOrientation> connRoutingMap;
		for (QList<NeuronSWC>::const_iterator nodeIt = segIt->second.nodes.begin(); nodeIt != segIt->second.nodes.end(); ++nodeIt)
		{
			for (vector<int>::iterator baseSegIt = baseSegs.begin(); baseSegIt != baseSegs.end(); ++baseSegIt)
			{
				NeuronSWC baseSegHeadNode = *baseTree.segs.at(*baseSegIt).nodes.begin();
				if (nodeIt->x == baseSegHeadNode.x && nodeIt->y == baseSegHeadNode.y && nodeIt->z == baseSegHeadNode.z)
				{
					connRoutingMap.insert(pair<int, connectOrientation>(*baseSegIt, head));
					goto SEGMENT_CONFIRMED;
				}

				for (vector<int>::const_iterator tailIt = baseTree.segs.at(*baseSegIt).tails.begin(); tailIt != baseTree.segs.at(*baseSegIt).tails.end(); ++tailIt)
				{
					NeuronSWC baseSegTailNode = baseTree.segs.at(*baseSegIt).nodes.at(baseTree.segs.at(*baseSegIt).seg_nodeLocMap.at(*tailIt));
					if (nodeIt->x == baseSegTailNode.x && nodeIt->y == baseSegTailNode.y && nodeIt->z == baseSegTailNode.z)
					{
						connRoutingMap.insert(pair<int, connectOrientation>(*baseSegIt, tail));
						goto SEGMENT_CONFIRMED;
					}
				}
			}

		SEGMENT_CONFIRMED:
			continue;
		}

		if (connRoutingMap.size() == 0) continue;

		int segID1 = baseTree.segs.at(connRoutingMap.begin()->first).segID;
		connectOrientation ort1 = connRoutingMap.begin()->second;
		for (boost::container::flat_map<int, connectOrientation>::iterator connIt = connRoutingMap.begin() + 1; connIt != connRoutingMap.end(); ++connIt)
		{
			int segID2 = connIt->first;
			connectOrientation ort2 = connIt->second;
			if (segID1 == segID2)
			{
				segID1 = segID2;
				ort1 = ort2;
				continue;
			}

			if (ort1 == head && ort2 == head)
			{
				int nodeID1 = outputProfiledTree.segs.at(segID1).head;
				int nodeID2 = outputProfiledTree.segs.at(segID2).head;
				size_t nodeLoc1 = outputProfiledTree.node2LocMap.at(nodeID1);
				outputProfiledTree.tree.listNeuron[nodeLoc1].parent = nodeID2;
			}
			else if (ort1 == head && ort2 == tail)
			{
				int nodeID1 = outputProfiledTree.segs.at(segID1).head;
				int nodeID2 = *outputProfiledTree.segs.at(segID2).tails.begin();
				size_t nodeLoc1 = outputProfiledTree.node2LocMap.at(nodeID1);
				outputProfiledTree.tree.listNeuron[nodeLoc1].parent = nodeID2;
			}
			else if (ort1 == tail && ort2 == head)
			{
				int nodeID1 = *outputProfiledTree.segs.at(segID1).tails.begin();
				int nodeID2 = outputProfiledTree.segs.at(segID2).head;
				size_t nodeLoc2 = outputProfiledTree.node2LocMap.at(nodeID2);
				outputProfiledTree.tree.listNeuron[nodeLoc2].parent = nodeID1;
			}
		}
	}

	return outputProfiledTree;
}
// ===================================== END of [Segment Analysis / Operations] =====================================

// ================================================== Geometry =================================================
double NeuronStructExplorer::segPointingCompare(const segUnit& elongSeg, const segUnit& connSeg, connectOrientation connOrt)
{
	if (elongSeg.tails.size() > 1 || connSeg.tails.size() > 1)
	{
		cerr << "Invalid input: Currently segment elongation only allows to happen between 2 non-branching segments." << endl;
		return -1;
	}

	NeuronSWC elongHeadNode = elongSeg.nodes[elongSeg.seg_nodeLocMap.at(elongSeg.head)];
	NeuronSWC elongTailNode;
	if (elongSeg.tails.size() == 0)
	{
		//cerr << "Elongating segment only has head. Do nothinig and return.";
		return -1;
	}
	else elongTailNode = elongSeg.nodes[elongSeg.seg_nodeLocMap.at(*elongSeg.tails.begin())];

	NeuronSWC connHeadNode = connSeg.nodes[connSeg.seg_nodeLocMap.at(connSeg.head)];
	NeuronSWC connTailNode;
	if (connSeg.tails.size() == 0)
	{
		if (connOrt == head_tail || connOrt == tail_tail)
		{
			//cerr << "Connecting segment only has head. Do nothing and return";
			return -1;
		}
	}
	else connTailNode = connSeg.nodes[connSeg.seg_nodeLocMap.at(*connSeg.tails.begin())];

	vector<float> elongHeadLoc;
	elongHeadLoc.push_back(elongHeadNode.x);
	elongHeadLoc.push_back(elongHeadNode.y);
	elongHeadLoc.push_back(elongHeadNode.z * zRATIO);

	vector<float> elongTailLoc;
	elongTailLoc.push_back(elongTailNode.x);
	elongTailLoc.push_back(elongTailNode.y);
	elongTailLoc.push_back(elongTailNode.z * zRATIO);

	vector<float> connHeadLoc;
	connHeadLoc.push_back(connHeadNode.x);
	connHeadLoc.push_back(connHeadNode.y);
	connHeadLoc.push_back(connHeadNode.z * zRATIO);

	vector<float> connTailLoc;
	connTailLoc.push_back(connTailNode.x);
	connTailLoc.push_back(connTailNode.y);
	connTailLoc.push_back(connTailNode.z * zRATIO);

	vector<float> elongDispUnitVec;
	vector<float> connDispUnitVec;
	if (connOrt == head_head)
	{
		elongDispUnitVec = this->getDispUnitVector(elongHeadLoc, elongTailLoc);
		connDispUnitVec = this->getDispUnitVector(connTailLoc, connHeadLoc);

		double radAngle = this->getRadAngle(elongDispUnitVec, connDispUnitVec);
		return radAngle;
	}
	else if (connOrt == head_tail)
	{
		elongDispUnitVec = this->getDispUnitVector(elongHeadLoc, elongTailLoc);
		connDispUnitVec = this->getDispUnitVector(connHeadLoc, connTailLoc);

		double radAngle = this->getRadAngle(elongDispUnitVec, connDispUnitVec);
		return radAngle;
	}
	else if (connOrt == tail_head)
	{
		elongDispUnitVec = this->getDispUnitVector(elongHeadLoc, elongTailLoc);
		connDispUnitVec = this->getDispUnitVector(connHeadLoc, connTailLoc);

		double radAngle = this->getRadAngle(elongDispUnitVec, connDispUnitVec);
		return radAngle;
	}
	else if (connOrt == tail_tail)
	{
		elongDispUnitVec = this->getDispUnitVector(elongTailLoc, elongHeadLoc);
		connDispUnitVec = this->getDispUnitVector(connHeadLoc, connTailLoc);

		double radAngle = this->getRadAngle(elongDispUnitVec, connDispUnitVec);
		return radAngle;
	}
}

double NeuronStructExplorer::segTurningAngle(const segUnit& elongSeg, const segUnit& connSeg, connectOrientation connOrt)
{
	if (elongSeg.tails.size() > 1 || connSeg.tails.size() > 1)
	{
		cerr << "Invalid input: Currently segment elongation only allows to happen between 2 non-branching segments." << endl;
		return -1;
	}

	NeuronSWC elongHeadNode = elongSeg.nodes[elongSeg.seg_nodeLocMap.at(elongSeg.head)];
	NeuronSWC elongTailNode;
	if (elongSeg.tails.size() == 0)
	{
		//cerr << "Elongating segment only has head. Do nothinig and return.";
		return -1;
	}
	else elongTailNode = elongSeg.nodes[elongSeg.seg_nodeLocMap.at(*elongSeg.tails.begin())];

	NeuronSWC connHeadNode = connSeg.nodes[connSeg.seg_nodeLocMap.at(connSeg.head)];
	NeuronSWC connTailNode;
	if (connSeg.tails.size() == 0)
	{
		if (connOrt == head_tail || connOrt == tail_tail)
		{
			//cerr << "Connecting segment only has head. Do nothing and return";
			return -1;
		}
	}
	else connTailNode = connSeg.nodes[connSeg.seg_nodeLocMap.at(*connSeg.tails.begin())];

	vector<float> elongHeadLoc;
	elongHeadLoc.push_back(elongHeadNode.x);
	elongHeadLoc.push_back(elongHeadNode.y);
	elongHeadLoc.push_back(elongHeadNode.z * zRATIO);

	vector<float> elongTailLoc;
	elongTailLoc.push_back(elongTailNode.x);
	elongTailLoc.push_back(elongTailNode.y);
	elongTailLoc.push_back(elongTailNode.z * zRATIO);

	vector<float> elongDispUnitVec;
	vector<float> connPointUnitVec;
	if (connOrt == head_head)
	{
		elongDispUnitVec = this->getDispUnitVector(elongHeadLoc, elongTailLoc);
		vector<float> connPointLoc;
		connPointLoc.push_back(connHeadNode.x);
		connPointLoc.push_back(connHeadNode.y);
		connPointLoc.push_back(connHeadNode.z * zRATIO);
		connPointUnitVec = this->getDispUnitVector(connPointLoc, elongHeadLoc);

		double radAngle = this->getRadAngle(elongDispUnitVec, connPointUnitVec);
		return radAngle;
	}
	else if (connOrt == head_tail)
	{
		elongDispUnitVec = this->getDispUnitVector(elongHeadLoc, elongTailLoc);
		vector<float> connPointLoc;
		connPointLoc.push_back(connTailNode.x);
		connPointLoc.push_back(connTailNode.y);
		connPointLoc.push_back(connTailNode.z * zRATIO);
		connPointUnitVec = this->getDispUnitVector(connPointLoc, elongHeadLoc);

		double radAngle = this->getRadAngle(elongDispUnitVec, connPointUnitVec);
		return radAngle;
	}
	else if (connOrt == tail_head)
	{
		elongDispUnitVec = this->getDispUnitVector(elongTailLoc, elongHeadLoc);
		vector<float> connPointLoc;
		connPointLoc.push_back(connHeadNode.x);
		connPointLoc.push_back(connHeadNode.y);
		connPointLoc.push_back(connHeadNode.z * zRATIO);
		connPointUnitVec = this->getDispUnitVector(connPointLoc, elongTailLoc);

		double radAngle = this->getRadAngle(elongDispUnitVec, connPointUnitVec);
		return radAngle;
	}
	else if (connOrt == tail_tail)
	{
		elongDispUnitVec = this->getDispUnitVector(elongTailLoc, elongHeadLoc);
		vector<float> connPointLoc;
		connPointLoc.push_back(connTailNode.x);
		connPointLoc.push_back(connTailNode.y);
		connPointLoc.push_back(connTailNode.z * zRATIO);
		connPointUnitVec = this->getDispUnitVector(connPointLoc, elongTailLoc);

		double radAngle = this->getRadAngle(elongDispUnitVec, connPointUnitVec);
		return radAngle;
	}
}
// ============================================= End of [Geometry] =============================================

NeuronTree NeuronStructExplorer::SWC2MSTtree(NeuronTree const& inputTreePtr)
{
	NeuronTree MSTtrees;
	undirectedGraph graph(inputTreePtr.listNeuron.size());
	for (int i = 0; i < inputTreePtr.listNeuron.size(); ++i)
	{

		float x1, y1, z1;
		x1 = inputTreePtr.listNeuron.at(i).x;
		y1 = inputTreePtr.listNeuron.at(i).y;
		z1 = inputTreePtr.listNeuron.at(i).z;
		for (int j = 0; j < inputTreePtr.listNeuron.size(); ++j)
		{
			float x2, y2, z2;
			x2 = inputTreePtr.listNeuron.at(j).x;
			y2 = inputTreePtr.listNeuron.at(j).y;
			z2 = inputTreePtr.listNeuron.at(j).z;

			double Vedge = sqrt(double(x1 - x2) * double(x1 - x2) + double(y1 - y2) * double(y1 - y2) + zRATIO * zRATIO * double(z1 - z2) * double(z1 - z2));
			pair<undirectedGraph::edge_descriptor, bool> edgeQuery = boost::edge(i, j, graph);
			if (!edgeQuery.second && i != j) boost::add_edge(i, j, lastVoted(i, weights(Vedge)), graph);
		}
	}

	vector <boost::graph_traits<undirectedGraph>::vertex_descriptor > p(num_vertices(graph));
	boost::prim_minimum_spanning_tree(graph, &p[0]);
	NeuronTree MSTtree;
	QList<NeuronSWC> listNeuron;
	QHash<int, int>  hashNeuron;
	listNeuron.clear();
	hashNeuron.clear();

	for (size_t ii = 0; ii != p.size(); ++ii)
	{
		int pn;
		if (p[ii] == ii) pn = -1;
		else pn = p[ii] + 1;

		NeuronSWC S;
		S.n = ii + 1;
		S.type = 7;
		S.x = inputTreePtr.listNeuron.at(ii).x;
		S.y = inputTreePtr.listNeuron.at(ii).y;
		S.z = inputTreePtr.listNeuron.at(ii).z;
		S.r = 1;
		S.pn = pn;
		listNeuron.append(S);
		hashNeuron.insert(S.n, listNeuron.size() - 1);
	}
	MSTtree.listNeuron = listNeuron;
	MSTtree.hashNeuron = hashNeuron;

	return MSTtree;
}

NeuronTree NeuronStructExplorer::MSTbranchBreak(const profiledTree& inputProfiledTree, double spikeThre, bool spikeRemove)
{
	// -- This method breaks all branching points of a MST-connected tree.
	// -- When the value of spikeRemove is true, it eliminates spikes on a main route without breaking it. The default value is true.

	profiledTree outputProfiledTree(inputProfiledTree.tree);

	vector<size_t> spikeLocs;
	if (spikeRemove)
	{
		for (QList<NeuronSWC>::iterator it = outputProfiledTree.tree.listNeuron.begin(); it != outputProfiledTree.tree.listNeuron.end(); ++it)
		{
			if (outputProfiledTree.node2childLocMap.find(it->n) != outputProfiledTree.node2childLocMap.end())
			{
				vector<size_t> childLocs = outputProfiledTree.node2childLocMap.at(it->n);
				if (childLocs.size() >= 2)
				{
					int nodeRemoveCount = 0;
					for (vector<size_t>::iterator locIt = childLocs.begin(); locIt != childLocs.end(); ++locIt)
					{
						if (outputProfiledTree.node2childLocMap.find(outputProfiledTree.tree.listNeuron.at(*locIt).n) == outputProfiledTree.node2childLocMap.end())
						{
							double spikeDist = sqrt((outputProfiledTree.tree.listNeuron.at(*locIt).x - it->x) * (outputProfiledTree.tree.listNeuron.at(*locIt).x - it->x) +
								(outputProfiledTree.tree.listNeuron.at(*locIt).y - it->y) * (outputProfiledTree.tree.listNeuron.at(*locIt).y - it->y) +
								(outputProfiledTree.tree.listNeuron.at(*locIt).z - it->z) * (outputProfiledTree.tree.listNeuron.at(*locIt).z - it->z) * zRATIO * zRATIO);
							if (spikeDist <= spikeThre) // Take out splikes.
							{
								spikeLocs.push_back(*locIt);
								++nodeRemoveCount;
							}
						}
					}

					if (nodeRemoveCount == childLocs.size() - 1) continue; // If there is only 1 child left after taking out all spikes, then this node is supposed to be on the main route.
					else
					{
						for (vector<size_t>::iterator locCheckIt = childLocs.begin(); locCheckIt != childLocs.end(); ++locCheckIt)
						{
							if (find(spikeLocs.begin(), spikeLocs.end(), *locCheckIt) == spikeLocs.end())
								outputProfiledTree.tree.listNeuron[*locCheckIt].parent = -1; // 'at' operator treats the container as const, and cannot assign values. Therefore, use [] instead.
						}
					}
				}
			}
		}
	}
	else
	{
		for (QList<NeuronSWC>::iterator it = outputProfiledTree.tree.listNeuron.begin(); it != outputProfiledTree.tree.listNeuron.end(); ++it)
		{
			if (outputProfiledTree.node2childLocMap.find(it->n) != outputProfiledTree.node2childLocMap.end())
			{
				if (outputProfiledTree.node2childLocMap.at(it->n).size() >= 2)
				{
					for (vector<size_t>::const_iterator locIt = outputProfiledTree.node2childLocMap.at(it->n).begin(); locIt != outputProfiledTree.node2childLocMap.at(it->n).end(); ++locIt)
						outputProfiledTree.tree.listNeuron[*locIt].parent = -1;
				}
			}
		}
	}

	if (spikeRemove) // Erase spike nodes from outputTree.listNeuron.
	{
		sort(spikeLocs.rbegin(), spikeLocs.rend());
		for (vector<size_t>::iterator delIt = spikeLocs.begin(); delIt != spikeLocs.end(); ++delIt)
			outputProfiledTree.tree.listNeuron.erase(outputProfiledTree.tree.listNeuron.begin() + ptrdiff_t(*delIt));
	}

	return outputProfiledTree.tree;
}

/*vector<segUnit> NeuronStructExplorer::MSTtreeTrim(vector<segUnit>& inputSegUnits)
{
	vector<segUnit> outputSegUnits = inputSegUnits;

	for (vector<segUnit>::iterator segIt = outputSegUnits.begin(); segIt != outputSegUnits.end(); ++segIt)
	{
		if (segIt->nodes.size() <= 3) continue;

		NeuronStructUtil::node2loc_node2childLocMap(segIt->nodes, segIt->seg_nodeLocMap, segIt->seg_childLocMap);
		for (QList<NeuronSWC>::iterator nodeIt = segIt->nodes.begin(); nodeIt != segIt->nodes.end(); ++nodeIt)
		{
			if (segIt->seg_childLocMap[nodeIt->n].size() == 2)
			{
				if (nodeIt->parent == -1) continue;

				topoCharacter newTopoCenter(*nodeIt);
				newTopoCenter.topoCenterPa = segIt->nodes.at(segIt->seg_nodeLocMap[nodeIt->parent]);
				for (vector<size_t>::iterator immedChildLocIt = segIt->seg_childLocMap[nodeIt->n].begin(); immedChildLocIt != segIt->seg_childLocMap[nodeIt->n].end(); ++immedChildLocIt)
					newTopoCenter.topoCenterImmedChildren.push_back(segIt->nodes.at(*immedChildLocIt));

				if (segIt->seg_childLocMap[newTopoCenter.topoCenterImmedChildren.at(0).n].size() == 0 || segIt->seg_childLocMap[newTopoCenter.topoCenterImmedChildren.at(1).n].size() == 0)
					continue;

				double dot = (newTopoCenter.topoCenterImmedChildren.at(0).x - nodeIt->x) * (newTopoCenter.topoCenterImmedChildren.at(1).x - nodeIt->x) +
							 (newTopoCenter.topoCenterImmedChildren.at(0).y - nodeIt->y) * (newTopoCenter.topoCenterImmedChildren.at(1).y - nodeIt->y) +
							 (newTopoCenter.topoCenterImmedChildren.at(0).z - nodeIt->z) * (newTopoCenter.topoCenterImmedChildren.at(1).z - nodeIt->z);

				double sq1 = (newTopoCenter.topoCenterImmedChildren.at(0).x - nodeIt->x) * (newTopoCenter.topoCenterImmedChildren.at(0).x - nodeIt->x) +
							 (newTopoCenter.topoCenterImmedChildren.at(0).y - nodeIt->y) * (newTopoCenter.topoCenterImmedChildren.at(0).y - nodeIt->y) +
							 (newTopoCenter.topoCenterImmedChildren.at(0).z - nodeIt->z) * (newTopoCenter.topoCenterImmedChildren.at(0).z - nodeIt->z);

				double sq2 = (newTopoCenter.topoCenterImmedChildren.at(1).x - nodeIt->x) * (newTopoCenter.topoCenterImmedChildren.at(1).x - nodeIt->x) +
							 (newTopoCenter.topoCenterImmedChildren.at(1).y - nodeIt->y) * (newTopoCenter.topoCenterImmedChildren.at(1).y - nodeIt->y) +
							 (newTopoCenter.topoCenterImmedChildren.at(1).z - nodeIt->z) * (newTopoCenter.topoCenterImmedChildren.at(1).z - nodeIt->z);

				newTopoCenter.childAngle = acos(dot / sqrt(sq1 * sq2));
				if (isnan(acos(dot / sqrt(sq1 * sq2)))) newTopoCenter.childAngle = -1;

				newTopoCenter.immedChildrenLengths.insert(pair<int, double>(newTopoCenter.topoCenterImmedChildren.at(0).n, sqrt(sq1)));
				newTopoCenter.immedChildrenLengths.insert(pair<int, double>(newTopoCenter.topoCenterImmedChildren.at(1).n, sqrt(sq2)));

				segIt->topoCenters.push_back(newTopoCenter);
			}
		}
	}

	for (vector<segUnit>::iterator segIt = outputSegUnits.begin(); segIt != outputSegUnits.end(); ++segIt)
	{
		for (vector<topoCharacter>::iterator topoIt = segIt->topoCenters.begin(); topoIt != segIt->topoCenters.end(); ++topoIt)
		{
			if (topoIt->childAngle / PI <= 0.55 && topoIt->childAngle / PI >= 0.45) segIt->nodes[segIt->seg_nodeLocMap[topoIt->topoCenter.n]].type = 3;
		}
	}

    return outputSegUnits;*/

	/*if (inputSeg.size() <= 3) return;

#define PI 3.1415926
	//long segInfoNodeCount = 0;

	double dot, sq1, sq2, dist, turnCost, radAngle, turnCostMean = 0;
	double mainSqr = ((inputSeg.end() - 1)->x - inputSeg.begin()->x)*((inputSeg.end() - 1)->x - inputSeg.begin()->x) +
		((inputSeg.end() - 1)->y - inputSeg.begin()->y)*((inputSeg.end() - 1)->y - inputSeg.begin()->y) +
		((inputSeg.end() - 1)->z - inputSeg.begin()->z)*((inputSeg.end() - 1)->z - inputSeg.begin()->z);
	double segDist = sqrt(mainSqr);
	double nodeHeadDot, nodeHeadSqr, nodeHeadRadAngle, nodeToMainDist, nodeToMainDistMean = 0;
	//cout << "segment displacement: " << segDist << endl << endl;

	cout << "    -- nodes contained in the segment to be straightned: " << segInfoPtr->nodeCount << endl;
	for (vector<V_NeuronSWC_unit>::iterator check = inputSeg.begin(); check != inputSeg.end(); ++check) cout << "[" << check->x << " " << check->y << " " << check->z << "] ";
	cout << endl;
	vector<nodeInfo> pickedNode;
	for (vector<V_NeuronSWC_unit>::iterator it = inputSeg.begin() + 1; it != inputSeg.end() - 1; ++it)
	{
		dot = ((it - 1)->x - it->x)*((it + 1)->x - it->x) + ((it - 1)->y - it->y)*((it + 1)->y - it->y) + ((it - 1)->z - it->z)*((it + 1)->z - it->z);
		sq1 = ((it - 1)->x - it->x)*((it - 1)->x - it->x) + ((it - 1)->y - it->y)*((it - 1)->y - it->y) + ((it - 1)->z - it->z)*((it - 1)->z - it->z);
		sq2 = ((it + 1)->x - it->x)*((it + 1)->x - it->x) + ((it + 1)->y - it->y)*((it + 1)->y - it->y) + ((it + 1)->z - it->z)*((it + 1)->z - it->z);
		if (isnan(acos(dot / sqrt(sq1*sq2)))) return;
		radAngle = acos(dot / sqrt(sq1*sq2));
		nodeHeadDot = (it->x - inputSeg.begin()->x)*((inputSeg.end() - 1)->x - inputSeg.begin()->x) +
			(it->y - inputSeg.begin()->y)*((inputSeg.end() - 1)->y - inputSeg.begin()->y) +
			(it->z - inputSeg.begin()->z)*((inputSeg.end() - 1)->z - inputSeg.begin()->z);
		nodeHeadSqr = (it->x - inputSeg.begin()->x)*(it->x - inputSeg.begin()->x) + (it->y - inputSeg.begin()->y)*(it->y - inputSeg.begin()->y) + (it->z - inputSeg.begin()->z)*(it->z - inputSeg.begin()->z);
		nodeHeadRadAngle = PI - acos(nodeHeadDot / sqrt(mainSqr*nodeHeadSqr));
		nodeToMainDist = sqrt(nodeHeadSqr) * sin(nodeHeadRadAngle);
		nodeToMainDistMean = nodeToMainDistMean + nodeToMainDist;
		cout << "       d(node-main):" << nodeToMainDist << " radian/pi:" << (radAngle / PI) << " turning cost:" << (sqrt(sq1) + sqrt(sq2)) / (radAngle / PI) << " " << it->x << " " << it->y << " " << it->z << endl;

		if ((radAngle / PI) < 0.6) // Detecting sharp turns and distance outliers => a) obviously errorneous depth situation
		{
			nodeInfo sharp;
			sharp.x = it->x; sharp.y = it->y; sharp.z = it->z;
			cout << "this node is picked" << endl;

			sharp.segID = it->seg_id;
			sharp.distToMain = nodeToMainDist;
			sharp.sqr1 = sq1; sharp.sqr2 = sq2; sharp.dot = dot;
			sharp.radAngle = radAngle;
			sharp.nodeAddress = it;
			sharp.turnCost = (sqrt(sq1) + sqrt(sq2)) / (radAngle / PI);

			pickedNode.push_back(sharp);
			turnCostMean = turnCostMean + sharp.turnCost;
		}
	}
	if (pickedNode.empty()) return;

	nodeToMainDistMean = nodeToMainDistMean / (inputSeg.size() - 2);
	turnCostMean = turnCostMean / pickedNode.size();
	cout << endl << endl << "  ==== start deleting nodes... " << endl;

	int delete_count = 0;
	ptrdiff_t order = 0;
	vector<V_NeuronSWC> preservedSegs;
	for (vector<nodeInfo>::iterator it = pickedNode.begin(); it != pickedNode.end(); ++it)
	{
		cout << "  Avg(d(node_main)):" << nodeToMainDistMean << " d(node-main):" << it->distToMain << " Avg(turning cost):" << turnCostMean << " turning cost:" << it->turnCost;
		cout << " [" << it->x << " " << it->y << " " << it->z << "] " << endl;
		if (it->distToMain >= nodeToMainDistMean || it->turnCost >= turnCostMean || it->distToMain >= segDist)
		{
			++delete_count;
			if (connectEdit == segmentEdit) curImgPtr->tracedNeuron.seg[it->segID].to_be_deleted = true;
			--(segInfoPtr->nodeCount);

			V_NeuronSWC preservedNode;
			preservedNode.row.push_back(*(it->nodeAddress - order));
			preservedNode.row[0].data[6] = -1;
			preservedSegs.push_back(preservedNode);
			cout << "delete [" << (it->nodeAddress - order)->x << " " << (it->nodeAddress - order)->y << " " << (it->nodeAddress - order)->z << "] " << delete_count << endl;
			inputSeg.erase(it->nodeAddress - order);
			++order;
			cout << "----" << endl;
		}
	}

	cout << endl << "  ==== cheking angles... " << endl;

	int deleteCount2;
	do
	{
		deleteCount2 = 0;
		for (vector<V_NeuronSWC_unit>::iterator it = inputSeg.begin() + 1; it != inputSeg.end() - 1; ++it)
		{
			double dot2 = (it->x - (it - 1)->x)*((it + 1)->x - it->x) + (it->y - (it - 1)->y)*((it + 1)->y - it->y) + (it->z - (it - 1)->z)*((it + 1)->z - it->z);
			double sq1_2 = ((it - 1)->x - it->x)*((it - 1)->x - it->x) + ((it - 1)->y - it->y)*((it - 1)->y - it->y) + ((it - 1)->z - it->z)*((it - 1)->z - it->z);
			double sq2_2 = ((it + 1)->x - it->x)*((it + 1)->x - it->x) + ((it + 1)->y - it->y)*((it + 1)->y - it->y) + ((it + 1)->z - it->z)*((it + 1)->z - it->z);
			if (isnan(acos(dot2 / sqrt(sq1_2*sq2_2)))) break;
			double radAngle_2 = acos(dot2 / sqrt(sq1_2*sq2_2));
			cout << "2nd rad Angle:" << radAngle_2 << " [" << it->x << " " << it->y << " " << it->z << "]" << endl;

			if ((radAngle_2 / PI) * 180 > 75)
			{
				if (sqrt(sq1_2) > (1 / 10)*sqrt(sq2_2))
				{
					--(segInfoPtr->nodeCount);
					++deleteCount2;
					cout << "delete " << " [" << it->x << " " << it->y << " " << it->z << "] " << deleteCount2 << endl;
					if ((inputSeg.size() - deleteCount2) <= 2)
					{
						--deleteCount2;
						++(segInfoPtr->nodeCount);
						break;
					}
					if (connectEdit == segmentEdit) curImgPtr->tracedNeuron.seg[it->seg_id].to_be_deleted = true;

					V_NeuronSWC preservedNode;
					preservedNode.row.push_back(*it);
					preservedNode.row[0].data[6] = -1;
					preservedSegs.push_back(preservedNode);
					inputSeg.erase(it);
				}
			}
		}
		cout << "deleted nodes: " << deleteCount2 << "\n=================" << endl;
	} while (deleteCount2 > 0);

	size_t label = 1;
	cout << "number of nodes after straightening process: " << inputSeg.size() << " ( segID = " << segInfoPtr->segID << " )" << endl;
	//cout << "seg num: " << curImgPtr->tracedNeuron.seg.size() << endl;
	for (vector<V_NeuronSWC_unit>::iterator it = inputSeg.begin(); it != inputSeg.end(); ++it)
	{
		it->seg_id = segInfoPtr->segID;
		it->data[0] = label;
		it->data[6] = label + 1;
		++label;
		cout << "[" << it->seg_id << ": " << it->x << " " << it->y << " " << it->z << "] ";
	}
	cout << endl;
	(inputSeg.end() - 1)->data[6] = -1;

	if (connectEdit == segmentEdit)
	{
		V_NeuronSWC newSeg;
		newSeg.row = inputSeg;
		curImgPtr->tracedNeuron.seg[segInfoPtr->segID] = newSeg;

		size_t singleNodeCount = 1;
		for (vector<V_NeuronSWC>::iterator nodeIt = preservedSegs.begin(); nodeIt != preservedSegs.end(); ++nodeIt)
		{
			nodeIt->row[0].seg_id = curImgPtr->tracedNeuron.seg.size() + singleNodeCount;
			++singleNodeCount;
			curImgPtr->tracedNeuron.seg.push_back(*nodeIt);
			//cout << "seg num: " << curImgPtr->tracedNeuron.seg.size() << endl;
		}
	}

    return;
}*/

void NeuronStructExplorer::falsePositiveList(NeuronTree* detectedTreePtr, NeuronTree* manualTreePtr, float distThreshold)
{
	// -- Determing false positives out of the detected structure by the threshold of specified distance to the manual structure.
	// -- if (the shortest distance of node i from detectedTree to manualTree > distThreshold) node i is deemed false positive.

	long int nodeCount = 1;
	for (QList<NeuronSWC>::iterator it = detectedTreePtr->listNeuron.begin(); it != detectedTreePtr->listNeuron.end(); ++it)
	{
		cout << "Scanning detected node " << nodeCount << ".. " << endl;
		++nodeCount;

		QList<NeuronSWC>::iterator checkIt = manualTreePtr->listNeuron.begin();
		while (checkIt != manualTreePtr->listNeuron.end())
		{
			float xSquare = (it->x - checkIt->x) * (it->x - checkIt->x);
			float ySquare = (it->y - checkIt->y) * (it->y - checkIt->y);
			float zSquare = (it->z - checkIt->z) * (it->z - checkIt->z);
			float thisNodeDist = sqrtf(xSquare + ySquare + zSquare);

			if (thisNodeDist < distThreshold)
			{
				this->processedTree.listNeuron.push_back(*it);
				break;
			}

			++checkIt;
			if (checkIt == manualTreePtr->listNeuron.end())
			{
				vector<float> FPcoords;
				FPcoords.push_back(it->x);
				FPcoords.push_back(it->y);
				FPcoords.push_back(it->z);
				this->FPsList.push_back(FPcoords);
				FPcoords.clear();
			}
		}
	}
}

void NeuronStructExplorer::falseNegativeList(NeuronTree* detectedTreePtr, NeuronTree* manualTreePtr, float distThreshold)
{
	// -- False negavies are selected out of the manual structure if none of detected nodes are close enough to them.
	// -- if (the shortest distance of node i from manualTree to detectedTree > distThreshold) node i is deemed false negative.

	long int nodeCount = 1;
	for (QList<NeuronSWC>::iterator it = manualTreePtr->listNeuron.begin(); it != manualTreePtr->listNeuron.end(); ++it)
	{
		cout << "Scanning detected node " << nodeCount << ".. " << endl;
		++nodeCount;

		QList<NeuronSWC>::iterator checkIt = detectedTreePtr->listNeuron.begin();
		while (checkIt != detectedTreePtr->listNeuron.end())
		{
			float xSquare = (it->x - checkIt->x) * (it->x - checkIt->x);
			float ySquare = (it->y - checkIt->y) * (it->y - checkIt->y);
			float zSquare = (it->z - checkIt->z) * (it->z - checkIt->z);
			float thisNodeDist = sqrtf(xSquare + ySquare + zSquare);

			if (thisNodeDist > distThreshold)
			{
				vector<float> FNcoords;
				FNcoords.push_back(it->x);
				FNcoords.push_back(it->y);
				FNcoords.push_back(it->z);
				this->FNsList.push_back(FNcoords);
				FNcoords.clear();

				break;
			}

			++checkIt;
		}
	}
}

void NeuronStructExplorer::detectedDist(NeuronTree* inputTreePtr1, NeuronTree* inputTreePtr2)
{
	// -- Compute the shortest distance of each node from input structure (inputTreePtr1) to refernce structure (inputTreePtr2)
	// -- The order of inputTreePtr1 and inputTreePtr2 represent the distances from opposite comparing directions.

	vector<float> minDists;
	long int count = 1;
	for (QList<NeuronSWC>::iterator it1 = inputTreePtr1->listNeuron.begin(); it1 != inputTreePtr1->listNeuron.end(); ++it1)
	{
		++count;
		//cout << "node No. " << count << ".." << endl;
		float minDist = 10000;
		for (QList<NeuronSWC>::iterator it2 = inputTreePtr2->listNeuron.begin(); it2 != inputTreePtr2->listNeuron.end(); ++it2)
		{
			float xSquare = ((it2->x) - (it1->x)) * ((it2->x) - (it1->x));
			float ySquare = ((it2->y) - (it1->y)) * ((it2->y) - (it1->y));
			float zSquare = ((it2->z) - (it1->z)) * ((it2->z) - (it1->z));
			float currDist = sqrtf(xSquare + ySquare + zSquare);

			if (currDist <= minDist) minDist = currDist;
		}
		minDists.push_back(minDist);
	}

	float distSum = 0;
	for (vector<float>::iterator it = minDists.begin(); it != minDists.end(); ++it) distSum = distSum + *it;
	float distMean = distSum / minDists.size();
	float varSum = 0;
	for (int i = 0; i < minDists.size(); ++i) varSum = varSum + (minDists[i] - distMean) * (minDists[i] - distMean);
	float distVar = varSum / minDists.size();
	float distStd = sqrtf(distVar);

	cout << distMean << " " << distStd << endl;
}

void NeuronStructExplorer::shortestDistCDF(NeuronTree* inputTreePtr1, NeuronTree* inputTreePtr2, int upperBound, int binNum)
{
	// -- Compute the PDF and CDF of shortest distance of detected nodes to manual nodes.

	float interval = float(upperBound) / float(binNum);
	vector<float> shortestDists;
	for (QList<NeuronSWC>::iterator it1 = inputTreePtr1->listNeuron.begin(); it1 != inputTreePtr1->listNeuron.end(); ++it1)
	{
		float minDist = 10000;
		for (QList<NeuronSWC>::iterator it2 = inputTreePtr2->listNeuron.begin(); it2 != inputTreePtr2->listNeuron.end(); ++it2)
		{
			float xSquare = ((it2->x) - (it1->x)) * ((it2->x) - (it1->x));
			float ySquare = ((it2->y) - (it1->y)) * ((it2->y) - (it1->y));
			float zSquare = ((it2->z) - (it1->z)) * ((it2->z) - (it1->z));
			float currDist = sqrtf(xSquare + ySquare + zSquare);

			if (currDist <= minDist) minDist = currDist;
		}
		shortestDists.push_back(minDist);
	}

	for (int i = 0; i < binNum; ++i)
	{
		this->nodeDistPDF[i] = 0;
		this->nodeDistPDF[i] = 0;
	}
	for (vector<float>::iterator distIt = shortestDists.begin(); distIt != shortestDists.end(); ++distIt)
	{
		int binNo = (*distIt) / interval;
		++(this->nodeDistPDF[binNo]);
	}
	this->nodeDistCDF[0] = this->nodeDistPDF[0];
	for (int i = 1; i < binNum; ++i) this->nodeDistCDF[i] = this->nodeDistCDF[i - 1] + nodeDistPDF[i];
}

void NeuronStructExplorer::segmentDecompose(NeuronTree* inputTreePtr)
{
	// -- This function used get_link_map and decompose in v_neuronswc.cpp to get segment hierarchical information.

	this->segmentList.clear();
	this->segmentList = NeuronTree__2__V_NeuronSWC_list(*inputTreePtr);
}


/////////////////////////////////////////////////////////////////
//
//          Copyright Vadim Stadnik 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)
//
/////////////////////////////////////////////////////////////////
//
//  See folder "../doc" for documentation
//
/////////////////////////////////////////////////////////////////

#ifndef _TIMER_CHRONO_HPP
#define _TIMER_CHRONO_HPP

#include <sstream>
#include <string>

#define BOOST_SYSTEM_DYN_LINK
#define BOOST_CHRONO_HEADER_ONLY
//  GCC specific
#define BOOST_SYSTEM_NO_DEPRECATED
#include <boost/chrono/chrono.hpp>

namespace test_performance
{

    class TimerChrono
    {
        typedef boost::chrono::high_resolution_clock    _clock_impl ;
        typedef _clock_impl::time_point                 _time_point ;
        typedef boost::ratio< 1 , 1000000 >             _micro      ;
        typedef boost::chrono::duration< double , _micro >
                                                        _duration   ;

    public:

        TimerChrono ( ) : start( ) , stop( ) {  }

        void Start ( ) { start = _clock_impl::now ( ) ; }
        void Stop  ( ) { stop  = _clock_impl::now ( ) ; }

        double DurationMicroSec ( ) const
        {
            _duration   durn_mcsec = stop - start ;
            double      time = durn_mcsec . count ( ) ;
            return time ;
        }

        void TimeToString ( std::string const &  info    ,
                            std::string &        str_res ) const
        {
            double              time = DurationMicroSec ( ) ;
            std::ostringstream  ostr_out ;

            ostr_out << " T = " << time << " mcs ; " << info << std::endl ;
            str_res  = ostr_out . str ( ) ;
        }

    private:

        _time_point     start ;
        _time_point     stop  ;
    } ;

}   //  namespace test_performance

#endif  // _TIMER_CHRONO_HPP