// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

//
#include <vw/Core/Exception.h>
#include <vw/Core/Log.h>
#include <vw/Core/TypeDeduction.h>
#include <vw/Math/Matrix.h>
#include <vw/Math/Quaternion.h>
#include <vw/Math/Vector.h>
#include <vw/Camera/CAHVModel.h>
#include <vw/Camera/PinholeModel.h>

#include <boost/algorithm/string/predicate.hpp>

namespace vw {
namespace camera {

  /// This constructor takes a filename and reads in a camera model
  /// from the file.  The file may contain either CAHV parameters or
  /// pinhole camera parameters.
  CAHVModel::CAHVModel(std::string const& filename) {
    if (filename.empty())
      vw_throw( IOErr() << "CAHVModel: null file name passed to constructor." );

    if (boost::ends_with(filename, ".cahv"))
      read_cahv(filename);
    else if (boost::ends_with(filename, ".pin"))
      read_pinhole(filename);
    else
      vw_throw( IOErr() << "CAHVModel: Unknown camera file suffix." );
  }

  /// Initialize the CAHV vectors directly in the native CAHV format.
  CAHVModel::CAHVModel(Vector3 const& C_vec,
                       Vector3 const& A_vec,
                       Vector3 const& H_vec,
                       Vector3 const& V_vec) :
    C(C_vec), A(A_vec), H(H_vec), V(V_vec) {}

  /// Initialize a CAHV model from a pinhole model
  CAHVModel::CAHVModel(PinholeModel const& pin_model) {
    //  Intrinsic parametes (in pixel units)
    Vector2 focal = pin_model.focal_length();
    Vector2 offset = pin_model.point_offset();

    Vector3 u,v,w;
    pin_model.coordinate_frame(u,v,w);

    Matrix<double,3,3> R = pin_model.camera_pose().rotation_matrix();

    Vector3 Hvec = R*u;
    Vector3 Vvec = R*v;

    C = pin_model.camera_center();
    A = R*w;
    H = focal[0]*Hvec + offset[0]*A;
    V = focal[1]*Vvec + offset[1]*A;
  }

  std::string
  CAHVModel::type() const { return "CAHV"; }

  // FIXME -- Double check anything related to PinholeModel
  CAHVModel CAHVModel::operator= (PinholeModel const& pin_model) {

    //  Pinhole model parameters (in pixel units)
    double fH, fV, Hc, Vc;
    pin_model.intrinsic_parameters(fH, fV, Hc, Vc);

    //  Unit vectors defining camera coordinate frame
    Vector3 u,v,w;
    pin_model.coordinate_frame(u,v,w);

    //  The true rotation between world and camera coordinate
    //  frames includes the rotation R --AND-- a rotation from
    //  specifying the directions of increasing u,v,w pixels
    Matrix<double,3,3> R = pin_model.camera_pose().rotation_matrix();

    //  Now create the components of the CAHV model...
    Vector3 Hvec = R*u;
    Vector3 Vvec = R*v;

    C = pin_model.camera_center();
    A = R*w;
    H = fH*Hvec + Hc*A;
    V = fV*Vvec + Vc*A;

    return *this;
  }

  CAHVModel::CAHVModel(double f, Vector2 const& pixel_size,
                       double xmin, double /*xmax*/, double ymin, double /*ymax*/,
                       Matrix<double, 4, 4> const& view_matrix) {

    double fH = f/pixel_size.x();
    double fV = f/pixel_size.y();
    double Hc = -xmin;
    double Vc = -ymin;

    Vector3 Hvec(view_matrix[0][0], view_matrix[0][1], view_matrix[0][2]);
    Vector3 Vvec(view_matrix[1][0], view_matrix[1][1], view_matrix[1][2]);

    C = Vector3(view_matrix[3][0], view_matrix[3][1], view_matrix[3][2]);
    A = Vector3(view_matrix[2][0], view_matrix[2][1], view_matrix[2][2]);
    H = fH*Hvec + Hc*A;
    V = fV*Vvec + Vc*A;
  }

  CAHVModel::CAHVModel(double f, Vector2 const& pixel_size, double Hc, double Vc,
                       Vector3 const& Cinit, Vector3 const& Ainit,
                       Vector3 const& Hvec, Vector3 const& Vvec) {
    double fH = f/pixel_size.x();
    double fV = f/pixel_size.y();
    C = Cinit;
    A = Ainit;
    H = fH*Hvec + Hc*A;
    V = fV*Vvec + Vc*A;
  }

  Vector2 CAHVModel::point_to_pixel(Vector3 const& point) const {
    double dDot = dot_prod(point-C, A);
    return Vector2( dot_prod(point-C, H) / dDot,
                    dot_prod(point-C, V) / dDot );
  }

  Vector3 CAHVModel::pixel_to_vector(Vector2 const& pix) const {

    // Find vector
    Vector3 vec =
      normalize(cross_prod(V - pix.y() * A,
                           H - pix.x() * A));

    // The vector VxH should be pointing in the same directions as A,
    // if it isn't (because we have a left handed system), flip the
    // vector.
    if (dot_prod(cross_prod(V, H), A) < 0.0)
      vec *= -1.0;
    return vec;
  }

  Vector3 CAHVModel::camera_center(Vector2 const& pix) const {
    return C;
  };

  // --------------------------------------------------
  //                 Private Methods
  // --------------------------------------------------
  void CAHVModel::read_cahv(std::string const& filename) {

    try {
      std::ifstream input(filename.c_str(), std::ifstream::in);
      input.exceptions(std::ifstream::failbit | std::ifstream::badbit);

      vw_out(InfoMessage, "camera") << "Reading CAHV file: "
                                    << filename << ".\n";

      char r1, r2;

      while (true) {
        input.ignore(1024, 'C');
        input >> r1;
        if (r1 == '=')
          break;
      }
      input >> C[0] >> C[1] >> C[2];

      input >> r1 >> r2;
      if (r1 != 'A' || r2 != '=')
        vw_throw( IOErr() << "CAHVModel: Could not read A vector\n" );
      input >> A(0) >> A(1) >> A(2);

      input >> r1 >> r2;
      if (r1 != 'H' || r2 != '=')
        vw_throw( IOErr() << "CAHVModel: Could not read H vector\n" );
      input >> H(0) >> H(1) >> H(2);

      input >> r1 >> r2;
      if (r1 != 'V' || r2 != '=')
        vw_throw( IOErr() << "CAHVModel: Could not read V vector\n" );
      input >> V(0) >> V(1) >> V(2);

    } catch ( const std::ifstream::failure& e ) {
      vw_throw( IOErr() << "CAHVModel: Could not read file: " << filename << " (" << e.what() << ")" );
    }
  }

  void CAHVModel::read_pinhole(std::string const& filename) {
    FILE *camFP = fopen(filename.c_str(), "r");

    if (camFP == 0)
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not open file\n" );

    char line[2048];
    double f, fH, fV, Hc, Vc;
    Vector2 pixelSize;
    Vector3 Hvec, Vvec;

    // Read intrinsic parameters
    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"f = %lf", &f) != 1) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read focal length\n" );
      fclose(camFP);
    }

    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"SP = %lf %lf", &pixelSize.x(), &pixelSize.y()) != 2) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read pixel size\n" );
      fclose(camFP);
    }

    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"IC = %lf %lf", &Hc, &Vc) != 2) {
      vw_throw( IOErr() << "CAHVModel::ReadPinhole: Could not read image center pos\n" );
      fclose(camFP);
    }

    // Read extrinsic parameters
    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"C = %lf %lf %lf", &C(0), &C(1), &C(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read C vector\n" );
      fclose(camFP);
    }

    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"A = %lf %lf %lf", &A(0), &A(1), &A(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read A vector\n" );
      fclose(camFP);
    }

    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"Hv = %lf %lf %lf", &Hvec(0), &Hvec(1), &Hvec(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read Hvec\n" );
      fclose(camFP);
    }

    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"Vv = %lf %lf %lf", &Vvec(0), &Vvec(1), &Vvec(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read Vvec\n" );
      fclose(camFP);
    }

    // In the future, we should also read in a view matrix -- LJE
    //     double dummy
    //     if (sscanf(line, "VM = %lf %lf %lf %f %lf %lf %lf %f "
    //        "%lf %lf %lf %f %lf %lf %lf %f ",
    //        &Hvec(0), &Hvec(1), &Hvec(2), &dummy,
    //        &Vvec(0), &Vvec(1), &Vvec(2), &dummy,
    //        &A(0), &A(1), &A(2), &dummy,
    //        &C(0), &C(1), &C(2), &dummy) != 16)
    //     {
    //       vw_throw( IOErr()
    //  << "CAHVModel::ReadPinhole: Could not read view matrix\n" );
    //       fclose(camFP);
    //     }

    fH = f/pixelSize.x();
    fV = f/pixelSize.y();

    H = fH*Hvec + Hc*A;
    V = fV*Vvec + Vc*A;

    fclose(camFP);
  }

  void epipolar(CAHVModel const src_camera0, CAHVModel const src_camera1,
                CAHVModel &dst_camera0, CAHVModel &dst_camera1) {

    // Compute a common image center and scale for the two models
    double hc = dot_prod(src_camera0.H, src_camera0.A) / 2.0 +
      dot_prod(src_camera1.H, src_camera1.A) / 2.0;
    double vc = dot_prod(src_camera0.V, src_camera0.A) / 2.0 +
      dot_prod(src_camera1.V, src_camera1.A) / 2.0;

    double hs = norm_2(cross_prod(src_camera0.A, src_camera0.H))/2.0 +
      norm_2(cross_prod(src_camera1.A, src_camera1.H))/2.0;

    double vs = norm_2(cross_prod(src_camera0.A, src_camera0.V))/2.0 +
      norm_2(cross_prod(src_camera1.A, src_camera1.V))/2.0;

    // Use common center and scale to construct common A, H, V
    Vector3 app  = src_camera0.A + src_camera1.A;

    // Note the directionality of 1 to 0, for consistency later
    Vector3 f = cross_prod(cross_prod(app, src_camera1.C - src_camera0.C), app);

    Vector3 hp;
    if (dot_prod(f, src_camera0.H) > 0)
      hp = f * hs / (norm_2(f));
    else
      hp = -f * hs / (norm_2(f));

    app *= 0.5;
    Vector3 g = hp * dot_prod(app,hp) / (hs * hs);
    Vector3 a = normalize(app - g);
    Vector3 vp = cross_prod(a, hp) * vs / hs;

    dst_camera0.C = src_camera0.C;
    dst_camera1.C = src_camera1.C;

    dst_camera0.A = dst_camera1.A = a;
    dst_camera0.H = dst_camera1.H = hp + hc * a;
    dst_camera0.V = dst_camera1.V = vp + vc * a;
  }

  void CAHVModel::write(std::string const& filename ) {
    try {
      std::ofstream output(filename.c_str(), std::ofstream::out);
      output.exceptions(std::ofstream::failbit | std::ofstream::badbit);
      output.precision(20);

      vw_out(InfoMessage, "camera") << "Writing CAHV file: " << filename << "\n";

      output << "C = " << C[0] << " " << C[1] << " " << C[2] << "\n"
             << "A = " << A[0] << " " << A[1] << " " << A[2] << "\n"
             << "H = " << H[0] << " " << H[1] << " " << H[2] << "\n"
             << "V = " << V[0] << " " << V[1] << " " << V[2] << "\n";
    } catch ( const std::ofstream::failure& e ) {
      vw_throw( IOErr() << "CAHVModel: Could not write file: " << filename << "(" << e.what() << ")" );
    }
  }

  std::ostream& operator<<(std::ostream& str, CAHVModel const& model) {
    str << "CAHV camera: \n";
    str << "\tC: " << model.C << "\n";
    str << "\tA: " << model.A << "\n";
    str << "\tH: " << model.H << "\n";
    str << "\tV: " << model.V << "\n";
    return str;
  }

}} // namespace vw::camera


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <vw/Core/Log.h>
#include <vw/Camera/CAHVOREModel.h>
#include <fstream>
#include <boost/foreach.hpp>

using namespace vw;
using namespace vw::camera;

bool CAHVOREModel::check_line( std::istream& istream, char letter ) {
  char r1,r2;
  istream >> r1 >> r2;
  if ( r1 != letter || r2 != '=' )
    return false;
  return true;
}

// Overloaded constructor - this one reads in the file name
// where the CAHVORE camera model is saved.
CAHVOREModel::CAHVOREModel(std::string const& filename) {

  try {
    std::ifstream input(filename.c_str(), std::ifstream::in);
    input.exceptions(std::ifstream::failbit | std::ifstream::badbit);

    vw_out(InfoMessage, "camera") << "Reading CAHVORE file: "
                                  << filename << ".\n";

    char r1;

    while (true) {
      input.ignore(1024, 'C');
      input >> r1;
      if (r1 == '=')
        break;
    }
    input >> C(0) >> C(1) >> C(2);

    if ( !check_line( input, 'A') )
      vw_throw( IOErr() << "CAHVOREModel: Could not read A vector\n" );
    input >> A(0) >> A(1) >> A(2);

    if ( !check_line( input, 'H') )
      vw_throw( IOErr() << "CAHVOREModel: Could not read H vector\n" );
    input >> H(0) >> H(1) >> H(2);

    if ( !check_line( input, 'V') )
      vw_throw( IOErr() << "CAHVOREModel: Could not read V vector\n" );
    input >> V(0) >> V(1) >> V(2);

    if ( !check_line( input, 'O') )
      vw_throw( IOErr() << "CAHVOREModel: Could not read O vector\n" );
    input >> O(0) >> O(1) >> O(2);

    if ( !check_line( input, 'R') )
      vw_throw( IOErr() << "CAHVOREModel: Could not read R vector\n" );
    input >> R(0) >> R(1) >> R(2);

    if ( !check_line( input, 'E') )
      vw_throw( IOErr() << "CAHVOREModel: could not read R vector\n" );
    input >> E(0) >> E(1) >> E(2);

    if ( !check_line( input, 'T') )
      vw_throw( IOErr() << "CAHVOREModel: could not read T element\n" );
    int T;
    input >> T;

    if ( !check_line( input, 'P') )
      vw_throw( IOErr() << "CAHVOREModel: could not read P element\n" );
    input >> P;

    switch( T ) {
    case 1: P = 1.0; break;
    case 2: P = 0.0; break;
    case 3:
      if ( P < 0 || P > 1 ) vw_throw( ArgumentErr() << "Invalid P value: "
                                      << P << "\n" );
      break;
    default: vw_throw( ArgumentErr() << "Unknown CAHVORE type: " << T << "\n" );
    }

  } catch (const std::ifstream::failure& e) {
    vw_throw( IOErr() << "CAHVOREModel: Could not read file: " << filename << " (" << e.what() << ")" );
  }
}

// Write CAHVOR model to file.
void vw::camera::CAHVOREModel::write(std::string const& filename) {
  try {
    std::ofstream output(filename.c_str(), std::ofstream::out);
    output.exceptions(std::ofstream::failbit | std::ofstream::badbit);
    output.precision(20);

    vw_out(InfoMessage, "camera") << "Writing CAHVORE file: " << filename << "\n";

    output << "C = " << C(0) << " " << C(1) << " " << C(2) << "\n"
           << "A = " << A(0) << " " << A(1) << " " << A(2) << "\n"
           << "H = " << H(0) << " " << H(1) << " " << H(2) << "\n"
           << "V = " << V(0) << " " << V(1) << " " << V(2) << "\n"
           << "O = " << O(0) << " " << O(1) << " " << O(2) << "\n"
           << "R = " << R(0) << " " << R(1) << " " << R(2) << "\n"
           << "E = " << E(0) << " " << E(1) << " " << E(2) << "\n";
    if ( P == 0 ) {
      output << "T = 2\n";
    } else if ( P == 1 ) {
      output << "T = 1\n";
    } else {
      output << "T = 3\n";
    }
    output << "P = " << P << "\n";
  } catch ( const std::ofstream::failure& e ) {
    vw_throw( IOErr() << "CAHVOREModel: Could not write file: " << filename << " (" << e.what() << ")" );
  }
}

vw::Vector3 CAHVOREModel::pixel_to_vector(vw::Vector2 const& pix) const {
  // Based on JPL's cmod_cahvore_2d_to_3d
  Vector3 result;

  // Calculate initial terms
  Vector3 w3 = cross_prod(V - pix[1]*A,
                          H - pix[0]*A);
  Vector3 rp = (1/dot_prod(A,cross_prod(V,H))) * w3;

  double zetap = dot_prod(rp,O);
  Vector3 lambdap3 = rp - zetap*O;
  double lambdap = norm_2(lambdap3);
  double chip = lambdap / zetap;

  if (chip < 1e-8) {
    // Approximations for small angles
    result = O;
  } else {
    // Full calculations

    // Calculate chi using Newton's Method
    double chi = chip;
    double dchi = 1;
    for (int32 n = 0;; ++n) {
      // Checking exit conditions
      if (n > 100)
        vw_throw( PixelToRayErr() << "CAHVOREModel: Did not converge.\n" );
      if (fabs(dchi) < 1e-8)
        break;

      // Compute terms from the current value of chi
      double chi2 = chi * chi;
      double chi3 = chi * chi2;
      double chi4 = chi * chi3;
      double chi5 = chi * chi4;

      // Update chi
      double deriv = (1 + R[0]) + 3*R[1]*chi2 + 5*R[2]*chi4;
      dchi = ((1 + R[0])*chi + R[1]*chi3 + R[2]*chi5 - chip) / deriv;
      chi -= dchi;
    }

    // Compute the incoming ray's angle
    double linchi, theta;
    linchi = P * chi;
    if (P < -1e-15)
      theta = asin(linchi) / P;
    else if (P > 1e-15)
      theta = atan(linchi) / P;
    else
      theta = chi;

    double theta2 = theta*theta;
    double mu = E[2];
    mu = E[1] + theta2*mu;
    mu = E[0] + theta2*mu;

    result = sin(theta)*normalize(lambdap3) + cos(theta)*O;
  }

  return result;
}

Vector2 CAHVOREModel::point_to_pixel(vw::Vector3 const& point) const {
  // Base on JPL's cmod_cahvore_3d_to_2d_general

  // Calculate initial terms
  Vector3 p_c = point - C;
  double zeta = dot_prod(p_c, O);
  Vector3 lambda3 = p_c - zeta * O;
  double lambda = norm_2(lambda3);

  // Calculate theta using Newton's Method
  double theta = atan2(lambda, zeta);
  double dtheta = 1;
  for (int32 n = 0;;++n) {

    // Checking exit conditions
    if (n > 100)
      vw_throw( PointToPixelErr() << "CAHVOREModel: Did not converge.\n" );
    if (fabs(dtheta) < 1e-8)
      break;

    // Compute terms from the current value of theta
    double costh = cos(theta);
    double sinth = sin(theta);
    double theta2 = theta * theta;
    double theta3 = theta * theta2;
    double theta4 = theta * theta3;
    double upsilon = zeta*costh + lambda*sinth
      - (1     - costh) * (E[0] +  E[1]*theta2 +   E[2]*theta4)
      - (theta - sinth) * (      2*E[1]*theta  + 4*E[2]*theta3);

    // Update theta
    dtheta = (
              zeta*sinth - lambda*costh
              - (theta - sinth) * (E[0] + E[1]*theta2 + E[2]*theta4)
              ) / upsilon;
    theta -= dtheta;
  }

  // Check the value of theta
  if ((theta * fabs(P)) > M_PI/2)
    vw_throw( PointToPixelErr() << "CAHVOREModel: Theta out of bounds.\n" );

  // Approximations for small theta
  Vector3 rp;
  if (theta < 1e-8) {
    rp = p_c;
  } else {
    // Full calculations
    double linth, chi;

    linth = P * theta;
    if (P < -1e-15)
      chi = sin(linth) / P;
    else if (P > 1e-15)
      chi = tan(linth) / P;
    else
      chi = theta;

    double chi2 = chi*chi;
    double mu = R[2];
    mu = R[1] + chi2 * mu;
    mu = R[0] + chi2 * mu;
    rp = (lambda / chi) * O + (1+mu)*lambda3;
  }

  // Calculate the projection
  double alpha  = dot_prod(rp, A);
  return Vector2( dot_prod(rp,H) / alpha,
                  dot_prod(rp,V) / alpha );
}

CAHVModel camera::linearize_camera( CAHVOREModel const& camera_model,
                                    Vector2i const& cahvore_image_size,
                                    Vector2i const& cahv_image_size ) {
  // Limit to field of view
  const static double limfov = M_PI * 3/4; // 135 degrees field of view.
  const bool minfov          = true;       // Yes, minimize the common field of view

  CAHVModel cahv_model;
  cahv_model.C = camera_model.C;

  // Record the landmark 2D coordinates around the perimeter of the image
  Vector2 hpts[6], vpts[6];
  hpts[0] = Vector2();
  hpts[1] = Vector2(0,(cahvore_image_size[1]-1)/2);
  hpts[2] = Vector2(0,cahvore_image_size[1]-1);
  hpts[3] = Vector2(cahvore_image_size[0]-1,0);
  hpts[4] = Vector2(cahvore_image_size[0]-1,(cahvore_image_size[1]-1)/2);
  hpts[5] = cahvore_image_size - Vector2(1,1);
  vpts[0] = Vector2();
  vpts[1] = Vector2((cahvore_image_size[0]-1)/2,0);
  vpts[2] = Vector2(cahvore_image_size[0]-1,0);
  vpts[3] = Vector2(0,cahvore_image_size[1]-1);
  vpts[4] = Vector2((cahvore_image_size[0]-1)/2,cahvore_image_size[1]-1);
  vpts[5] = cahvore_image_size - Vector2(1,1);

  // Choose a camera axis in the middle of the image
  Vector2 p2 = (cahvore_image_size-Vector2i(1,1))/2.0;
  cahv_model.A = camera_model.pixel_to_vector( p2 );

  // Compute the original right and down vectors
  Vector3 dn = cross_prod(camera_model.A, camera_model.H);
  Vector3 rt = normalize(cross_prod(dn,   camera_model.A));
  dn = normalize( dn );

  // Adjust the right and down vectors to be orthogonal to new axis
  rt = cross_prod(dn, cahv_model.A);
  dn = normalize(cross_prod(cahv_model.A, rt));
  rt = normalize( rt );

  // Find horizontal and vertical fields of view
  double hmin = 1, hmax = -1;
  BOOST_FOREACH( Vector2 const& loop, hpts ) {
    const Vector3 u3 = camera_model.pixel_to_vector(loop);
    double cs = dot_prod(cahv_model.A, normalize(u3 - dot_prod(dn, u3) * dn));
    if (hmin > cs) hmin = cs;
    if (hmax < cs) hmax = cs;
  }
  double vmin = 1, vmax = -1;
  BOOST_FOREACH( Vector2 const& loop, vpts ) {
    const Vector3 u3 = camera_model.pixel_to_vector(loop);
    double cs = dot_prod(cahv_model.A,normalize(u3 - dot_prod(rt, u3)*rt));
    if (vmin > cs) vmin = cs;
    if (vmax < cs) vmax = cs;
  }

  // Compute the all-encompassing scale factors
  Vector2 cosines;
  if ( minfov ) {
    // use max
    cosines = Vector2(hmax,vmax);
  } else {
    // use min
    cosines = Vector2(hmin,vmin);
  }
  if ( acos(cosines[0]) > limfov ) cosines[0] = cos(limfov);
  if ( acos(cosines[1]) > limfov ) cosines[1] = cos(limfov);
  Vector2 scalars =
    elem_quot(elem_prod(cahv_image_size/2.0,cosines),
              sqrt(Vector2(1,1) - elem_prod(cosines,cosines)));

  // Assign idealized image centers and coordinate angles
  Vector2 centers = (cahv_image_size - Vector2(1,1))/2.0;

  // Construct H and V
  cahv_model.H = scalars[0] * rt + centers[0] * cahv_model.A;
  cahv_model.V = scalars[1] * dn + centers[1] * cahv_model.A;

  return cahv_model;
}


// Copyright (c) 2020 fortiss GmbH
//
// Authors: Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef BARK_GEOMETRY_LINE_HPP_
#define BARK_GEOMETRY_LINE_HPP_

#include <algorithm>
#include <cmath>
#include <limits>
#include <tuple>
#include <utility>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/tuple/tuple.hpp>

#include "bark/geometry/angle.hpp"
#include "bark/geometry/commons.hpp"

#include "src/spline.h"

namespace bark {
namespace geometry {

//! templated line class with a boost polygon as a member function
template <typename T>
class Line_t : public Shape<bg::model::linestring<T>, T> {
 public:
  Line_t()
      : Shape<bg::model::linestring<T>, T>(Pose(0, 0, 0), std::vector<T>(), 0) {
  }

  explicit Line_t(const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& points);

  virtual Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> ToArray() const;

  virtual std::shared_ptr<Shape<bg::model::linestring<T>, T>> Clone() const;

  //! TODO(@all): do not recompute full s but only add one point
  bool AddPoint(const T& p) {
    return Shape<bg::model::linestring<T>, T>::AddPoint(p) && RecomputeS();
  }

  bool AddPoints(const std::vector<T>& pts) {
    for (const auto& p : pts) {
      Shape<bg::model::linestring<T>, T>::AddPoint(p);
    }
    return RecomputeS();
  }

  auto Length() const {
    return bg::length(Shape<bg::model::linestring<T>, T>::obj_);
  }

  unsigned int size() const {
    return Shape<bg::model::linestring<T>, T>::obj_.size();
  }

  void AppendLinestring(const Line_t& ls) {
    bg::append(Shape<bg::model::linestring<T>, T>::obj_, ls.obj_);
    RecomputeS();
  }

  std::vector<T> GetPointsInSInterval(double begin, double end) const {
    std::vector<T> points;
    uint begin_idx = std::upper_bound(s_.begin(), s_.end(), begin) - s_.begin();
    uint end_idx = std::lower_bound(s_.begin(), s_.end(), end) - s_.begin();
    std::copy(Shape<bg::model::linestring<T>, T>::obj_.begin() + begin_idx,
              Shape<bg::model::linestring<T>, T>::obj_.begin() + end_idx,
              std::back_inserter(points));
    return points;
  }

  virtual bool Valid() const {
    return Shape<bg::model::linestring<T>, T>::Valid() && s_.size() == size();
  }

  void Reverse() {
    boost::geometry::reverse(Shape<bg::model::linestring<T>, T>::obj_);
  }

  typedef typename std::vector<T>::iterator point_iterator;
  typedef typename std::vector<T>::const_iterator const_point_iterator;
  point_iterator begin() {
    return Shape<bg::model::linestring<T>, T>::obj_.begin();
  }
  const_point_iterator begin() const {
    return Shape<bg::model::linestring<T>, T>::obj_.begin();
  }
  point_iterator end() {
    return Shape<bg::model::linestring<T>, T>::obj_.end();
  }
  const_point_iterator end() const {
    return Shape<bg::model::linestring<T>, T>::obj_.end();
  }

  typedef typename std::vector<T>::reverse_iterator reverse_point_iterator;
  typedef typename std::vector<T>::const_reverse_iterator
      const_reverse_point_iterator;  // NOLINT

  reverse_point_iterator rbegin() {
    return Shape<bg::model::linestring<T>, T>::obj_.rbegin();
  }
  const_reverse_point_iterator rbegin() const {
    return Shape<bg::model::linestring<T>, T>::obj_.rbegin();
  }
  reverse_point_iterator rend() {
    return Shape<bg::model::linestring<T>, T>::obj_.rend();
  }
  const_reverse_point_iterator rend() const {
    return Shape<bg::model::linestring<T>, T>::obj_.rend();
  }

  //! local coordinates 0..[total distance] along the lines
  std::vector<double> s_;

  //! @todo free function, s_ private?
  bool RecomputeS() {
    s_.clear();
    // edge case no points
    if (Shape<bg::model::linestring<T>, T>::obj_.empty()) {
      return true;
    } else if (Shape<bg::model::linestring<T>, T>::obj_.size() == 1) {
      // edge case one point
      s_.push_back(0.0);
      return true;
    } else {  // nominal case
      // compute distance from last point to next point and
      // store these on vector s_ (avoid additional tmp vector)
      T last_pt = Shape<bg::model::linestring<T>, T>::obj_.front();
      s_.reserve(Shape<bg::model::linestring<T>, T>::obj_.size());
      double distance_until_now = 0.0;
      for (const T& next_pt : Shape<bg::model::linestring<T>, T>::obj_) {
        distance_until_now += bg::distance(next_pt, last_pt);
        s_.push_back(distance_until_now);
        last_pt = next_pt;
      }
      return true;
    }
  }

  void RemoveDuplicates() {
    bg::unique(Shape<bg::model::linestring<T>, T>::obj_);
    RecomputeS();
  }
  bool operator==(const Line_t& rhs) const {
    return bg::equals(this->obj_, rhs.obj_);
  }
  bool operator!=(const Line_t& rhs) const { return !(rhs == *this); }
};

//! for better usage simple double defines
using LinePoint = Point2d;
using Line = Line_t<LinePoint>;

template <typename T>
inline Line_t<T>::Line_t(
    const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& points)
    : Shape<bg::model::linestring<T>, T>(points, 0) {
  RecomputeS();
}

template <>
inline Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Line::ToArray()
    const {
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat(obj_.size(), 2);
  for (uint32_t i = 0; i < obj_.size(); i++) {
    mat.row(i) << bg::get<0>(obj_[i]), bg::get<1>(obj_[i]);
  }
  return mat;
}

template <typename T>
inline std::shared_ptr<Shape<bg::model::linestring<T>, T>> Line_t<T>::Clone()
    const {
  std::shared_ptr<Line_t<T>> new_line = std::make_shared<Line_t<T>>(*this);
  return new_line;
}

template <typename T>
inline Line Reverse(const T& l) {
  T lr = l;
  lr.Reverse();
  return lr;
}

inline double Distance(const Line& line, const Point2d& p) {
  return bg::distance(line.obj_, p);
}

inline double Distance(const Line& line, const Line& line2) {
  return bg::distance(line.obj_, line2.obj_);
}

template <typename T>
inline T Length(const Line& line) {
  return bg::length<T>(line.obj_);
}

inline Line Rotate(const Line& line, double hdg) {
  using boost::geometry::strategy::transform::rotate_transformer;
  rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(hdg);
  Line line_rotated;
  boost::geometry::transform(line.obj_, line_rotated.obj_, rotate);
  line_rotated.RecomputeS();
  return line_rotated;
}

inline Line Translate(const Line& line, double x, double y) {
  using boost::geometry::strategy::transform::translate_transformer;
  translate_transformer<double, 2, 2> translate(x, y);
  Line line_translated;
  boost::geometry::transform(line.obj_, line_translated.obj_, translate);
  return line_translated;
}

inline Line Simplify(const Line& line, double max_distance) {
  Line temp_line;
  boost::geometry::simplify(line.obj_, temp_line.obj_, max_distance);
  temp_line.RecomputeS();
  return temp_line;
}

inline int GetSegmentEndIdx(Line l, double s) {
  std::vector<double>::iterator up =
      std::upper_bound(l.s_.begin(), l.s_.end(), s);
  if (up != l.s_.end()) {
    int retval = up - l.s_.begin();
    return retval;
  } else {
    return l.s_.size() - 1;  // last point if s is larger then line length
  }
}

inline bool CheckSForSegmentIntersection(Line l, double s) {
  int start_it = GetSegmentEndIdx(l, s);
  std::vector<double>::iterator low =
      std::lower_bound(l.s_.begin(), l.s_.end(), s);
  int start_it_low = low - l.s_.begin();
  return start_it != start_it_low;
}

inline Point2d GetPointAtIdx(const Line& l, const uint idx) {
  if (idx > l.obj_.size() - 1) {
    LOG(WARNING) << "idx is outside line";
    return l.obj_.back();
  } else {
    return l.obj_.at(idx);
  }
}

inline Eigen::VectorXd Gradient(Eigen::VectorXd vec) {
  // calculating central difference
  Eigen::VectorXd g(vec.size());
  for (int i = 1; i < vec.size() - 1; i++) {
    g(i) = (vec(i + 1) - vec(i - 1)) / 2;
  }
  // TODO: find better solution for first and last point
  g(0) = g(1);
  g(vec.size() - 1) = g(vec.size() - 2);
  return g;
}

inline Eigen::VectorXd GetCurvature(const Line& l) {
  Eigen::MatrixXd larray = l.ToArray();
  Eigen::VectorXd dx = Gradient(larray.col(0));
  Eigen::VectorXd ddx = Gradient(dx);
  Eigen::VectorXd dy = Gradient(larray.col(1));
  Eigen::VectorXd ddy = Gradient(dy);

  // elementwise, as pow(vector, scalar) does not work
  Eigen::VectorXd curvature(larray.rows());
  for (int i = 0; i < curvature.size(); i++) {
    double n = dx(i) * ddy(i) - ddx(i) * dy(i);
    double r = pow(dx(i), 2) + pow(dy(i), 2);
    double d = pow(r, 1.5);
    if (d == 0) {
      curvature(i) = 0;
    } else {
      curvature(i) = n / d;
    }
  }
  return curvature;
}

inline Point2d GetPointAtS(Line l, double s) {
  const size_t& length = l.obj_.size();
  if (length <= 1) {  // this is an error Line consist of 0 or 1 element
    return Point2d(0, 0);
  } else if (s <= 0.0) {  // edge case begin
    return l.obj_.at(0);
  } else if (s >= l.s_.back()) {  // edge case end
    return l.obj_.at(length - 1);
  } else {  // nominal case
    int segment_end_idx = GetSegmentEndIdx(l, s);
    int segment_begin_idx = segment_end_idx - 1;

    double s_on_segment =
        (s - l.s_.at(segment_begin_idx)) /
        (l.s_.at(segment_end_idx) - l.s_.at(segment_begin_idx));
    double interp_pt_x =
        bg::get<0>(l.obj_.at(segment_begin_idx)) +
        s_on_segment * (bg::get<0>(l.obj_.at(segment_end_idx)) -
                        bg::get<0>(l.obj_.at(segment_begin_idx)));
    double interp_pt_y =
        bg::get<1>(l.obj_.at(segment_begin_idx)) +
        s_on_segment * (bg::get<1>(l.obj_.at(segment_end_idx)) -
                        bg::get<1>(l.obj_.at(segment_begin_idx)));
    return Point2d(interp_pt_x, interp_pt_y);
  }
}

inline double GetTangentAngleAtS(Line l, double s) {
  if (s >= l.s_.back()) {
    Point2d p1 = l.obj_.at(l.obj_.size() - 2);
    Point2d p2 = l.obj_.at(l.obj_.size() - 1);
    double angle =
        atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1));
    return angle;
  } else if (s <= 0.0) {
    Point2d p1 = l.obj_.at(0);
    Point2d p2 = l.obj_.at(1);
    return atan2(bg::get<1>(p2) - bg::get<1>(p1),
                 bg::get<0>(p2) - bg::get<0>(p1));
  } else {  // not start or end
    int end_segment_it = GetSegmentEndIdx(l, s);
    // check if s is at intersection, if true then calculate the angle
    if (CheckSForSegmentIntersection(l, s)) {
      Point2d p1 = l.obj_.at(end_segment_it - 2);
      Point2d p2 = l.obj_.at(end_segment_it - 1);
      Point2d p3 = l.obj_.at(end_segment_it);
      double sin_mean = 0.5 * (sin(atan2(bg::get<1>(p2) - bg::get<1>(p1),
                                         bg::get<0>(p2) - bg::get<0>(p1))) +
                               sin(atan2(bg::get<1>(p3) - bg::get<1>(p2),
                                         bg::get<0>(p3) - bg::get<0>(p2))));
      double cos_mean = 0.5 * (cos(atan2(bg::get<1>(p2) - bg::get<1>(p1),
                                         bg::get<0>(p2) - bg::get<0>(p1))) +
                               cos(atan2(bg::get<1>(p3) - bg::get<1>(p2),
                                         bg::get<0>(p3) - bg::get<0>(p2))));
      return atan2(sin_mean, cos_mean);
    } else {  // every s not start, end or intersection
      Point2d p1 = l.obj_.at(end_segment_it - 1);
      Point2d p2 = l.obj_.at(end_segment_it);
      return atan2(bg::get<1>(p2) - bg::get<1>(p1),
                   bg::get<0>(p2) - bg::get<0>(p1));
    }
  }
}

inline Point2d GetNormalAtS(const Line& l, double s) {
  double tangent = GetTangentAngleAtS(l, s);
  // rotate unit vector anti-clockwise with angle = tangent by 1/2 pi
  Point2d t(cos(tangent + asin(1)), sin(tangent + asin(1)));
  return t;
}

inline Line GetLineFromSInterval(const Line& line, double begin, double end) {
  Line new_line;
  new_line.AddPoint(GetPointAtS(line, begin));
  std::vector<Point2d> points = line.GetPointsInSInterval(begin, end);
  for (auto const& point : points) {
    new_line.AddPoint(point);
  }
  new_line.AddPoint(GetPointAtS(line, end));
  return new_line;
}

inline Line GetLineShiftedLaterally(const Line& line, double lateral_shift) {
  Line new_line;
  for (const auto& s : line.s_) {
    const Point2d normal = GetNormalAtS(line, s);
    const Point2d point_at_s = GetPointAtS(line, s);
    const Point2d shifted = point_at_s + (normal * lateral_shift);
    new_line.AddPoint(shifted);
  }
  return new_line;
}

inline std::tuple<Point2d, double, uint> GetNearestPointAndS(
    const Line& l, const Point2d& p) {  // GetNearestPoint
  // edge cases: empty or one-point line
  if (l.obj_.empty()) {
    return std::make_tuple(Point2d(0, 0), 0.0, 0);
  } else if (l.obj_.size() == 1) {
    return std::make_tuple(l.obj_.at(0), 0.0, 0);
  }

  // nominal case:
  // check distance to each line segment and find closest segment
  double min_dist = boost::numeric::bounds<double>::highest();
  int min_segment_idx = 0;
  for (uint line_idx = 0; line_idx < l.obj_.size() - 1; ++line_idx) {
    bg::model::linestring<Point2d> current_segment;
    bg::append(current_segment, l.obj_.at(line_idx));
    bg::append(current_segment, l.obj_.at(line_idx + 1));
    double d = bg::comparable_distance(current_segment, p);
    if (d < min_dist) {
      min_dist = d;
      min_segment_idx = line_idx;
    }
  }

  const double a1 = bg::get<0>(l.obj_.at(min_segment_idx));
  const double a2 = bg::get<1>(l.obj_.at(min_segment_idx));
  const double b1 = bg::get<0>(l.obj_.at(min_segment_idx + 1));
  const double b2 = bg::get<1>(l.obj_.at(min_segment_idx + 1));
  const double p1 = bg::get<0>(p);
  const double p2 = bg::get<1>(p);

  const double lambda = -(a1 * b1 + a2 * b2 + a1 * p1 + a2 * p2 - b1 * p1 -
                          b2 * p2 - a1 * a1 - a2 * a2)  // NOLINT
                        / (a1 * a1 - 2 * a1 * b1 + a2 * a2 - 2 * a2 * b2 +
                           b1 * b1 + b2 * b2);  // NOLINT

  // calculate interpolated s value
  double s;
  // double dist;  // unused
  Point2d retval;

  if (lambda < 0) {  // extrapolation front
    s = l.s_.at(min_segment_idx);
    retval = Point2d(a1, a2);
    // debug
    // dist = sqrt(pow(p1 - a1, 2) + pow(p2 - a2, 2));
  } else if (lambda > 1) {  // extrapolation end
    s = l.s_.at(min_segment_idx + 1);
    retval = Point2d(b1, b2);
    // debug
    // dist = sqrt(pow(p1 - b1, 2) + pow(p2 - b2, 2));
  } else {  // real interpolation
    s = (1 - lambda) * l.s_.at(min_segment_idx) +
        lambda * l.s_.at(min_segment_idx + 1);  // NOLINT

    const double s1 =
        (p1 * a1 * a1 - a1 * a2 * b2 + p2 * a1 * a2 - 2 * p1 * a1 * b1 +
         a1 * b2 * b2 - p2 * a1 * b2 + a2 * a2 * b1 - a2 * b1 * b2 -
         p2 * a2 * b1 + p1 * b1 * b1 + p2 * b1 * b2)  // NOLINT
        / (a1 * a1 - 2 * a1 * b1 + a2 * a2 - 2 * a2 * b2 + b1 * b1 +
           b2 * b2);  // NOLINT
    const double s2 =
        (a1 * a1 * b2 - a1 * a2 * b1 + p1 * a1 * a2 - a1 * b1 * b2 -
         p1 * a1 * b2 + p2 * a2 * a2 + a2 * b1 * b1 - p1 * a2 * b1 -
         2 * p2 * a2 * b2 + p1 * b1 * b2 + p2 * b2 * b2)  // NOLINT
        / (a1 * a1 - 2 * a1 * b1 + a2 * a2 - 2 * a2 * b2 + b1 * b1 +
           b2 * b2);  // NOLINT

    // debug
    // dist = sqrt(pow(p1 - s1, 2) + pow(p2 - s2, 2));
    retval = Point2d(s1, s2);
  }

  // debug
  // const double dist_boost = bg::distance(l.obj_, p);

  // return
  return std::make_tuple(retval, s, min_segment_idx);
}
inline Point2d GetNearestPoint(const Line& l, const Point2d& p) {
  return std::get<0>(GetNearestPointAndS(l, p));
}
inline double GetNearestS(const Line& l, const Point2d& p) {
  return std::get<1>(GetNearestPointAndS(l, p));
}
inline uint FindNearestIdx(const Line& l, const Point2d& p) {
  return std::get<2>(GetNearestPointAndS(l, p));
}
//! Point - Line collision checker using boost::intersection
inline bool Collide(const Line& l, const LinePoint& p) {
  std::vector<LinePoint> shape_intersect;
  bg::intersection(l.obj_, p, shape_intersect);
  return !shape_intersect.empty();
}

//! Line - Point collision checker
inline bool Collide(const LinePoint& p, const Line& l) { return Collide(l, p); }

//! Line - Line collision checker using boost::intersection
inline bool Collide(const Line& l1, const Line& l2) {
  std::vector<bg::model::linestring<LinePoint>> shape_intersect;
  bg::intersection(l1.obj_, l2.obj_, shape_intersect);
  return !shape_intersect.empty();
}

// An oriented point can have a linestring (the nearest point on it)
// on the left or right side, left side < 0, right side > 0
inline double SignedDistance(const Line& line, const Point2d& p,
                             const double& orientation) {
  auto closest_point = GetNearestPoint(line, p);
  auto direction_vector = closest_point - p;

  double diff = SignedAngleDiff(
      orientation,
      atan2(bg::get<1>(direction_vector), bg::get<0>(direction_vector)));
  double sign = (diff > 0) ? 1 : ((diff < 0) ? -1 : 0);

  return bg::distance(line.obj_, p) * sign;
}

inline Line AppendLinesNoIntersect(const Line& ls1, const Line& ls2) {
  std::vector<Point2d> intersecting_points;
  bg::intersection(ls1.obj_, ls2.obj_, intersecting_points);
  Line lout;
  if (intersecting_points.size() == 1) {
    // get s value for both lines
    double s_i1 = GetNearestS(ls1, intersecting_points.at(0));
    double s_i2 = GetNearestS(ls2, intersecting_points.at(0));
    double rel_s_i1 = s_i1 / ls1.Length();
    double rel_s_i2 = s_i2 / ls2.Length();

    Line ls1_part, ls2_part;
    if (rel_s_i1 < 0.3) {
      // take latter part of line
      ls1_part = GetLineFromSInterval(ls1, s_i1, ls1.Length());
    } else if (rel_s_i1 > 0.7) {
      // take front part of line
      ls1_part = GetLineFromSInterval(ls1, 0, s_i1);
    } else {
      LOG(WARNING) << "Lines intersecting too much, only appending";
      ls1_part = ls1;
    }

    if (rel_s_i2 < 0.3) {
      // take latter part of line
      ls2_part = GetLineFromSInterval(ls2, s_i2, ls2.Length());
    } else if (rel_s_i2 > 0.7) {
      // take front part of line
      ls2_part = GetLineFromSInterval(ls2, 0, s_i2);
    } else {
      LOG(WARNING) << "Lines intersecting too much, only appending";
      ls2_part = ls2;
    }

    lout = ls1_part;
    lout.AppendLinestring(ls2_part);
  } else if (intersecting_points.size() > 1) {
    // do something
    LOG(ERROR) << "two intersecting points";
    lout = ls1;
    lout.AppendLinestring(ls2);
  } else {
    lout = ls1;
    lout.AppendLinestring(ls2);
  }

  lout.RemoveDuplicates();

  if (boost::geometry::intersects(lout.obj_)) {
    const double tol = 1e-6;
    LOG(WARNING) << "AppendLinesNoIntersect yields self intersecting line, "
                    "will simplify it with "
                 << tol;
    VLOG(5) << "ls1: " << ls1.ToArray();
    VLOG(5) << "ls2: " << ls2.ToArray();
    VLOG(5) << "lout: " << lout.ToArray();

    lout = Simplify(lout, tol);
  }
  return lout;
}

inline Line ConcatenateLinestring(const Line& ls1, const Line& ls2) {
  // Get first and last points
  auto first_point_this = *ls1.begin();
  auto last_point_this = *(ls1.end() - 1);
  auto first_point_other = *ls2.begin();
  auto last_point_other = *(ls2.end() - 1);

  double d_first_first = Distance(first_point_this, first_point_other);
  double d_first_last = Distance(first_point_this, last_point_other);
  double d_last_first = Distance(last_point_this, first_point_other);
  double d_last_last = Distance(last_point_this, last_point_other);

  Line lconcat;
  if (d_first_first <= std::min({d_first_last, d_last_first, d_last_last})) {
    // Reverse this
    lconcat = AppendLinesNoIntersect(Reverse(ls1), ls2);
  } else if (d_first_last <=
             std::min({d_first_first, d_last_first, d_last_last})) {
    // Reverse both
    lconcat = AppendLinesNoIntersect(Reverse(ls1), Reverse(ls2));
  } else if (d_last_first <=
             std::min({d_first_first, d_first_last, d_last_last})) {
    // No reversing
    lconcat = AppendLinesNoIntersect(ls1, ls2);
  } else {
    // Reverse other
    lconcat = AppendLinesNoIntersect(ls1, Reverse(ls2));
  }
  return lconcat;
}

// Subsampling using spline
inline Line SmoothLine(const Line& l, const double ds) {
  if (l.size() < 3) {
    LOG(WARNING) << "cannot subsample line with only 3 points";
    return l;
  } else {
    int num_points = l.Length() / ds;

    tk::spline splineX, splineY;
    std::vector<double> xVec, yVec;
    for (size_t i = 0; i < l.obj_.size(); i++) {
      xVec.push_back(bg::get<0>(l.obj_[i]));
      yVec.push_back(bg::get<1>(l.obj_[i]));
    }
    std::vector<double> sVec(l.s_.begin(), l.s_.end());
    splineX.set_points(sVec, xVec);
    splineY.set_points(sVec, yVec);

    Line lss;
    for (size_t j = 0; j <= num_points; ++j) {
      double x = splineX(j * ds);
      double y = splineY(j * ds);
      lss.AddPoint(geometry::Point2d(x, y));
    }
    if (l.Length() > num_points * ds) {
      lss.AddPoint(l.obj_.at(l.size() - 1));
    }
    return lss;
  }
}

inline Line ComputeCenterLine(const Line& outer_line, const Line& inner_line) {
  if (boost::geometry::intersects(outer_line.obj_)) {
    LOG(WARNING) << "Computing center line, but outer line self-intersects";
  }
  if (boost::geometry::intersects(inner_line.obj_)) {
    LOG(WARNING) << "Computing center line, but inner line self-intersects";
  }

  Line center_line_;
  Line line_more_points = outer_line;
  Line line_less_points = inner_line;
  if (inner_line.obj_.size() > outer_line.obj_.size()) {
    line_more_points = inner_line;
    line_less_points = outer_line;
  }
  for (Point2d& point_loop : line_more_points.obj_) {
    Point2d nearest_point_other =
        geometry::GetNearestPoint(line_less_points, point_loop);
    geometry::Point2d middle_point = (point_loop + nearest_point_other) / 2;
    center_line_.AddPoint(middle_point);
  }
  return center_line_;
}

template <typename T>
std::pair<T, T> MergeBoundingBoxes(std::pair<T, T> bb1, std::pair<T, T> bb2) {
  Line_t<T> line;  // just use a line and add all points
  line.AddPoint(bb1.first);
  line.AddPoint(bb1.second);
  line.AddPoint(bb2.first);
  line.AddPoint(bb2.second);
  return line.BoundingBox();
}

}  // namespace geometry
}  // namespace bark

#endif  // BARK_GEOMETRY_LINE_HPP_


#include <boost/function_output_iterator.hpp>
#include <boost/geometry/geometry.hpp>
#include <chrono>
#include <iostream>
#include <random>
#include <vector>
#include <string>
#include <fstream>

// using cxxopts for CLI argument parsing
#include "packages/cxxopts.hpp"

// using prakhar1989's ProgressBar for CLI progress indicator
#include "packages/ProgressBar.hpp"

// using tinyobjloader for OBJ file parsing
#define TINYOBJLOADER_IMPLEMENTATION
#define EPSILON 0.00000001
#include "packages/tiny_obj_loader.h"

using namespace std;

// full 3D model to inhibit growth
string fullModelFilename = "";
tinyobj::attrib_t fullModelAttrib;
vector<tinyobj::shape_t> fullModelShapes;
vector<tinyobj::material_t> fullModelMaterials;

// 3D model of selected faces to seed growth
string seedModelFilename = "";
tinyobj::attrib_t seedModelAttrib;
vector<tinyobj::shape_t> seedModelShapes;
vector<tinyobj::material_t> seedModelMaterials;

// number of particles
const int DefaultNumberOfParticles = 1000000;
int numParticles = DefaultNumberOfParticles;

// progress bar
ProgressBar progressBar(numParticles, 40);

// output file
string filename = "points";
ofstream file;

// interval (in iterations) that point data is outputted
int lastOutputIteration = 0;
int interval = numParticles;

// number of dimensions (must be 2 or 3)
const int D = 3;

// default parameters (documented below)
const double DefaultParticleSpacing = 1;
const double DefaultAttractionDistance = 3;
const double DefaultMinMoveDistance = 1;
const int DefaultStubbornness = 0;
const double DefaultStickiness = 1;
const double DefaultBoundingRadius = 0;

// boost is used for its spatial index
using BoostPoint = boost::geometry::model::point<double, D, boost::geometry::cs::cartesian>;
using IndexValue = pair<BoostPoint, int>;
using Index = boost::geometry::index::rtree<IndexValue, boost::geometry::index::linear<4>>;

// Vector represents a point or a vector
class Vector {
public:
    Vector() :
        m_X(0), m_Y(0), m_Z(0) {}

    Vector(double x, double y) :
        m_X(x), m_Y(y), m_Z(0) {}

    Vector(double x, double y, double z) :
        m_X(x), m_Y(y), m_Z(z) {}

    double X() const {
        return m_X;
    }

    double Y() const {
        return m_Y;
    }

    double Z() const {
        return m_Z;
    }

    BoostPoint ToBoost() const {
        return BoostPoint(m_X, m_Y, m_Z);
    }

    double Length() const {
        return sqrt(m_X * m_X + m_Y * m_Y + m_Z * m_Z);
    }

    double LengthSquared() const {
        return m_X * m_X + m_Y * m_Y + m_Z * m_Z;
    }

    double Distance(const Vector &v) const {
        const double dx = m_X - v.m_X;
        const double dy = m_Y - v.m_Y;
        const double dz = m_Z - v.m_Z;
        return sqrt(dx * dx + dy * dy + dz * dz);
    }

    Vector Normalized() const {
        const double m = 1 / Length();
        return Vector(m_X * m, m_Y * m, m_Z * m);
    }

    Vector GetCrossed(const Vector &v) const {
        return Vector(m_Y * v.m_Z - m_Z * v.m_Y,
                      m_Z * v.m_X - m_X * v.m_Z,
                      m_X * v.m_Y - m_Y * v.m_X);
    }

    double GetDot(const Vector &v) const {
        return m_X * v.m_X +
               m_Y * v.m_Y +
               m_Z * v.m_Z;
    }

    Vector operator+(const Vector &v) const {
        return Vector(m_X + v.m_X, m_Y + v.m_Y, m_Z + v.m_Z);
    }

    Vector operator-(const Vector &v) const {
        return Vector(m_X - v.m_X, m_Y - v.m_Y, m_Z - v.m_Z);
    }

    Vector operator*(const double a) const {
        return Vector(m_X * a, m_Y * a, m_Z * a);
    }

    Vector &operator+=(const Vector &v) {
        m_X += v.m_X; m_Y += v.m_Y; m_Z += v.m_Z;
        return *this;
    }

    bool operator==(const Vector &v) const {
        return m_X == v.m_X && m_Y == v.m_Y && m_Z == v.m_Z;
    }

private:
    double m_X;
    double m_Y;
    double m_Z;
};

// Lerp linearly interpolates from a to b by distance.
Vector Lerp(const Vector &a, const Vector &b, const double d) {
    return a + (b - a).Normalized() * d;
}

// Random returns a uniformly distributed random number between lo and hi
double Random(const double lo = 0, const double hi = 1) {
    static thread_local mt19937 gen(chrono::high_resolution_clock::now().time_since_epoch().count());
    uniform_real_distribution<double> dist(lo, hi);
    return dist(gen);
}

// RandomInUnitSphere returns a random, uniformly distributed point inside the
// unit sphere (radius = 1)
Vector RandomInUnitSphere() {
    while (true) {
        const Vector p = Vector(
            Random(-1, 1),
            Random(-1, 1),
            D == 2 ? 0 : Random(-1, 1));
        if (p.LengthSquared() < 1) {
            return p;
        }
    }
}

// IsIntersectingFace checks for intersection of a ray (O) with random direction (D) and a triangle face defined by vertices V1, V2, and V3
// Adapted from Amnon Owed's ofxPointInMesh::triangleIntersection: https://github.com/AmnonOwed/ofxPointInMesh
// Uses Möller–Trumbore: http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
bool IsIntersectingFace(const Vector &V1, const Vector &V2, const Vector &V3, const Vector &O, const Vector &D, Vector &R) {
	Vector e1, e2; // Edge1, Edge2
	Vector P, Q, T;
	float det, inv_det, u, v;
	float t;

	// Find vectors for two edges sharing V1
	e1 = V2 - V1;
	e2 = V3 - V1;

	// Begin calculating determinant - also used to calculate u parameter
	P = D.GetCrossed(e2);

	// if determinant is near zero, ray lies in plane of triangle
	det = e1.GetDot(P);

	// NOT CULLING
	if(det > -EPSILON && det < EPSILON){
		return false;
	}

	inv_det = 1.f / det;

	// calculate distance from V1 to ray origin
	T = O - V1;

	// calculate u parameter and test bound
	u = T.GetDot(P) * inv_det;

	// the intersection lies outside of the triangle
	if(u < 0.f || u > 1.f){
		return false;
	}

	// prepare to test v parameter
	Q = T.GetCrossed(e1);

	// calculate V parameter and test bound
	v = D.GetDot(Q) * inv_det;

	// the intersection lies outside of the triangle
	if(v < 0.f || u + v  > 1.f){
		return false;
	}

	t = e2.GetDot(Q) * inv_det;

	if(t > EPSILON){ // ray intersection
		R = O + D * t; // store intersection point
		return true;
	}

	// no hit, no win
	return false;
}

// IsInsideMesh determines if a given point is inside of the base model mesh
// Adapted from Amnon Owed's ofxPointInMesh::isInside: https://github.com/AmnonOwed/ofxPointInMesh
bool IsInsideMesh(Vector &p) {
    // if no mesh was provided, skip this test
    if(fullModelFilename.empty()) {
        return false;
    }

    Vector foundIntersection; // variable to store a single found intersection
	vector<Vector> results;  // vector to store all found intersections
	Vector randomDirection = Vector(0.1, 0.2, 0.3); // a random direction

    // go over all the shapes in the mesh
    for (size_t s = 0; s < fullModelShapes.size(); s++) {
        size_t index_offset = 0;

        // go over all the faces in the shape
        for (size_t f = 0; f < fullModelShapes[s].mesh.num_face_vertices.size(); f++) {
            unsigned int fv = fullModelShapes[s].mesh.num_face_vertices[f];
            vector<Vector> vertices;

            // collect the vertices
            for (size_t v = 0; v < fv; v++) {
                tinyobj::index_t idx = fullModelShapes[s].mesh.indices[index_offset + v];
                tinyobj::real_t vx = fullModelAttrib.vertices[3*idx.vertex_index+0];
                tinyobj::real_t vy = fullModelAttrib.vertices[3*idx.vertex_index+1];
                tinyobj::real_t vz = fullModelAttrib.vertices[3*idx.vertex_index+2];
                vertices.push_back(Vector(vx, vy, vz));
            }

            index_offset += fv;

            // do a triangle-ray intersection on each face in the mesh
            // store the intersection (if any) in the variable foundIntersection
            if(IsIntersectingFace(vertices[0], vertices[1], vertices[2], p, randomDirection, foundIntersection)) {
                // store all found intersections
                results.push_back(foundIntersection);
            }
        }
    }

    // handle multiple mesh intersections at the same point (by removing duplicates)
    vector<Vector> unique_results;
    unique_copy(results.begin(), results.end(), back_inserter(unique_results));

    // // determine if the point is inside or outside the mesh, based on the number of unique intersections
    if(unique_results.size() % 2 == 1) {
        return true;
    } else {
        return false;
    }
}

// GetDot2 calculates the dot product of a vector with itself
double GetDot2(const Vector &p) {
    return p.GetDot(p);
}

// GetSign returns -1 for negative numbers, 1 for positive numbers, and 0 for 0
int GetSign(const double &v) {
    int result = 0;
    if(v < 0) {
        result = -1;
    } else if(v > 0) {
        result = 1;
    }
    return result;
}

// clamp ensures that a value (x) is within the range defined by [lower] and [upper]
double clamp(double x, double lower, double upper) {
    return min(upper, max(x, lower));
}

// DistanceToTriangle calculates the shortest distance between a point (p) and a 3D triangle defined by vertices v1, v2, and v3
// Adapted from: https://iquilezles.org/www/articles/triangledistance/triangledistance.htm
double DistanceToTriangle(const Vector &v1, const Vector &v2, const Vector &v3, const Vector &p) {
    Vector v21 = v2 - v1;
    Vector v32 = v3 - v2;
    Vector v13 = v1 - v3;
    Vector p1 = p - v1;
    Vector p2 = p - v2;
    Vector p3 = p - p3;
    Vector nor = v21.GetCrossed(v13);

    return sqrt(
        // inside/outside test
        (GetSign(p1.GetDot(v21.GetCrossed(nor))) +
         GetSign(p2.GetDot(v32.GetCrossed(nor))) +
         GetSign(p3.GetDot(v13.GetCrossed(nor))) < 2.0)
        ?
        // 3 edges
        min(
            min(
                GetDot2(v21 * clamp(p1.GetDot(v21) / GetDot2(v21), 0.0, 1.0) - p1),
                GetDot2(v32 * clamp(p2.GetDot(v32) / GetDot2(v32), 0.0, 1.0) - p2)
            ),
            GetDot2(v13 * clamp(p3.GetDot(v13) / GetDot2(v13), 0.0, 1.0) - p3)
        )
        :
        // 1 face
        p1.GetDot(nor) * p1.GetDot(nor) / GetDot2(nor)
    );
}

// Model holds all of the particles and defines their behavior.
class Model {
public:
    Model() :
        m_ParticleSpacing(DefaultParticleSpacing),
        m_AttractionDistance(DefaultAttractionDistance),
        m_MinMoveDistance(DefaultMinMoveDistance),
        m_Stubbornness(DefaultStubbornness),
        m_Stickiness(DefaultStickiness),
        m_BoundingRadius(DefaultBoundingRadius) {}

    void SetParticleSpacing(const double a) {
        m_ParticleSpacing = a;
    }

    void SetAttractionDistance(const double a) {
        m_AttractionDistance = a;
    }

    void SetMinMoveDistance(const double a) {
        m_MinMoveDistance = a;
    }

    void SetStubbornness(const int a) {
        m_Stubbornness = a;
    }

    void SetStickiness(const double a) {
        m_Stickiness = a;
    }

    void SetBoundingRadius(const double a) {
        m_BoundingRadius = a;
    }

    // Add adds a new particle with the specified parent particle
    void Add(const Vector &p, const int parent = -1) {
        const int id = m_Points.size();
        m_Index.insert(make_pair(p.ToBoost(), id));
        m_Points.push_back(p);
        m_Parents.push_back(parent);
        m_JoinAttempts.push_back(0);
        m_BoundingRadius = max(m_BoundingRadius, p.Length() + m_AttractionDistance);

        // update and display progress bar
        ++progressBar;
        progressBar.display();

        // wrap up the progress bar when last particle is placed
        if(id == numParticles) {
            progressBar.done();
        }
    }

    // Nearest returns the index of the particle nearest the specified point
    int Nearest(const Vector &point) const {
        int result = -1;
        m_Index.query(
            boost::geometry::index::nearest(point.ToBoost(), 1),
            boost::make_function_output_iterator([&result](const auto &value) {
                result = value.second;
            }));
        return result;
    }

    // DistanceToNearestFace calculates the shortest distance from a particle (p) and
    // TODO: if needed, consider putting mesh vertices in spatial index, then doing knn search search within radius defined by largest edge found in mesh (precomputed)
    double DistanceToNearestFace(const Vector &p) {
        double distance = m_AttractionDistance;

        // go over all the shapes in the mesh
        for (size_t s = 0; s < seedModelShapes.size(); s++) {
            size_t index_offset = 0;

            // go over all the faces in the shape
            for (size_t f = 0; f < seedModelShapes[s].mesh.num_face_vertices.size(); f++) {
                unsigned int fv = seedModelShapes[s].mesh.num_face_vertices[f];
                vector<Vector> vertices;

                // collect the vertices
                for (size_t v = 0; v < fv; v++) {
                    tinyobj::index_t idx = seedModelShapes[s].mesh.indices[index_offset + v];
                    tinyobj::real_t vx = seedModelAttrib.vertices[3*idx.vertex_index+0];
                    tinyobj::real_t vy = seedModelAttrib.vertices[3*idx.vertex_index+1];
                    tinyobj::real_t vz = seedModelAttrib.vertices[3*idx.vertex_index+2];
                    vertices.push_back(Vector(vx, vy, vz));
                }

                index_offset += fv;

                // calculate distance from point to face
                distance = min(distance, DistanceToTriangle(vertices[0], vertices[1], vertices[2], p));
            }
        }

        return distance;
    }

    // RandomStartingPosition returns a random point to start a new particle
    Vector RandomStartingPosition() const {
        const double d = m_BoundingRadius;
        return RandomInUnitSphere().Normalized() * d;
    }

    // ShouldReset returns true if the particle has gone too far away and
    // should be reset to a new random starting position
    bool ShouldReset(const Vector &p) const {
        return p.Length() > m_BoundingRadius * 2;
    }

    // ShouldJoin returns true if the point should attach to the specified
    // parent particle. This is only called when the point is already within
    // the required attraction distance.
    bool ShouldJoin(const Vector &p, const int parent) {
        m_JoinAttempts[parent]++;
        if (m_JoinAttempts[parent] < m_Stubbornness) {
            return false;
        }
        return Random() <= m_Stickiness;
    }

    // PlaceParticle computes the final placement of the particle.
    Vector PlaceParticle(const Vector &p, const int parent) const {
        return Lerp(m_Points[parent], p, m_ParticleSpacing);
    }

    // MotionVector returns a vector specifying the direction that the
    // particle should move for one iteration. The distance that it will move
    // is determined by the algorithm.
    Vector MotionVector(const Vector &p) const {
        return RandomInUnitSphere();
    }

    // AddParticle diffuses one new particle and adds it to the model
    void AddParticle() {
        // compute particle starting location
        Vector p = RandomStartingPosition();

        // do the random walk
        while (true) {
            // get distance to nearest other particle
            const int parent = Nearest(p);
            const double d = p.Distance(m_Points[parent]);

            // do not allow particle to stick when its inside of the base model mesh
            if(!IsInsideMesh(p)) {
                // check for particle-particle collisions
                if (d < m_AttractionDistance) {
                    if (!ShouldJoin(p, parent)) {
                        // push particle away a bit
                        p = Lerp(m_Points[parent], p, m_AttractionDistance + m_MinMoveDistance);
                        continue;
                    }

                    // adjust particle position in relation to its parent
                    p = PlaceParticle(p, parent);

                    // add the point
                    Add(p);
                    return;
                }

                // get distance to the nearest seed face
                const double df = DistanceToNearestFace(p);

                // check for particle-face collisions
                if (df < m_AttractionDistance) {
                    Add(p, -1);
                    return;
                }
            }

            // move randomly
            const double m = max(m_MinMoveDistance, d - m_AttractionDistance);
            p += MotionVector(p).Normalized() * m;

            // check if particle is too far away, reset if so
            if (ShouldReset(p)) {
                p = RandomStartingPosition();
            }
        }
    }

    // OutputPointData creates a new point data file and fills it with most current point data
    void OutputPointData(const int iteration) const {
        file.open("data/" + filename + "-" + to_string(iteration) + ".csv");

        for(unsigned int id = 0; id < m_Points.size(); id++) {
            file << id << "," << m_Parents[id] << "," << m_Points[id].X() << "," << m_Points[id].Y() << "," << m_Points[id].Z() << endl;
        }

        file.close();
    }

private:
    // m_ParticleSpacing defines the distance between particles that are
    // joined together
    double m_ParticleSpacing;

    // m_AttractionDistance defines how close together particles must be in
    // order to join together
    double m_AttractionDistance;

    // m_MinMoveDistance defines the minimum distance that a particle will move
    // during its random walk
    double m_MinMoveDistance;

    // m_Stubbornness defines how many interactions must occur before a
    // particle will allow another particle to join to it.
    int m_Stubbornness;

    // m_Stickiness defines the probability that a particle will allow another
    // particle to join to it.
    double m_Stickiness;

    // m_BoundingRadius defines the radius of the bounding sphere that bounds
    // all of the particles
    double m_BoundingRadius;

    // m_Points stores the final particle positions
    vector<Vector> m_Points;

    // m_Parents stores the parent IDs of each clustered particle
    vector<int> m_Parents;

    // m_JoinAttempts tracks how many times other particles have attempted to
    // join with each finalized particle
    vector<int> m_JoinAttempts;

    // m_Index is the spatial index used to accelerate nearest neighbor queries
    Index m_Index;
};

// create the model in global scope so that parseArgs can configure it
Model model;

// Parses CLI arguments and configures simulation with what is passed
void ParseArgs(int argc, char* argv[]) {
    try {
        cxxopts::Options options(argv[0]);

        options
        .allow_unrecognised_options()
        .add_options()
            ("p,particles", "Number of walker particles", cxxopts::value<int>())
            ("i,input", "Full 3D model filename (.obj)", cxxopts::value<string>())
            ("f,faces", "Seed faces 3D model filename (.obj)", cxxopts::value<string>())
            ("o,output", "Point data output filename", cxxopts::value<string>())
            ("n,interval", "Point data capture interval", cxxopts::value<int>())
            ("s,spacing", "Particle spacing", cxxopts::value<double>())
            ("a,attraction", "Attraction distance", cxxopts::value<double>())
            ("m,move", "Minimum move distance", cxxopts::value<double>())
            ("b,stubbornness", "Stubbornness", cxxopts::value<int>())
            ("k,stickiness", "Stickiness", cxxopts::value<double>())
            ("r,radius", "Initial bounding radius", cxxopts::value<double>())
        ;

        auto result = options.parse(argc, argv);

        if(result.count("particles")) {
            numParticles = result["particles"].as<int>();
            interval = numParticles;
            progressBar.setTotal(numParticles);
        }

        if(result.count("input")) {
            fullModelFilename = result["input"].as<string>();

            // throw error when filename doesn't end in `.obj`
            if(fullModelFilename.substr(fullModelFilename.length() - 4, fullModelFilename.length() - 1).compare(".obj") != 0) {
                cerr << "Base model file must be an OBJ file" << endl;
                exit(1);
            }
        }

        if(result.count("faces")) {
            seedModelFilename = result["faces"].as<string>();

            // throw error when filename doesn't end in `.obj`
            if(seedModelFilename.substr(seedModelFilename.length() - 4, seedModelFilename.length() - 1).compare(".obj") != 0) {
                cerr << "Seed model file must be an OBJ file" << endl;
                exit(1);
            }
        }

        if(result.count("output")) {
            filename = result["output"].as<string>();
        }

        if(result.count("interval")) {
            interval = result["interval"].as<int>();
        }

        if(result.count("spacing")) {
            model.SetParticleSpacing(result["spacing"].as<double>());
        }

        if(result.count("attraction")) {
            model.SetAttractionDistance(result["attraction"].as<double>());
        }

        if(result.count("move")) {
            model.SetMinMoveDistance(result["move"].as<double>());
        }

        if(result.count("stubbornness")) {
            model.SetStubbornness(result["stubbornness"].as<int>());
        }

        if(result.count("stickiness")) {
            model.SetStickiness(result["stickiness"].as<double>());
        }

        if(result.count("radius")) {
            model.SetBoundingRadius(result["radius"].as<double>());
        }
    } catch(const cxxopts::OptionException& e) {
        cout << "Error parsing options: " << e.what() << endl;
        exit(1);
    }
}

// LoadFullModel loads the 3D model passed with the `-i` option
void LoadFullModel() {
    string warn;
    string err;

    bool ret = tinyobj::LoadObj(&fullModelAttrib, &fullModelShapes, &fullModelMaterials, &warn, &err, fullModelFilename.c_str());

    if (!warn.empty()) {
        cout << warn << endl;
    }

    if (!err.empty()) {
        cerr << err << endl;
    }

    if (!ret) {
        exit(1);
    }
}

// LoadSeedModel loads the 3D model passed with the `-f` option
void LoadSeedModel() {
    string warn;
    string err;

    bool ret = tinyobj::LoadObj(&seedModelAttrib, &seedModelShapes, &seedModelMaterials, &warn, &err, seedModelFilename.c_str());

    if (!warn.empty()) {
        cout << warn << endl;
    }

    if (!err.empty()) {
        cerr << err << endl;
    }

    if (!ret) {
        exit(1);
    }
}

int main(int argc, char* argv[]) {
    // parse the CLI arguments
    ParseArgs(argc, argv);

    // load the full 3D model for inhibiting growth
    if(!fullModelFilename.empty()) {
        LoadFullModel();
    }

    // load the seed faces 3D model
    if(!seedModelFilename.empty()) {
        LoadSeedModel();
    }

    // add seed point at origin (a single point is necessary for now)
    model.Add(Vector());

    // {
    //     const int n = 3600;
    //     const double r = 1000;
    //     for (int i = 0; i < n; i++) {
    //         const double t = (double)i / n;
    //         const double a = t * 2 * M_PI;
    //         const double x = cos(a) * r;
    //         const double y = sin(a) * r;
    //         model.Add(Vector(x, y, 0));
    //     }
    // }

    // run diffusion-limited aggregation
    for (int i = 1; i <= numParticles; i++) {
        model.AddParticle();

        // output current point data based on interval
        if(i - lastOutputIteration >= interval) {
            model.OutputPointData(i);
            lastOutputIteration = i;
        }
    }

    return 0;
}


/*

Copyright (c) 2013, Project OSRM, Dennis Luxen, others
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include <osrm/Coordinate.h>
#include "../Util/MercatorUtil.h"
#ifndef NDEBUG
#include "../Util/simple_logger.hpp"
#endif
#include "../Util/string_util.hpp"

#include <boost/assert.hpp>

#ifndef NDEBUG
#include <bitset>
#endif
#include <iostream>
#include <limits>

FixedPointCoordinate::FixedPointCoordinate()
    : lat(std::numeric_limits<int>::min()), lon(std::numeric_limits<int>::min())
{
}

FixedPointCoordinate::FixedPointCoordinate(int lat, int lon) : lat(lat), lon(lon)
{
#ifndef NDEBUG
    if (0 != (std::abs(lat) >> 30))
    {
        std::bitset<32> y_coordinate_vector(lat);
        SimpleLogger().Write(logDEBUG) << "broken lat: " << lat
                                       << ", bits: " << y_coordinate_vector;
    }
    if (0 != (std::abs(lon) >> 30))
    {
        std::bitset<32> x_coordinate_vector(lon);
        SimpleLogger().Write(logDEBUG) << "broken lon: " << lon
                                       << ", bits: " << x_coordinate_vector;
    }
#endif
}

void FixedPointCoordinate::Reset()
{
    lat = std::numeric_limits<int>::min();
    lon = std::numeric_limits<int>::min();
}
bool FixedPointCoordinate::isSet() const
{
    return (std::numeric_limits<int>::min() != lat) && (std::numeric_limits<int>::min() != lon);
}
bool FixedPointCoordinate::is_valid() const
{
    if (lat > 90 * COORDINATE_PRECISION || lat < -90 * COORDINATE_PRECISION ||
        lon > 180 * COORDINATE_PRECISION || lon < -180 * COORDINATE_PRECISION)
    {
        return false;
    }
    return true;
}
bool FixedPointCoordinate::operator==(const FixedPointCoordinate &other) const
{
    return lat == other.lat && lon == other.lon;
}

double FixedPointCoordinate::ApproximateDistance(const int lat1,
                                                 const int lon1,
                                                 const int lat2,
                                                 const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
    double RAD = 0.017453292519943295769236907684886;
    double lt1 = lat1 / COORDINATE_PRECISION;
    double ln1 = lon1 / COORDINATE_PRECISION;
    double lt2 = lat2 / COORDINATE_PRECISION;
    double ln2 = lon2 / COORDINATE_PRECISION;
    double dlat1 = lt1 * (RAD);

    double dlong1 = ln1 * (RAD);
    double dlat2 = lt2 * (RAD);
    double dlong2 = ln2 * (RAD);

    double dLong = dlong1 - dlong2;
    double dLat = dlat1 - dlat2;

    double aHarv = pow(sin(dLat / 2.0), 2.0) + cos(dlat1) * cos(dlat2) * pow(sin(dLong / 2.), 2);
    double cHarv = 2. * atan2(sqrt(aHarv), sqrt(1.0 - aHarv));
    // earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
    // The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
    const double earth = 6372797.560856;
    return earth * cHarv;
}

double FixedPointCoordinate::ApproximateDistance(const FixedPointCoordinate &coordinate_1,
                                                 const FixedPointCoordinate &coordinate_2)
{
    return ApproximateDistance(
        coordinate_1.lat, coordinate_1.lon, coordinate_2.lat, coordinate_2.lon);
}

float FixedPointCoordinate::ApproximateEuclideanDistance(const FixedPointCoordinate &coordinate_1,
                                                         const FixedPointCoordinate &coordinate_2)
{
    return ApproximateEuclideanDistance(
        coordinate_1.lat, coordinate_1.lon, coordinate_2.lat, coordinate_2.lon);
}

float FixedPointCoordinate::ApproximateEuclideanDistance(const int lat1,
                                                         const int lon1,
                                                         const int lat2,
                                                         const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());

    const float RAD = 0.017453292519943295769236907684886f;
    const float float_lat1 = (lat1 / COORDINATE_PRECISION) * RAD;
    const float float_lon1 = (lon1 / COORDINATE_PRECISION) * RAD;
    const float float_lat2 = (lat2 / COORDINATE_PRECISION) * RAD;
    const float float_lon2 = (lon2 / COORDINATE_PRECISION) * RAD;

    const float x_value = (float_lon2 - float_lon1) * cos((float_lat1 + float_lat2) / 2.f);
    const float y_value = float_lat2 - float_lat1;
    const float earth_radius = 6372797.560856f;
    return sqrt(x_value * x_value + y_value * y_value) * earth_radius;
}

float
FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &source_coordinate,
                                                   const FixedPointCoordinate &target_coordinate,
                                                   const FixedPointCoordinate &point)
{
    // initialize values
    const float x_value = static_cast<float>(lat2y(point.lat / COORDINATE_PRECISION));
    const float y_value = point.lon / COORDINATE_PRECISION;
    float a = static_cast<float>(lat2y(source_coordinate.lat / COORDINATE_PRECISION));
    float b = source_coordinate.lon / COORDINATE_PRECISION;
    float c = static_cast<float>(lat2y(target_coordinate.lat / COORDINATE_PRECISION));
    float d = target_coordinate.lon / COORDINATE_PRECISION;
    float p, q;
    if (std::abs(a - c) > std::numeric_limits<float>::epsilon())
    {
        const float slope = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x_value + (slope * y_value)) + (slope * slope * a - slope * b)) /
            (1.f + slope * slope);
        q = b + slope * (p - a);
    }
    else
    {
        p = c;
        q = y_value;
    }

    float ratio;
    bool inverse_ratio = false;

    // straight line segment on equator
    if (std::abs(c) < std::numeric_limits<float>::epsilon() &&
        std::abs(a) < std::numeric_limits<float>::epsilon())
    {
        ratio = (q - b) / (d - b);
    }
    else
    {
        if (std::abs(c) < std::numeric_limits<float>::epsilon())
        {
            // swap start/end
            std::swap(a, c);
            std::swap(b, d);
            inverse_ratio = true;
        }

        float nY = (d * p - c * q) / (a * d - b * c);
        // discretize the result to coordinate precision. it's a hack!
        if (std::abs(nY) < (1.f / COORDINATE_PRECISION))
        {
            nY = 0.f;
        }

        // compute ratio
        ratio = (p - nY * a) / c;
    }

    if (std::isnan(ratio))
    {
        ratio = (target_coordinate == point ? 1.f : 0.f);
    }
    else if (std::abs(ratio) <= std::numeric_limits<float>::epsilon())
    {
        ratio = 0.f;
    }
    else if (std::abs(ratio - 1.f) <= std::numeric_limits<float>::epsilon())
    {
        ratio = 1.f;
    }

    // we need to do this, if we switched start/end coordinates
    if (inverse_ratio)
    {
        ratio = 1.0f - ratio;
    }

    // compute the nearest location
    FixedPointCoordinate nearest_location;
    BOOST_ASSERT(!std::isnan(ratio));
    if (ratio <= 0.f)
    { // point is "left" of edge
        nearest_location = source_coordinate;
    }
    else if (ratio >= 1.f)
    { // point is "right" of edge
        nearest_location = target_coordinate;
    }
    else
    { // point lies in between
        nearest_location.lat = static_cast<int>(y2lat(p) * COORDINATE_PRECISION);
        nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION);
    }

    BOOST_ASSERT(nearest_location.is_valid());
    return FixedPointCoordinate::ApproximateEuclideanDistance(point, nearest_location);
}

float FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &segment_source,
                                                         const FixedPointCoordinate &segment_target,
                                                         const FixedPointCoordinate &query_location,
                                                         FixedPointCoordinate &nearest_location,
                                                         float &ratio)
{
    BOOST_ASSERT(query_location.is_valid());

    // initialize values
    const double x = lat2y(query_location.lat / COORDINATE_PRECISION);
    const double y = query_location.lon / COORDINATE_PRECISION;
    const double a = lat2y(segment_source.lat / COORDINATE_PRECISION);
    const double b = segment_source.lon / COORDINATE_PRECISION;
    const double c = lat2y(segment_target.lat / COORDINATE_PRECISION);
    const double d = segment_target.lon / COORDINATE_PRECISION;
    double p, q /*,mX*/, nY;
    if (std::abs(a - c) > std::numeric_limits<double>::epsilon())
    {
        const double m = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m);
        q = b + m * (p - a);
    }
    else
    {
        p = c;
        q = y;
    }
    nY = (d * p - c * q) / (a * d - b * c);

    // discretize the result to coordinate precision. it's a hack!
    if (std::abs(nY) < (1.f / COORDINATE_PRECISION))
    {
        nY = 0.f;
    }

    // compute ratio
    ratio = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need
    // not calculate the explicit values of m an n as we
    // are just interested in the ratio
    if (std::isnan(ratio))
    {
        ratio = (segment_target == query_location ? 1.f : 0.f);
    }
    else if (std::abs(ratio) <= std::numeric_limits<double>::epsilon())
    {
        ratio = 0.f;
    }
    else if (std::abs(ratio - 1.f) <= std::numeric_limits<double>::epsilon())
    {
        ratio = 1.f;
    }

    // compute nearest location
    BOOST_ASSERT(!std::isnan(ratio));
    if (ratio <= 0.f)
    {
        nearest_location = segment_source;
    }
    else if (ratio >= 1.f)
    {
        nearest_location = segment_target;
    }
    else
    {
        // point lies in between
        nearest_location.lat = static_cast<int>(y2lat(p) * COORDINATE_PRECISION);
        nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION);
    }
    BOOST_ASSERT(nearest_location.is_valid());

    const float approximate_distance =
        FixedPointCoordinate::ApproximateEuclideanDistance(query_location, nearest_location);
    BOOST_ASSERT(0. <= approximate_distance);
    return approximate_distance;
}

void FixedPointCoordinate::convertInternalLatLonToString(const int value, std::string &output)
{
    char buffer[12];
    buffer[11] = 0; // zero termination
    output = printInt<11, 6>(buffer, value);
}

void FixedPointCoordinate::convertInternalCoordinateToString(const FixedPointCoordinate &coord,
                                                             std::string &output)
{
    std::string tmp;
    tmp.reserve(23);
    convertInternalLatLonToString(coord.lon, tmp);
    output = tmp;
    output += ",";
    convertInternalLatLonToString(coord.lat, tmp);
    output += tmp;
}

void
FixedPointCoordinate::convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord,
                                                                std::string &output)
{
    std::string tmp;
    tmp.reserve(23);
    convertInternalLatLonToString(coord.lat, tmp);
    output = tmp;
    output += ",";
    convertInternalLatLonToString(coord.lon, tmp);
    output += tmp;
}

void FixedPointCoordinate::Output(std::ostream &out) const
{
    out << "(" << lat / COORDINATE_PRECISION << "," << lon / COORDINATE_PRECISION << ")";
}

float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &first_coordinate,
                                       const FixedPointCoordinate &second_coordinate)
{
    const float lon_diff =
        second_coordinate.lon / COORDINATE_PRECISION - first_coordinate.lon / COORDINATE_PRECISION;
    const float lon_delta = DegreeToRadian(lon_diff);
    const float lat1 = DegreeToRadian(first_coordinate.lat / COORDINATE_PRECISION);
    const float lat2 = DegreeToRadian(second_coordinate.lat / COORDINATE_PRECISION);
    const float y = sin(lon_delta) * cos(lat2);
    const float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon_delta);
    float result = RadianToDegree(std::atan2(y, x));
    while (result < 0.f)
    {
        result += 360.f;
    }

    while (result >= 360.f)
    {
        result -= 360.f;
    }
    return result;
}

float FixedPointCoordinate::GetBearing(const FixedPointCoordinate &other) const
{
    const float lon_delta =
        DegreeToRadian(lon / COORDINATE_PRECISION - other.lon / COORDINATE_PRECISION);
    const float lat1 = DegreeToRadian(other.lat / COORDINATE_PRECISION);
    const float lat2 = DegreeToRadian(lat / COORDINATE_PRECISION);
    const float y_value = std::sin(lon_delta) * std::cos(lat2);
    const float x_value =
        std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
    float result = RadianToDegree(std::atan2(y_value, x_value));

    while (result < 0.f)
    {
        result += 360.f;
    }

    while (result >= 360.f)
    {
        result -= 360.f;
    }
    return result;
}

float FixedPointCoordinate::DegreeToRadian(const float degree)
{
    return degree * (static_cast<float>(M_PI) / 180.f);
}

float FixedPointCoordinate::RadianToDegree(const float radian)
{
    return radian * (180.f * static_cast<float>(M_1_PI));
}

// This distance computation does integer arithmetic only and is a lot faster than
// the other distance function which are numerically correct('ish).
// It preserves some order among the elements that make it useful for certain purposes
int FixedPointCoordinate::OrderedPerpendicularDistanceApproximation(
    const FixedPointCoordinate &input_point,
    const FixedPointCoordinate &segment_source,
    const FixedPointCoordinate &segment_target)
{
    // initialize values
    const float x = static_cast<float>(lat2y(input_point.lat / COORDINATE_PRECISION));
    const float y = input_point.lon / COORDINATE_PRECISION;
    const float a = static_cast<float>(lat2y(segment_source.lat / COORDINATE_PRECISION));
    const float b = segment_source.lon / COORDINATE_PRECISION;
    const float c = static_cast<float>(lat2y(segment_target.lat / COORDINATE_PRECISION));
    const float d = segment_target.lon / COORDINATE_PRECISION;

    float p, q;
    if (a == c)
    {
        p = c;
        q = y;
    }
    else
    {
        const float m = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m);
        q = b + m * (p - a);
    }

    const float nY = (d * p - c * q) / (a * d - b * c);
    float ratio = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need
    // not calculate the explicit values of m an n as we
    // are just interested in the ratio
    if (std::isnan(ratio))
    {
        ratio = (segment_target == input_point) ? 1.f : 0.f;
    }

    // compute target quasi-location
    int dx, dy;
    if (ratio < 0.f)
    {
        dx = input_point.lon - segment_source.lon;
        dy = input_point.lat - segment_source.lat;
    }
    else if (ratio > 1.f)
    {
        dx = input_point.lon - segment_target.lon;
        dy = input_point.lat - segment_target.lat;
    }
    else
    {
        // point lies in between
        dx = input_point.lon - static_cast<int>(q * COORDINATE_PRECISION);
        dy = input_point.lat - static_cast<int>(y2lat(p) * COORDINATE_PRECISION);
    }

    // return an approximation in the plane
    return static_cast<int>(sqrt(dx * dx + dy * dy));
}