// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform 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__

// Take as input a directory of ASTER images. Apply radiometric
// correction to the VNIR_Band3N and VNIR_Band3B images, writing
// <output-prefix>-Band3N.tif and <output-prefix>-Band3B.tif. Generate
// RPC coefficients from input metadata and write
// <output-prefix>-Band3N.xml and <output-prefix>-Band3B.xml. These
// files can then be used as input for stereo with -t rpc.

// References:
// -----------
// ASTER User Handbook Version 2
// https://asterweb.jpl.nasa.gov/content/03_data/04_Documents/aster_user_guide_v2.pdf
//
// IMPROVEMENT OF DEM GENERATION FROM ASTER IMAGES USING SATELLITE
// JITTER ESTIMATION AND OPEN SOURCE IMPLEMENTATION
// Luc Girod, Christopher Nutha, and Andreas Kaab
// http://www.int-arch-photogramm-remote-sens-spatial-inf-sci.net/XL-1-W5/249/2015/isprsarchives-XL-1-W5-249-2015.pdf

#include <vw/FileIO/DiskImageView.h>
#include <vw/Core/StringUtils.h>
#include <vw/Cartography/Datum.h>
#include <vw/Cartography/GeoReference.h>
#include <asp/Core/Common.h>
#include <asp/Core/Macros.h>
#include <asp/Core/FileUtils.h>
#include <asp/Camera/RPCModelGen.h>

#include <limits>
#include <cstring>

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>

namespace fs = boost::filesystem;
namespace po = boost::program_options;

using namespace vw;
using namespace std;
using namespace vw::cartography;

struct Options : public vw::cartography::GdalWriteOptions {
  string input_dir, output_prefix;
  double min_height, max_height;
  int num_samples;
  double penalty_weight;
  Options(): min_height(-1), max_height(-1), num_samples(-1), penalty_weight(-1) {}
};

void handle_arguments(int argc, char *argv[], Options& opt) {
  po::options_description general_options("");
  general_options.add_options()
    ("output-prefix,o",   po::value(&opt.output_prefix), "Specify the output prefix.")
    ("min-height",     po::value(&opt.min_height)->default_value(0),
     "The minimum height (in meters) above the WGS84 datum of the simulation box in which to compute the RPC approximation.")
    ("max-height",     po::value(&opt.max_height)->default_value(8.0e+3),
     "The maximum height (in meters) above the WGS84 datum of the simulation box in which to compute the RPC approximation.")
    ("num-samples",     po::value(&opt.num_samples)->default_value(100),
     "How many samples to use between the minimum and maximum heights.")
    ("penalty-weight",     po::value(&opt.penalty_weight)->default_value(0.1),
     "Penalty weight to use to keep the higher-order RPC coefficients small. Higher penalty weight results in smaller such coefficients.");

  general_options.add( vw::cartography::GdalWriteOptionsDescription(opt) );

  po::options_description positional("");
  positional.add_options()
    ("input_dir", po::value(&opt.input_dir), "An ASTER data directory.");

  po::positional_options_description positional_desc;
  positional_desc.add("input_dir", 1);

  string usage("<input directory> -o <output prefix>");
  bool allow_unregistered = false;
  std::vector<std::string> unregistered;
  po::variables_map vm =
    asp::check_command_line(argc, argv, opt, general_options, general_options,
			    positional, positional_desc, usage,
			    allow_unregistered, unregistered);

  if ( opt.input_dir.empty() )
    vw_throw( ArgumentErr() << "Missing input directory.\n" << usage << general_options );

  if ( opt.output_prefix.empty() )
    vw_throw( ArgumentErr() << "Missing output prefix.\n" << usage << general_options );

  // Create the output directory
  vw::create_out_dir(opt.output_prefix);
}

// See if the input file matches the given pattern. If yes, store the result
// in matched_file. If matched_file is not empty originally, that means
// we have more than one match, which is not good.
bool match_file(std::string const& input_file, std::string const& pattern,
		std::string & matched_file){

  size_t it = input_file.find(pattern);

  // No match
  if (it == std::string::npos)
    return false;

  // Check if it does not match at the end of the string
  if (it + pattern.size() != input_file.size() ) return false;

  // Must match L1A images
  if (input_file.find("L1A") == std::string::npos)
    return false;

  // We expect only one match
  if (matched_file != "")
    vw_throw( ArgumentErr()
	      << "Found two files matching: '" << pattern << "'. Those are: "
	      << matched_file << " and " << input_file << "\n");

  matched_file = input_file;

  return true;
}

// Identify the appropriate inputs in the ASTER directory
void locate_inputs(Options const& opt,
		   std::string& nadir_image,
		   std::string& back_image,
		   std::string& nadir_sat_pos,
		   std::string& back_sat_pos,
		   std::string& nadir_sight_vec,
		   std::string& back_sight_vec,
		   std::string& nadir_corr_table,
		   std::string& back_corr_table,
		   std::string& nadir_longitude,
		   std::string& back_longitude,
		   std::string& nadir_latitude,
		   std::string& back_latitude,
		   std::string& nadir_lattice_point,
		   std::string& back_lattice_point){

  // Iterate through all files
  for (fs::directory_iterator itr(opt.input_dir); itr != fs::directory_iterator(); itr++){

    std::string file = itr->path().string();

    match_file(file, "VNIR_Band3N.ImageData.tif", nadir_image);
    match_file(file, "VNIR_Band3B.ImageData.tif", back_image);

    match_file(file, "VNIR_Band3N.SatellitePosition.txt", nadir_sat_pos);
    match_file(file, "VNIR_Band3B.SatellitePosition.txt", back_sat_pos);

    match_file(file, "VNIR_Band3N.SightVector.txt", nadir_sight_vec);
    match_file(file, "VNIR_Band3B.SightVector.txt", back_sight_vec);

    match_file(file, "VNIR_Band3N.RadiometricCorrTable.txt", nadir_corr_table);
    match_file(file, "VNIR_Band3B.RadiometricCorrTable.txt", back_corr_table);

    match_file(file, "VNIR_Band3N.Longitude.txt", nadir_longitude);
    match_file(file, "VNIR_Band3B.Longitude.txt", back_longitude);

    match_file(file, "VNIR_Band3N.Latitude.txt", nadir_latitude);
    match_file(file, "VNIR_Band3B.Latitude.txt", back_latitude);

    match_file(file, "VNIR_Band3N.LatticePoint.txt", nadir_lattice_point);
    match_file(file, "VNIR_Band3B.LatticePoint.txt", back_lattice_point);
  }

  if (nadir_image == "")
    vw_throw( ArgumentErr() << "Could not locate the nadir-looking camera image "
	      << "(VNIR_Band3N.ImageData.tif).\n");
  if (back_image == "")
    vw_throw( ArgumentErr() << "Could not locate the backward-looking camera image "
	      << "(VNIR_Band3B.ImageData.tif).\n");

  if (nadir_sat_pos == "")
    vw_throw( ArgumentErr() << "Could not locate the nadir-looking satellite position "
	      << "(VNIR_Band3N.SatellitePosition.txt).\n");
  if (back_sat_pos == "")
    vw_throw( ArgumentErr() << "Could not locate the back-looking satellite position "
	      << "(VNIR_Band3B.SatellitePosition.txt).\n");

  if (nadir_sight_vec == "")
    vw_throw( ArgumentErr() << "Could not locate the nadir-looking sight vector "
	      << "(VNIR_Band3N.SightVector.txt).\n");
  if (back_sight_vec == "")
    vw_throw( ArgumentErr() << "Could not locate the back-looking sight vector "
	      << "(VNIR_Band3B.SightVector.txt).\n");

  if (nadir_corr_table == "")
    vw_throw( ArgumentErr() << "Could not locate the nadir-looking radiometric correction table "
	      << "(VNIR_Band3N.RadiometricCorrTable.txt).\n");
  if (back_corr_table == "")
    vw_throw( ArgumentErr() << "Could not locate the back-looking radiometric correction table "
	      << "(VNIR_Band3B.RadiometricCorrTable.txt).\n");

  if (nadir_longitude == "")
    vw_throw( ArgumentErr() << "Could not locate the nadir-looking longitude file "
	      << "(VNIR_Band3N.Longitude.txt).\n");
  if (back_longitude == "")
    vw_throw( ArgumentErr() << "Could not locate the back-looking longitude file "
	      << "(VNIR_Band3B.Longitude.txt).\n");

  if (nadir_latitude == "")
    vw_throw( ArgumentErr() << "Could not locate the nadir-looking latitude file "
	      << "(VNIR_Band3N.Latitude.txt).\n");
  if (back_latitude == "")
    vw_throw( ArgumentErr() << "Could not locate the back-looking latitude file "
	      << "(VNIR_Band3B.Latitude.txt).\n");

  if (nadir_lattice_point == "")
    vw_throw( ArgumentErr() << "Could not locate the nadir-looking lattice point file "
	      << "(VNIR_Band3N.LatticePoint.txt).\n");
  if (back_lattice_point == "")
    vw_throw( ArgumentErr() << "Could not locate the back-looking lattice point file "
	      << "(VNIR_Band3B.LatticePoint.txt).\n");
}

// Apply the radiometric corrections
template <class ImageT>
class RadioCorrectView: public ImageViewBase< RadioCorrectView<ImageT> >{
  ImageT m_img;
  std::vector<Vector3> const& m_corr; // alias
  bool m_has_nodata;
  double m_nodata;

public:
  RadioCorrectView(ImageT const& img, std::vector<Vector3> const& corr,
		   bool has_nodata, double nodata):
    m_img(img), m_corr(corr), m_has_nodata(has_nodata), m_nodata(nodata){}

  typedef typename ImageT::pixel_type input_type;
  typedef float pixel_type;
  typedef float result_type;
  typedef ProceduralPixelAccessor<RadioCorrectView> pixel_accessor;

  inline int32 cols() const { return m_img.cols(); }
  inline int32 rows() const { return m_img.rows(); }
  inline int32 planes() const { return 1; }

  inline pixel_accessor origin() const { return pixel_accessor( *this, 0, 0 ); }

  inline pixel_type operator()( double/*i*/, double/*j*/, int32/*p*/ = 0 ) const {
    vw_throw(NoImplErr() << "RadioCorrectView::operator()(...) is not implemented");
    return pixel_type();
  }

  typedef CropView<ImageView<pixel_type> > prerasterize_type;
  inline prerasterize_type prerasterize(BBox2i const& bbox) const {

    ImageView<input_type> input_tile = crop(m_img, bbox); // to speed things up
    ImageView<result_type> tile(bbox.width(), bbox.height());
    for (int col = bbox.min().x(); col < bbox.max().x(); col++){
      Vector3 C = m_corr[col];
      for (int row = bbox.min().y(); row < bbox.max().y(); row++){
	input_type val = input_tile(col - bbox.min().x(), row - bbox.min().y());
	if (m_has_nodata && val == m_nodata)
	  tile(col - bbox.min().x(), row - bbox.min().y() ) = val;
	else
	  tile(col - bbox.min().x(), row - bbox.min().y() ) = C[1] * val / C[2] + C[0];
      }
    }

    return prerasterize_type(tile, -bbox.min().x(), -bbox.min().y(),
                             cols(), rows() );
  }

  template <class DestT>
  inline void rasterize(DestT const& dest, BBox2i bbox) const {
    vw::rasterize(prerasterize(bbox), dest, bbox);
  }
};
template <class ImageT>
RadioCorrectView<ImageT> radio_correct(ImageT const& img, std::vector<Vector3> const & corr,
				       bool has_nodata, double nodata){
  return RadioCorrectView<ImageT>(img, corr, has_nodata, nodata);
}

// ASTER L1A images come with radiometric corrections appended, but not applied.
// There is one correction per image column.
void apply_radiometric_corrections(Options const& opt,
				   std::string const& input_image,
				   std::string const& corr_table,
				   std::string const& out_image){

  // Extract the corrections
  std::vector<Vector3> corr;
  asp::read_3d_points(corr_table, corr);

  DiskImageView<float> input_img(input_image);

  // Sanity check
  if (input_img.cols() != int(corr.size()) )
    vw_throw( ArgumentErr() << "Expecting as many corrections in " << corr_table
	      << " as image columns in " << input_image << "\n" );

  bool has_nodata = false;
  double nodata   = -std::numeric_limits<float>::max();
  // See if there is a no-data value
  {
    boost::shared_ptr<DiskImageResource> img_rsrc(new DiskImageResourceGDAL(input_image));
    if (img_rsrc->has_nodata_read()){
      has_nodata = true;
      nodata = img_rsrc->nodata_read();
    }
  }

  // No georef, this being L1A imagery
  vw::cartography::GeoReference georef;
  bool has_georef = read_georeference(georef, input_image);
  if (has_georef)
    vw_throw( ArgumentErr() << "ASTER L1A images are not supposed to be georeferenced.\n" );

  vw_out() << "Writing: " << out_image << std::endl;
  vw::cartography::block_write_gdal_image(out_image,
			      radio_correct(input_img, corr, has_nodata, nodata),
			      has_georef, georef,
			      has_nodata, nodata,
			      opt,
			      TerminalProgressCallback("asp", "\t-->: "));
}

// Generate lon-lat-height to image pixel correspondences that we will
// use to create the RPC model.
void generate_point_pairs(// Inputs
                          double min_height, double max_height, int num_samples,
                          double penalty_weight,
                          std::string const& sat_pos_file,
                          std::string const& sight_vec_file,
                          std::string const& longitude_file,
                          std::string const& latitude_file,
                          std::string const& lattice_file,
                          // Outputs
			  std::vector< std::vector<vw::Vector3> > & world_sight_mat,
                          Vector3 & llh_scale, Vector3 & llh_offset,
                          Vector2 & pixel_scale, Vector2 & pixel_offset,
                          Vector<double> & normalized_llh,
                          Vector<double> & normalized_pixels){

  // Read the sight vectors
  std::vector<Vector3> sight_vec;
  asp::read_3d_points(sight_vec_file, sight_vec);

  // Read the satellite positions
  std::vector<Vector3> sat_pos;
  asp::read_3d_points(sat_pos_file, sat_pos);

  int num_rows = sat_pos.size();
  int num_pts = sight_vec.size();
  int num_cols = num_pts / num_rows;

  // Sight vector in world coordinates
  world_sight_mat.clear();
  world_sight_mat.resize(num_rows);

  if (num_rows * num_cols != num_pts)
    vw_throw( ArgumentErr()
	      << "Found " << num_rows << " satellite positions in "
	      << sat_pos_file << " and "
              << num_pts << " sight vectors in " << sight_vec_file
	      << ". The latter must be a multiple of the former.");

  // For each satellite position there many sight vectors
  // corresponding to pixel positions on that image line. Clone the
  // satellite positions to make them one per each sight vector. It is
  // easier to work with things that way later.
  std::vector<Vector3> full_sat_pos(num_pts);
  int count = 0;
  for (int row = 0; row < num_rows; row++) {
    for (int col = 0; col < num_cols; col++) {
      full_sat_pos[count] = sat_pos[row];
      count++;
    }
  }
  if (count != num_pts)
    vw_throw( ArgumentErr() << "Book-keeping failure!\n" );

  std::vector<double> longitude;
  asp::read_1d_points(longitude_file, longitude);
  if (int(longitude.size()) != num_pts)
    vw_throw( ArgumentErr() << "Expecting " << num_pts << " longitude values in "
	      << longitude_file << " but got instead " << longitude.size() << ".\n" );

  std::vector<double> latitude;
  asp::read_1d_points(latitude_file, latitude);
  if (int(latitude.size()) != num_pts)
    vw_throw( ArgumentErr() << "Expecting " << num_pts << " latitude values in "
	      << latitude_file << " but got instead " << latitude.size() << ".\n" );

  // Covert geocentric latitude to geodetic latitude. Pages 60 and 79 of
  // https://asterweb.jpl.nasa.gov/content/03_data/04_Documents/aster_user_guide_v2.pdf
  // Geodetic = Arctan [(tan (Latitude)) / 0.99330562]
  double deg2rad = M_PI/180.0;
  for (size_t i = 0; i < latitude.size(); i++) {
    latitude[i] =  atan (tan (deg2rad*latitude[i]) / 0.99330562)/deg2rad;
  }

  std::vector<Vector2> pixels;
  asp::read_2d_points(lattice_file, pixels);
  if (int(pixels.size()) != num_pts)
    vw_throw( ArgumentErr() << "Expecting " << num_pts << " pixels in "
	      << lattice_file << " but got instead " << pixels.size() << ".\n" );

  // Convert from geodetic coordinates to xyz
  cartography::Datum datum;
  datum.set_well_known_datum("WGS84");
  std::vector<Vector3> ground_xyz(num_pts);
  for (int i = 0; i < num_pts; i++) {
    ground_xyz[i] = datum.geodetic_to_cartesian(Vector3(longitude[i], latitude[i], 0));
  }

  // Create the sight vectors, from the camera center to the ground, in world
  // coordinates, rather than in spacecraft coordinates, like sight_vec.
  for (int pt = 0; pt < num_pts; pt++) {
    Vector3 G = ground_xyz[pt];
    Vector3 C = full_sat_pos[pt];
    int row   = pt/num_cols;
    world_sight_mat[row].push_back( (G-C)/norm_2(G-C) );
  }
  if (world_sight_mat.empty() || (int)world_sight_mat[0].size() != num_cols) {
    vw_throw( ArgumentErr() << "Incorrect number of world sight vectors.\n");
  }

  // Form num_samples layers between min_height and max_height.
  // Each point there will have its corresponding pixel value.
  int num_total_pts = num_pts*num_samples;
  std::vector<Vector3> all_llh (num_total_pts);
  std::vector<Vector2> all_pixels(num_total_pts);
  count = 0;
  for (int sample = 0; sample < num_samples; sample++) {
    double height = min_height
      + double(sample)*(max_height - min_height)/(num_samples - 1.0);

    // Find an xyz position at roughly that height on the line
    // connecting the original ground point and the satellite
    // center. We need to solve a quadratic equation for that. We
    // assume the Earth is a sphere.
    for (int pt = 0; pt < num_pts; pt++) {

      Vector3 A = ground_xyz[pt];
      Vector3 B = full_sat_pos[pt];
      Vector3 D = B - A;

      // Find t such that norm(A + t*D) = norm(A) + height
      double  d = dot_prod(A, D) * dot_prod(A, D)
        + dot_prod(D, D) * (height*height + 2*norm_2(A)*height);
      double  t = ( -dot_prod(A, D) + sqrt(d) ) / dot_prod(D, D);
      Vector3 P = A + t*D;

      all_llh[count]  = datum.cartesian_to_geodetic(P);
      all_pixels[count] = pixels[pt];
      count++;
    }
  }

  // Find the range of lon-lat-heights
  BBox3 llh_box;
  for (size_t i = 0; i < all_llh.size(); i++)
    llh_box.grow(all_llh[i]);

  // Find the range of pixels
  BBox2 pixel_box;
  for (size_t i = 0; i < all_pixels.size(); i++)
    pixel_box.grow(all_pixels[i]);

  llh_scale  = (llh_box.max() - llh_box.min())/2.0; // half range
  llh_offset = (llh_box.max() + llh_box.min())/2.0; // center point

  pixel_scale  = (pixel_box.max() - pixel_box.min())/2.0; // half range
  pixel_offset = (pixel_box.max() + pixel_box.min())/2.0; // center point

  normalized_llh.set_size(asp::RPCModel::GEODETIC_COORD_SIZE*num_total_pts);
  normalized_pixels.set_size(asp::RPCModel::IMAGE_COORD_SIZE*num_total_pts
                             + asp::RpcSolveLMA::NUM_PENALTY_TERMS);
  for (size_t i = 0; i < normalized_pixels.size(); i++) {
    // Important: The extra penalty terms are all set to zero here.
    normalized_pixels[i] = 0.0;
  }

  // Form the arrays of normalized pixels and normalized llh
  for (int pt = 0; pt < num_total_pts; pt++) {

    // Normalize the pixel to -1 <> 1 range
    Vector3 llh_n   = elem_quot(all_llh[pt]    - llh_offset,   llh_scale);
    Vector2 pixel_n = elem_quot(all_pixels[pt] - pixel_offset, pixel_scale);

    subvector(normalized_llh, asp::RPCModel::GEODETIC_COORD_SIZE*pt,
              asp::RPCModel::GEODETIC_COORD_SIZE) = llh_n;
    subvector(normalized_pixels, asp::RPCModel::IMAGE_COORD_SIZE*pt,
              asp::RPCModel::IMAGE_COORD_SIZE   ) = pixel_n;

  }

  return;
}

// Save an XML file having all RPC information
void save_xml(int image_cols, int image_rows,
	      std::vector< std::vector<Vector2> > const& lattice_mat,
	      std::vector< std::vector<Vector3> > const& sight_mat,
	      std::vector< std::vector<Vector3> > const& world_sight_mat,
	      std::vector<Vector3>                const& sat_pos,
	      Vector3 const& llh_scale,
              Vector3 const& llh_offset,
              Vector2 const& pixel_scale,
              Vector2 const& pixel_offset,
              asp::RPCModel::CoeffVec const& line_num,
              asp::RPCModel::CoeffVec const& line_den,
              asp::RPCModel::CoeffVec const& samp_num,
              asp::RPCModel::CoeffVec const& samp_den,
              std::string const& out_cam_file){

  std::string lineoffset   = vw::num_to_str(pixel_offset.y());
  std::string sampoffset   = vw::num_to_str(pixel_offset.x());
  std::string latoffset    = vw::num_to_str(llh_offset.y());
  std::string longoffset   = vw::num_to_str(llh_offset.x());
  std::string heightoffset = vw::num_to_str(llh_offset.z());

  std::string linescale   = vw::num_to_str(pixel_scale.y());
  std::string sampscale   = vw::num_to_str(pixel_scale.x());
  std::string latscale    = vw::num_to_str(llh_scale.y());
  std::string longscale   = vw::num_to_str(llh_scale.x());
  std::string heightscale = vw::num_to_str(llh_scale.z());

  std::string linenumcoef = vw::vec_to_str(line_num);
  std::string linedencoef = vw::vec_to_str(line_den);
  std::string sampnumcoef = vw::vec_to_str(samp_num);
  std::string sampdencoef = vw::vec_to_str(samp_den);

  vw_out() << "Writing: " << out_cam_file << std::endl;
  std::ofstream ofs(out_cam_file.c_str());

  ofs << "<isd>\n";

  // Rigorous camera model

  // Lattice points
  ofs << "    <LATTICE_POINT>\n";
  for (size_t row = 0; row < lattice_mat.size(); row++) {
    for (size_t col = 0; col < lattice_mat[row].size(); col++) {
      ofs << vw::vec_to_str(lattice_mat[row][col]) << std::endl;
    }
    ofs << std::endl;
  }
  ofs << "    </LATTICE_POINT>\n";

  // Sight vector
  ofs << "    <SIGHT_VECTOR>\n";
  for (size_t row = 0; row < sight_mat.size(); row++) {
    for (size_t col = 0; col < sight_mat[row].size(); col++) {
      ofs << vw::vec_to_str(sight_mat[row][col]) << std::endl;
    }
    ofs << std::endl;
  }
  ofs << "    </SIGHT_VECTOR>\n";

  // Sight vector in world coordinates
  ofs << "    <WORLD_SIGHT_VECTOR>\n";
  for (size_t row = 0; row < world_sight_mat.size(); row++) {
    for (size_t col = 0; col < world_sight_mat[row].size(); col++) {
      ofs << vw::vec_to_str(world_sight_mat[row][col]) << std::endl;
    }
    ofs << std::endl;
  }
  ofs << "    </WORLD_SIGHT_VECTOR>\n";

  // Satellite position
  ofs << "    <SAT_POS>\n";
  for (size_t row = 0; row < sat_pos.size(); row++) {
    ofs << vw::vec_to_str(sat_pos[row]) << std::endl;
  }
  ofs << "    </SAT_POS>\n";

  // Image size
  ofs << "    <IMAGE_COLS>" << image_cols << "</IMAGE_COLS>\n";
  ofs << "    <IMAGE_ROWS>" << image_rows << "</IMAGE_ROWS>\n";

  // RPC
  ofs << "    <RPB>\n";
  ofs << "        <SATID>ASTER_L1A_VNIR_Band3</SATID>\n";
  ofs << "        <IMAGE>\n";
  ofs << "            <LINEOFFSET>"      << lineoffset   << "</LINEOFFSET>\n";
  ofs << "            <SAMPOFFSET>"      << sampoffset   << "</SAMPOFFSET>\n";
  ofs << "            <LATOFFSET>"       << latoffset    << "</LATOFFSET>\n";
  ofs << "            <LONGOFFSET>"      << longoffset   << "</LONGOFFSET>\n";
  ofs << "            <HEIGHTOFFSET>"    << heightoffset << "</HEIGHTOFFSET>\n";
  ofs << "            <LINESCALE>"       << linescale    << "</LINESCALE>\n";
  ofs << "            <SAMPSCALE>"       << sampscale    << "</SAMPSCALE>\n";
  ofs << "            <LATSCALE>"        << latscale     << "</LATSCALE>\n";
  ofs << "            <LONGSCALE>"       << longscale    << "</LONGSCALE>\n";
  ofs << "            <HEIGHTSCALE>"     << heightscale  << "</HEIGHTSCALE>\n";
  ofs << "            <LINENUMCOEFList>\n";
  ofs << "                <LINENUMCOEF>" << linenumcoef  << "</LINENUMCOEF>\n";
  ofs << "            </LINENUMCOEFList>\n";
  ofs << "            <LINEDENCOEFList>\n";
  ofs << "                <LINEDENCOEF>" << linedencoef  << "</LINEDENCOEF>\n";
  ofs << "            </LINEDENCOEFList>\n";
  ofs << "            <SAMPNUMCOEFList>\n";
  ofs << "                <SAMPNUMCOEF>" << sampnumcoef  << "</SAMPNUMCOEF>\n";
  ofs << "            </SAMPNUMCOEFList>\n";
  ofs << "            <SAMPDENCOEFList>\n";
  ofs << "                <SAMPDENCOEF>" << sampdencoef  << "</SAMPDENCOEF>\n";
  ofs << "            </SAMPDENCOEFList>\n";
  ofs << "        </IMAGE>\n";
  ofs << "    </RPB>\n";
  ofs << "</isd>\n";
  ofs.close();
}

// Create XML files containing rigorous camera info, and compute the RPC coefficients as well.
void gen_xml(double min_height, double max_height, int num_samples,
	     double penalty_weight,
	     std::string const& image_file,
	     std::string const& sat_pos_file,
	     std::string const& sight_vec_file,
	     std::string const& longitude_file,
	     std::string const& latitude_file,
	     std::string const& lattice_file,
	     std::string const& out_cam_file){

  std::vector< std::vector<Vector2> > lattice_mat;
  asp::read_matrix_from_file(lattice_file, lattice_mat);

  std::vector< std::vector<Vector3> > sight_mat;
  asp::read_matrix_from_file(sight_vec_file, sight_mat);

  if (lattice_mat.empty() || sight_mat.empty()     ||
      lattice_mat.size()     != sight_mat.size()   ||
      lattice_mat[0].size() != sight_mat[0].size() ) {
    vw_throw( ArgumentErr() << "Inconsistent lattice point and sight vector information.\n");
  }

  // Read the satellite positions
  std::vector<Vector3> sat_pos;
  asp::read_3d_points(sat_pos_file, sat_pos);
  if (sat_pos.size() != sight_mat.size())
    vw_throw( ArgumentErr() << "Inconsistent satellite position and sight vector information.\n");

  Vector3 llh_scale, llh_offset;
  Vector2 pixel_scale, pixel_offset;
  Vector<double> normalized_llh;
  Vector<double> normalized_pixels;
  std::vector< std::vector<vw::Vector3> > world_sight_mat; // sight dir in world coords
  generate_point_pairs(// inputs
                       min_height, max_height, num_samples, penalty_weight,
                       sat_pos_file, sight_vec_file,
                       longitude_file, latitude_file, lattice_file,
                       // Outputs
		       world_sight_mat,
                       llh_scale, llh_offset,
                       pixel_scale, pixel_offset,
                       normalized_llh,
                       normalized_pixels);

  // Find the RPC coefficients
  asp::RPCModel::CoeffVec line_num, line_den, samp_num, samp_den;
  std::string output_prefix = "";
  asp::gen_rpc(// Inputs
               penalty_weight, output_prefix,
               normalized_llh, normalized_pixels,
               llh_scale, llh_offset, pixel_scale, pixel_offset,
               // Outputs
               line_num, line_den, samp_num, samp_den);

#if 0
  // Dump the output to stdout
  asp::print_vec("pixel_scale",  pixel_scale );
  asp::print_vec("pixel_offset", pixel_offset);
  asp::print_vec("llh_scale",    llh_scale   );
  asp::print_vec("llh_offset",   llh_offset  );
  asp::print_vec("line_num",     line_num    );
  asp::print_vec("line_den",     line_den    );
  asp::print_vec("samp_num",     samp_num    );
  asp::print_vec("samp_den",     samp_den    );
#endif

  int image_cols, image_rows;
  DiskImageView<float> input_img(image_file);
  image_cols = input_img.cols();
  image_rows = input_img.rows();

  save_xml(image_cols, image_rows, lattice_mat,
	   sight_mat, world_sight_mat, sat_pos,
	   llh_scale, llh_offset, pixel_scale, pixel_offset,
           line_num, line_den, samp_num, samp_den,
           out_cam_file);
}

int main( int argc, char *argv[] ) {

  Options opt;
  try {
    handle_arguments(argc, argv, opt);

    std::string nadir_image, back_image, nadir_sat_pos, back_sat_pos;
    std::string nadir_sight_vec, back_sight_vec;
    std::string nadir_corr_table, back_corr_table;
    std::string nadir_longitude, back_longitude;
    std::string nadir_latitude, back_latitude;
    std::string nadir_lattice_point, back_lattice_point;

    locate_inputs(opt, nadir_image, back_image, nadir_sat_pos, back_sat_pos,
		  nadir_sight_vec, back_sight_vec, nadir_corr_table, back_corr_table,
		  nadir_longitude, back_longitude, nadir_latitude, back_latitude,
		  nadir_lattice_point, back_lattice_point);

    std::string out_nadir_image = opt.output_prefix + "-Band3N.tif";
    std::string out_back_image  = opt.output_prefix + "-Band3B.tif";

    std::string out_nadir_cam = opt.output_prefix + "-Band3N.xml";
    std::string out_back_cam  = opt.output_prefix + "-Band3B.xml";

#if 0
    std::cout 	<< "nadir_image             " 	<< nadir_image 		<< std::endl;
    std::cout 	<< "back_image              " 	<< back_image 		<< std::endl;
    std::cout 	<< "nadir_sat_pos           " 	<< nadir_sat_pos 	<< std::endl;
    std::cout 	<< "back_sat_pos            " 	<< back_sat_pos 	<< std::endl;
    std::cout 	<< "nadir_sight_vec         " 	<< nadir_sight_vec 	<< std::endl;
    std::cout 	<< "back_sight_vec          " 	<< back_sight_vec 	<< std::endl;
    std::cout 	<< "nadir_corr_table        " 	<< nadir_corr_table 	<< std::endl;
    std::cout 	<< "back_corr_table         " 	<< back_corr_table 	<< std::endl;
    std::cout 	<< "nadir_longitude         " 	<< nadir_longitude 	<< std::endl;
    std::cout 	<< "back_longitude          " 	<< back_longitude 	<< std::endl;
    std::cout 	<< "nadir_latitude          " 	<< nadir_latitude 	<< std::endl;
    std::cout 	<< "back_latitude           " 	<< back_latitude 	<< std::endl;
    std::cout 	<< "nadir_lattice_point     " 	<< nadir_lattice_point 	<< std::endl;
    std::cout 	<< "back_lattice_point      " 	<< back_lattice_point 	<< std::endl;
    std::cout 	<< "output nadir image:     " 	<< out_nadir_image 	<< ' '
              	<< out_nadir_cam 		<< std::endl;
    std::cout 	<< "output back image:      " 	<< out_back_image  	<< ' '
              	<< out_back_cam  		<< std::endl;
#endif

    gen_xml(opt.min_height, opt.max_height, opt.num_samples, opt.penalty_weight,
	    nadir_image, nadir_sat_pos, nadir_sight_vec, nadir_longitude, nadir_latitude,
	    nadir_lattice_point, out_nadir_cam);

    gen_xml(opt.min_height, opt.max_height, opt.num_samples, opt.penalty_weight,
	    back_image, back_sat_pos, back_sight_vec, back_longitude, back_latitude,
	    back_lattice_point, out_back_cam);

    apply_radiometric_corrections(opt, nadir_image, nadir_corr_table, out_nadir_image);
    apply_radiometric_corrections(opt, back_image,  back_corr_table,  out_back_image);

  } ASP_STANDARD_CATCHES;

  return 0;
}


/**
 * @file robot.cpp
 * @author Kavyashree Devadiga (kavya@umd.edu), Aswath Muthuselvam
 * (aswath@umd.edu)
 * @brief File for robot class method definitions
 * @version 0.1
 * @date 2021-10-05
 * @copyright All rights reserved
 *
 */

#include "robot.h"  // NOLINT

#include <unistd.h>

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <vector>

#include "simulator.h"  // NOLINT
#include "solver.h"     //NOLINT

using Eigen::Matrix4d;
using Eigen::MatrixXd;
using std::cin;
using std::cout;
using std::endl;
using std::vector;

Robot::Robot(Simulator* simulator_) {
  simulator = simulator_;
  ///< Variable to store origin
  char origin_name[] = "Floor";

  ///< Variable to store robot joint names
  char joint_names[6][11] = {"UR5_joint1", "UR5_joint2", "UR5_joint3",
                             "UR5_joint4", "UR5_joint5", "UR5_joint6"};

  ///< Variable to store robot link names
  char link_names[6][11] = {"UR5_link1", "UR5_link2", "UR5_link3",
                            "UR5_link4", "UR5_link5", "UR5_link6"};

  ///< Assign IDs values to handles
  origin_handle = simulator->GetObjectHandle(origin_name);

  for (int it = 0; it < 6; it++) {
    joint_handle.push_back(simulator->GetObjectHandle(joint_names[it]));
    link_handle.push_back(simulator->GetObjectHandle(link_names[it]));

    // cout << "\n------Joint matrix " << it + 1 << " --------\n";
    // float* joint_ptr = new float[12];
    // joint_matrix.push_back(joint_ptr);
    // simulator->GetJointMatrix(joint_handle[it], joint_matrix[it]);

    // cout << "\n------Link matrix " << it << " --------\n";
    // float* link_ptr = new float[12];
    // link_matrix.push_back(link_ptr);
    // simulator->GetJointMatrix(link_handle[it], link_matrix[it]);
  }

  solver = new Solver(simulator, joint_handle);
}

bool Robot::Initialize() {
  State state;
  try {
    float theta = 0.5;
    //< Perform some actions by commanding joint angles(in radians).
    Controller(theta, theta, theta, theta, theta, theta);

    sleep(1);

    //< Get positions of objects(joints in this case).
    simxFloat position[3];
    simxFloat orientation[3];

    ChainTransformations();

    for (int it = 0; it < 6; it++) {
      cout << "\n------Joint position of " << it + 1
           << " [x, y, x] wrt to Origin --------\n";
      simulator->GetObjectPosition(joint_handle[it], origin_handle, position);
      simulator->GetObjectOrientation(joint_handle[it], origin_handle,
                                      orientation);
    }

    cout << "\n---Joint position and orientation of 1"
         << "  wrt to "
         << " orgin is: "
         << "---\n";
    simulator->GetObjectPosition(joint_handle[0], origin_handle, position);
    simulator->GetObjectOrientation(joint_handle[0], origin_handle,
                                    orientation);
    for (int it = 1; it < 6; it++) {
      cout << "\n---Joint position and orientation of " << it + 1 << "  wrt to "
           << it << " th joint"
           << "---\n";
      simulator->GetObjectPosition(joint_handle[it], joint_handle[it - 1],
                                   position);
      simulator->GetObjectOrientation(joint_handle[it], joint_handle[it - 1],
                                      orientation);
    }

    //< Set a Joint Position.
    // simulator->SetJointPosition(joint_handle[0], position);
    // cout << position[0];
    return true;
  } catch (const char* msg) { /* catch exception if any */
    std::cout << "Exception occurred" << std::endl;
    return false;
  }
}

void Robot::ChainTransformations() {
  cout << "\nTransfomation matrix chain from Origin to End Effector:" << endl;
  double t1, t2, t3, t4, t5;  // t6;
  double a1 = -0.0703083;
  double d1 = 0.0660392;
  double a2 = 0.425103;
  // double d2 = 0;
  double a3 = 0.392149;
  double a4 = 0.0455737;
  double d4 = 0.0397052;
  double d5 = 0.0491754;
  double a5 = 0.0144247;

  t1 = simulator->GetJointAngle(joint_handle[0]);
  t2 = simulator->GetJointAngle(joint_handle[1]);
  t3 = simulator->GetJointAngle(joint_handle[2]);
  t4 = simulator->GetJointAngle(joint_handle[3]);
  t5 = simulator->GetJointAngle(joint_handle[4]);
  // t6 = simulator->GetJointAngle(joint_handle[5]);

  // Vector3f ot; << 0.3, -0.275, 0.04315;
  Matrix4d T(4, 4);
  T << 1, 0, 0, 0.3, 0, 1, 0, -0.275, 0, 0, 1, 0.04315, 0, 0, 0, 1;  // j1

  Matrix4d Tn(4, 4);

  Tn << cos(t1), -sin(t1), 0, a1, sin(t1), cos(t1), 0, 1.2219E-06, 0, 0, 1, d1,
      0, 0, 0, 1;
  T = T * Tn;

  // T = T*[roty(-90) z'; z 1]*[rotz(-90) z'; z 1]; %j2
  /*
  Tn << 0, 0, -1, 0,
  -1, 0, 0, 0,
  0, 1, 0, 0,
  0, 0, 0, 1;
  */

  // T = T*[roty(-90) z'; z 1]
  Tn << 0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;

  T = T * Tn;  // j2

  Tn << cos(t2), -sin(t2), 0, a2, sin(t2), cos(t2), 0, 1.74344e-05, 0, 0, 1,
      9.50694e-06, 0, 0, 0, 1;
  T = T * Tn;  // j3

  Tn << cos(t3), -sin(t3), 0, a3, sin(t3), cos(t3), 0, -1.60933e-06, 0, 0, 1,
      1.40071e-06, 0, 0, 0, 1;
  T = T * Tn;  // j4

  Tn << cos(t4), -sin(t4), 0, a4, sin(t4), cos(t4), 0, 2.95043e-06, 0, 0, 1, d4,
      0, 0, 0, 1;
  T = T * Tn;
  // T = T*[roty(90) [0 0 0]'; z 1]; %j5
  Tn << 0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0, 0, 0, 0, 1;
  T = T * Tn;

  // T = T*[Rz(t5) [a5 0 d5]'; z 1]; %j6
  Tn << cos(t5), -sin(t5), 0, a5, sin(t5), cos(t5), 0, 3.27826e-07, 0, 0, 1, d5,
      0, 0, 0, 1;
  T = T * Tn;
  // T = T*[roty(-90) [0 0 0]'; z 1];
  Tn << 0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
  T = T * Tn;

  GetEndEffectorPosition();
  float error = pow(pow(state.x - T(0, 3), 2) + pow(state.y - T(1, 3), 2) +
                        pow(state.z - T(2, 3), 2),
                    0.5);
  cout << "\nTransformation matrix is: \n" << T << endl;
  cout << " Error is: " << error;
}

void Robot::GetEndEffectorPosition() {
  simxFloat position[3];
  simulator->GetObjectPosition(joint_handle[5], origin_handle, position);
  state.x = position[0];
  state.y = position[1];
  state.z = position[2];
}

vector<double> Robot::TrajectoryPlanner(double goal_x, double goal_y,
                                        double goal_z) {
  GetEndEffectorPosition();
  std::vector<double> dx_dy_dz = {(goal_x - state.x) * 0.005,
                                  (goal_y - state.y) * 0.005,
                                  (goal_z - state.z) * 0.005};
  return dx_dy_dz;
}

bool Robot::Controller(float t1, float t2, float t3, float t4, float t5,
                       float t6) {
  simulator->SetJointTargetAngle(joint_handle[0], t1);
  simulator->SetJointTargetAngle(joint_handle[1], t2);
  simulator->SetJointTargetAngle(joint_handle[2], t3);
  simulator->SetJointTargetAngle(joint_handle[3], t4);
  simulator->SetJointTargetAngle(joint_handle[4], t5);
  simulator->SetJointTargetAngle(joint_handle[5], t6);
  return true;
}

bool Robot::Solve(double goal_x, double goal_y, double goal_z) {
  double start_x, start_y, start_z;
  GetEndEffectorPosition();
  start_x = state.x;
  start_y = state.y;
  start_z = state.z;
  double dxt = 0, dyt = 0, dzt = 0;
  float absolute_error =
      pow(pow((state.x - goal_x), 2) + pow((state.y - goal_y), 2) +
              pow((state.z - goal_z), 2),
          0.5);
  float threshold = 0.3;
  double t1, t2, t3, t4, t5, t6;

  while (absolute_error > threshold) {

    std::vector<double> dx_dy_dz = TrajectoryPlanner(goal_x, goal_y, goal_z);
    dxt += dx_dy_dz[0];
    dyt += dx_dy_dz[1];
    dzt += dx_dy_dz[2];
    t1 = simulator->GetJointAngle(joint_handle[0]);
    t2 = simulator->GetJointAngle(joint_handle[1]);
    t3 = simulator->GetJointAngle(joint_handle[2]);
    t4 = simulator->GetJointAngle(joint_handle[3]);
    t5 = simulator->GetJointAngle(joint_handle[4]);
    t6 = simulator->GetJointAngle(joint_handle[5]);

    MatrixXd q_(4, 1);
    bool status = solver->PerformIK(dx_dy_dz[0], dx_dy_dz[1], dx_dy_dz[2], &q_);
    if (!status) {
      cout << "IK calculation failed, trying again in next iteration." << endl;
      Controller(t1 + 0.0001, t2 + 0.0001, t3 + 0.0001, t4 + 0.0001,
                 t5 + 0.0001, t6 + 0.0001);
      continue;
    }
    cout << "\n\nQ is: " << q_.transpose() << endl;
    t1 += q_(0, 0);
    t2 += q_(1, 0);
    t3 += q_(2, 0);
    t4 += q_(3, 0);
    float error = pow(pow((state.x - start_x + dxt), 2) +
                          pow((state.y - start_y + dyt), 2) +
                          pow((state.z - start_z + dzt), 2),
                      0.5);
    cout << "Error from current desired position: " << error << endl;
    if (solver->IsErrorTolerable(error)) {
      cout << "Error is within tolerable limits" << endl;
    }
    Controller(t1, t2, t3, t4, t5, t6);
    absolute_error =
        pow(pow((state.x - goal_x), 2) + pow((state.y - goal_y), 2) +
                pow((state.z - goal_z), 2),
            0.5);
    cout << "Distance from goal position: " << absolute_error << endl;
    sleep(0.3);

  }

  cout << "Successfully reached goal position! " << absolute_error << endl;

  return true;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110221    K. Kumar          File created.
 *      110224    K. Kumar          Renamed class and file.
 *      110629    L. van der Ham    Added circular coplanar case.
 *      110803    L. van der Ham    Created base class and seperated approximatePlanetPositions
 *                                  from approximatePlanetPositionsCircularCoplanar.
 *      120322    D. Dirkx          Modified to new Ephemeris interfaces.
 *
 *    References
 *      Standish, E.M. Keplerian Elements for Approximate Positions of the Major Planets,
 *          http://ssd.jpl.nasa.gov/txt/aprx_pos_planets.pdf, last accessed: 24 February, 2011.
 *
 *    Notes
 *
 */

#include <iostream>

#include <boost/exception/all.hpp>
#include <boost/format.hpp>
#include <boost/throw_exception.hpp>

#include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositionsBase.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace ephemerides
{

//! Set planet.
void ApproximatePlanetPositionsBase::setPlanet( BodiesWithEphemerisData bodyWithEphemerisData )
{
    // Check if ephemeris data has been loaded, and reload data if not.
    if ( containerOfDataFromEphemerisFile_.size( ) == 0 )
    {
        reloadData( );
    }

    switch ( bodyWithEphemerisData )
    {
    case mercury:

        parseEphemerisLineData_( 18 );
        break;

    case venus:

        parseEphemerisLineData_( 20 );
        break;

    case earthMoonBarycenter:

        parseEphemerisLineData_( 22 );
        break;

    case mars:

        parseEphemerisLineData_( 24 );
        break;

    case jupiter:

        parseEphemerisLineData_( 26 );
        parseExtraTermsEphemerisLineData_( 48 );
        break;

    case saturn:

        parseEphemerisLineData_( 28 );
        parseExtraTermsEphemerisLineData_( 49 );
        break;

    case uranus:

        parseEphemerisLineData_( 30 );
        parseExtraTermsEphemerisLineData_( 50 );
        break;

    case neptune:

        parseEphemerisLineData_( 32 );
        parseExtraTermsEphemerisLineData_( 51 );
        break;

    case pluto:

        parseEphemerisLineData_( 34 );
        parseExtraTermsEphemerisLineData_( 52 );
        break;

    default:
        break;
    }
}

//! Parse ephemeris line data.
void ApproximatePlanetPositionsBase::parseEphemerisLineData_( const unsigned int& firstLineNumber )
{
    // Parse data from container of strings using a string stream.

    // Clear stringstream.
    ephemerisLineData_.clear( );

    // Read first line of data.
    ephemerisLineData_ << containerOfDataFromEphemerisFile_[ firstLineNumber ];

    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.planetName_;

    // Check if the line number corresponds to that for "EM Bary".
    if ( firstLineNumber == 22 )
    {
        std::string earthMoonBarycenter_;

        ephemerisLineData_ >> earthMoonBarycenter_;
        approximatePlanetPositionsDataContainer_.planetName_ += " " + earthMoonBarycenter_;
    }

    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.semiMajorAxis_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.eccentricity_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.inclination_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.meanLongitude_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.longitudeOfPerihelion_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.longitudeOfAscendingNode_;

    // Clear stringstream.
    ephemerisLineData_.clear( );

    // Read second line of data.
    ephemerisLineData_ << containerOfDataFromEphemerisFile_[ firstLineNumber + 1 ];

    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.rateOfChangeOfSemiMajorAxis_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.rateOfChangeOfEccentricity_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.rateOfChangeOfInclination_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.rateOfChangeOfMeanLongitude_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_
                          .rateOfChangeOfLongitudeOfPerihelion_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_
                          .rateOfChangeOfLongitudeOfAscendingNode_;
}

//! Parse line data for extra terms for ephemeris.
void ApproximatePlanetPositionsBase::parseExtraTermsEphemerisLineData_(
    const unsigned int& lineNumber )
{
    // Clear stringstream.
    ephemerisLineData_.clear( );

    // Read second line of data.
    ephemerisLineData_<< containerOfDataFromEphemerisFile_[ lineNumber ];

    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.planetName_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.additionalTermB_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.additionalTermC_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.additionalTermS_;
    ephemerisLineData_ >> approximatePlanetPositionsDataContainer_.additionalTermF_;
}

//! Load in ephemeris data for planets.
void ApproximatePlanetPositionsBase::reloadData( )
{
    // Set  path to ephemeris file in file reader.
    std::string filePath_ = input_output::getTudatRootPath( ) +
            "External/EphemerisData/p_elem_t2.txt";

    // Open ephemeris file.
    std::ifstream ephemerisFile_( filePath_.c_str( ) );
    if ( ephemerisFile_.fail( ) )
    {
        boost::throw_exception(
                    boost::enable_error_info(
                        std::runtime_error(
                            boost::str( boost::format( "Data file '%s' could not be opened." )
                                 % filePath_.c_str( ) ) ) )
            << boost::errinfo_file_name( filePath_.c_str( ) )
            << boost::errinfo_file_open_mode( "std::ios::binary" )
            << boost::errinfo_api_function( "std::ifstream::open" ) );
    }

    // Read the file into a container.
    for ( int line = 1; line < 53; line++ )
    {
        std::string lineData;
        getline( ephemerisFile_, lineData );
        containerOfDataFromEphemerisFile_[line] = lineData;
        if ( ephemerisFile_.fail( ) )
        {
            break;
        }
    }

    // Close file.
    ephemerisFile_.close( );
}

} // namespace ephemerides
} // namespace tudat


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "transposition_to_circuit.hpp"

#include <boost/assign/std/vector.hpp>

#include "../circuit.hpp"
#include "../gate.hpp"

#include "add_circuit.hpp"
#include "add_gates.hpp"
#include "reverse_circuit.hpp"

using namespace boost::assign;

namespace cirkit
{
  bool transposition_to_circuit( circuit& circ, const boost::dynamic_bitset<>& inputs, const boost::dynamic_bitset<>& outputs )
  {
    assert( inputs.size() == outputs.size() );
    assert( circ.lines() == inputs.size() );

    unsigned b = 0u, bs = 0u;
    circuit circ_block_a( inputs.size() );
    circuit circ_block_b( inputs.size() );
    circuit circ_block_bs( inputs.size() );
    circuit circ_block_c( inputs.size() );
    circuit circ_block_X( inputs.size() );
    append_not( circ_block_X, 0u );
    gate::control_container controls;
    unsigned target;

    for ( unsigned j = 0u; j < inputs.size(); ++j )
    {
      target = inputs.size() - 1u - j;
      if ( !inputs.test( j ) && !outputs.test( j ) )
      {
        append_not( circ_block_a, target );
      }
      else if ( inputs.test( j ) && !outputs.test( j ) )
      {
        controls.clear();
        append_not( circ_block_b, target );
        ++b;
        append_not( circ_block_c, target );
        for ( unsigned k = 0u; k < inputs.size(); ++k )
        {
          if ( k != target )
          {
            controls += make_var( k );
          }
        }
        append_toffoli( circ_block_c, controls, target );
        circ_block_X.remove_gate_at( 0u );
        append_toffoli( circ_block_X, controls, target );
      }
      else if ( !inputs.test( j ) && outputs.test( j ) )
      {
        controls.clear();
        append_not( circ_block_bs, target );
        ++bs;
        append_not( circ_block_c, target );
        for ( unsigned k = 0u; k < inputs.size(); ++k )
        {
          if ( k != target )
          {
            controls += make_var( k );
          }
        }
        append_toffoli( circ_block_c, controls, target );
        circ_block_X.remove_gate_at( 0u );
        append_toffoli( circ_block_X, controls, target );
      }
    }

    circ_block_c.remove_gate_at( circ_block_c.num_gates() - 1 );
    circ_block_c.remove_gate_at( circ_block_c.num_gates() - 1 );

    append_circuit( circ, circ_block_a );
    if ( b < bs )
    {
      append_circuit( circ, circ_block_b );
    }
    else
    {
      append_circuit( circ, circ_block_bs );
    }
    append_circuit( circ, circ_block_c );

    append_circuit( circ, circ_block_X );

    reverse_circuit( circ_block_c );
    append_circuit( circ, circ_block_c );

    if ( b < bs )
    {
      append_circuit( circ, circ_block_b );
    }
    else
    {
      append_circuit( circ, circ_block_bs );
    }
    append_circuit( circ, circ_block_a );

    return true;
  }
}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#ifndef __PARTICLEFILTER_HPP__
#define __PARTICLEFILTER_HPP__

#include <vector>
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <std_msgs/Bool.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
#include <vector>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <time.h>
#include <math.h>
#include <sewer_graph/sewer_graph.h>
#include <fstream>
#include <wall_detector/WallInfo.h>
#include <cmath>
#include <manhole_detector/Manhole.h>
#include <std_msgs/Float32.h>

// Particle filter Update type
enum UpdateType {
  REGULAR, MANHOLE, FORK, ANGULAR, YAW, GROUND_TRUTH, UNKNOWN = 100
};

//Struct that contains the data concerning one Particle
struct Particle
{
  //Position
  float x;
  float y;

  // Yaw angle (with respect to MAP frame)
  float a;

  // Weight (One weight)
  float w;
};

//Class definition
class ParticleFilter
{
public:

  //!Default contructor
  ParticleFilter(std::string &node_name) : lnh("~"),
  m_randVar(boost::mt19937(time(0)), boost::normal_distribution<>(0, 0.4)), s_g(NULL)
  {
    // Setup random number generator from GSL
    gsl_rng_env_setup();
    m_randomType = gsl_rng_default;
    m_randomValue = gsl_rng_alloc(m_randomType);

    // Read node parameters
    if(!lnh.getParam("base_frame_id", m_baseFrameId))
      m_baseFrameId = "base_link_vo";
    if(!lnh.getParam("odom_frame_id", m_odomFrameId))
      m_odomFrameId = "odom";
    if(!lnh.getParam("global_frame_id", m_globalFrameId))
      m_globalFrameId = "map";

    m_lastPoseCov.header.frame_id = m_globalFrameId;

    // Read amcl parameters
    if(!lnh.getParam("update_rate", m_updateRate))
      m_updateRate = 10.0;
    if(!lnh.getParam("angular_rate", m_angularRate))
      m_angularRate = 0.2;
    if(!lnh.getParam("min_particles", m_minParticles))
      m_minParticles = 300;
    if(!lnh.getParam("max_particles", m_maxParticles))
      m_maxParticles = 600;
    if(m_minParticles > m_maxParticles)
      m_maxParticles = m_minParticles;
    if(!lnh.getParam("odom_x_mod", m_odomXMod))
      m_odomXMod = 0.2;
    if(!lnh.getParam("odom_y_mod", m_odomYMod))
      m_odomYMod = 0.2;
    if(!lnh.getParam("odom_a_mod", m_odomAMod))
      m_odomAMod = 0.2;
    if(!lnh.getParam("odom_a_noise", m_odomANoise))
      m_odomANoise = 0.05;
    if(!lnh.getParam("initial_x", m_initX))
      m_initX = 0.0;
    if(!lnh.getParam("initial_y", m_initY))
      m_initY = 0.0;
    if(!lnh.getParam("initial_a", m_initA))
      m_initA = 0.0;
    if(!lnh.getParam("initial_x_dev", m_initXDev))
      m_initXDev = 0.3;
    if(!lnh.getParam("initial_y_dev", m_initYDev))
      m_initYDev = 0.3;
    if(!lnh.getParam("initial_a_dev", m_initADev))
      m_initADev = 0.2;
    if(!lnh.getParam("update_min_d", m_dTh))
      m_dTh = 0.5;
    if(!lnh.getParam("update_min_a", m_aTh))
      m_aTh = 0.2;
    if(!lnh.getParam("resample_interval", m_resampleInterval))
      m_resampleInterval = 0;
    // Get the parameters related with the sewer
    if (!lnh.getParam("detect_manhole_topic", m_detectManholeTopic))
      m_detectManholeTopic = "/manhole";

    if (!lnh.getParam("angular_weight", angular_weight))
      angular_weight = 5.0;

    if (!lnh.getParam("sewer_graph_file", m_sewer_graph_file))
      m_sewer_graph_file = "/home/siar/siar_ws/src/siar_packages/localization_siar/sewer_graph/test/sewer_graph_1";

    if (!lnh.getParam("min_manhole_detected", m_min_manhole_detected))
      m_min_manhole_detected = 10;
    if (!lnh.getParam("edgeDev", m_edgeDev))
      m_edgeDev = 1.0;

    edgeConst1 = 1./(m_edgeDev*sqrt(2*M_PI));
    edgeConst2 = 1./(2*m_edgeDev*m_edgeDev);

    if (!lnh.getParam("forkDev", m_forkDev))
      m_forkDev = 3.0;
    if (!lnh.getParam("fork_dist", m_fork_dist))
      m_fork_dist = 3.0;

    forkConst1 = 1./(m_forkDev*sqrt(2*M_PI));
    forkConst2 = 1./(2*m_forkDev*m_forkDev);

    if (!lnh.getParam("manholeDev", m_manholeDev))
      m_manholeDev = 0.6;
    if (!lnh.getParam("manholeThres", m_manholeThres))
      m_manholeThres = 0.15;

    manholeConst1 = 1./(m_manholeDev*sqrt(2*M_PI));
    manholeConst2 = 1./(2*m_manholeDev*m_manholeDev);

    if (!lnh.getParam("angleDev", m_angleDev))
      m_angleDev = 0.03;
    angleConst1 = 1./(m_angleDev*sqrt(2*M_PI));
    angleConst2 = 1./(2*m_angleDev*m_angleDev);

    // Save trajectory
    traj_file_open = false;
    if (!lnh.getParam("traj_file", traj_filename))
      traj_filename = "~/traj_.txt";
    try {
      traj_file.open(traj_filename.c_str());
      traj_file_open = true;
    }catch (std::exception &e) {
      std::cerr << "Could not open the traj file: " << traj_filename << std::endl;
    }

    // Init the particle filter internal stuff
    m_nUpdates = 0;
    m_init = false;
    m_doUpdate = false;
    m_p.resize(m_maxParticles);
    // Launch updater timer
    updateTimer = m_nh.createTimer(ros::Duration(1.0/m_updateRate), &ParticleFilter::checkUpdateThresholdsTimer, this);

    // Initialize TF from odom to map as identity
    m_lastGlobalTf.setIdentity();

    // Initialize sewer graph
    s_g = new sewer_graph::SewerGraph(m_sewer_graph_file);

    // PUblish gps reference position
    ROS_INFO("Publishing gps position: %f, %f", s_g->getReferencePosition().latitude, s_g->getReferencePosition().longitude);
    m_gpsPub.publish(s_g->getReferencePosition());

    // Initialize filter if requested
    bool initialize = false;
    if (!lnh.getParam("initialize", initialize)) {
      initialize = false;
    }
    if (initialize)
    {
      tf::Pose pose;
      tf::Vector3 v(m_initX, m_initY, 0.0);
      pose.setOrigin(v);
      pose.setRotation(tf::createQuaternionFromYaw(m_initA));
      ROS_INFO("Setting pose: %.3f %.3f %.3f", pose.getOrigin().x(), pose.getOrigin().y(), getYawFromTf(pose));
      setInitialPose(pose, m_initXDev, m_initYDev, m_initADev);
    }
    last_yaw = 1e100;
    // Launch subscribers
    m_detect_manhole_Sub = m_nh.subscribe(m_detectManholeTopic, 1, &ParticleFilter::manholeDetectedCallback, this);
    m_initialPoseSub = m_nh.subscribe(node_name+"/initial_pose", 2, &ParticleFilter::initialPoseReceived, this);
    m_wall_info_sub = m_nh.subscribe("/wall_info", 1, &ParticleFilter::wallInfoCallback, this);
    m_ground_sub = m_nh.subscribe("/ground_truth", 2, &ParticleFilter::groundCallback, this);

    // Launch publishers
    m_posesPub = m_nh.advertise<geometry_msgs::PoseArray>(node_name+"/particle_cloud", 1, true);
    m_posecovPub = m_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(node_name+"/estimated_pose", 1, true);
    m_graphPub = m_nh.advertise<visualization_msgs::Marker>(node_name+"/sewer_graph", 0, true);
    m_gpsPub = m_nh.advertise<sensor_msgs::NavSatFix>("/gps/fix", 2, true);
  }

  //!Default destructor
  virtual ~ParticleFilter()
  {
    gsl_rng_free(m_randomValue);
    delete s_g;
  }

  //! Check motion and time thresholds for AMCL update
  virtual bool checkUpdateThresholds()
  {
    // Publish current TF from odom to map
    m_tfBr.sendTransform(tf::StampedTransform(m_lastGlobalTf, ros::Time::now(), m_globalFrameId, m_odomFrameId));

    // If the filter is not initialized then exit
    if(!m_init)
      return false;

    // Compute odometric trasnlation and rotation since last update
    tf::StampedTransform odomTf;
    try
    {
      m_tfListener.waitForTransform(m_odomFrameId, m_baseFrameId, ros::Time(0), ros::Duration(1.0));
      m_tfListener.lookupTransform(m_odomFrameId, m_baseFrameId, ros::Time(0), odomTf);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("AMCL error: %s",ex.what());
      return false;
    }
    tf::Transform T = m_lastOdomTf.inverse()*odomTf;

    // Check translation threshold
    if(T.getOrigin().length() > m_dTh)
    {
      ROS_INFO("Translation update");
      m_doUpdate = true;
    }

    // Check yaw threshold
    double yaw, pitch, roll;
    T.getBasis().getRPY(roll, pitch, yaw);
    if(fabs(yaw) > m_dTh)
    {
      ROS_INFO("Rotation update");
      m_doUpdate = true;
    }

    if (m_doUpdate) {
      update(checkManhole());
    }

    return false;
  }

  visualization_msgs::Marker getPosMarker(const std::string &ref_frame, int id = 3, double scale = 3.0) {
    float mX, mY, mA, devX, devY, devA;
    computeDev(mX, mY, mA, devX, devY, devA);
    visualization_msgs::Marker pos;
    pos.header.frame_id = ref_frame;
    pos.header.stamp = ros::Time::now();
    pos.ns = "sewer_graph";
    pos.action = visualization_msgs::Marker::ADD;
    pos.pose.orientation.w = 1.0;
    pos.id = id;
    pos.type = visualization_msgs::Marker::SPHERE_LIST;
    pos.color.b = 1.0;
    pos.color.a = 1.0;
    pos.scale.x = scale;
    pos.scale.y = scale;
    pos.scale.z = scale;

    geometry_msgs::Point p;
    p.x = mX;
    p.y = mY;
    p.z = 0.0;

    pos.points.push_back(p);
    return pos;
  }

  void publishParticles()
  {
    static int seq = 0;

    // If the filter is not initialized then exit
    if(!m_init)
      return;

    // Build the msg based on the particles position and orinetation
    geometry_msgs::PoseArray particlesMsg;
    particlesMsg.header.stamp = ros::Time::now();
    particlesMsg.header.frame_id = m_globalFrameId;
    particlesMsg.header.seq = seq++;
    particlesMsg.poses.resize(m_p.size());
    for(int i=0; i<m_p.size(); i++)
    {
      particlesMsg.poses[i].position.x = m_p[i].x;
      particlesMsg.poses[i].position.y = m_p[i].y;
      particlesMsg.poses[i].position.z = 0.0;
      particlesMsg.poses[i].orientation.x = 0.0;
      particlesMsg.poses[i].orientation.y = 0.0;
      particlesMsg.poses[i].orientation.z = sin(m_p[i].a*0.5);
      particlesMsg.poses[i].orientation.w = cos(m_p[i].a*0.5);
    }

    // Publisg particle cloud
    m_posesPub.publish(particlesMsg);
  }

protected:

  inline bool isFork() {
    float x, y, z, xDev, yDev, aDev;
    computeDev(x, y, z, xDev, yDev, aDev);
    return isFork(x,y);
  }

  inline bool isFork(double x, double y) {
    return (s_g->getDistanceToClosestVertex(x, y, sewer_graph::FORK) < m_fork_dist);
  }

  void checkUpdateThresholdsTimer(const ros::TimerEvent& event)
  {
    checkUpdateThresholds();

    // Publish markers
    std::vector<visualization_msgs::Marker> markers = s_g->getMarkers(m_globalFrameId);
    for (unsigned int i = 0; i < markers.size();i++) {
      m_graphPub.publish(markers[i]);

    }
    m_graphPub.publish(getPosMarker(m_globalFrameId));
  }

  void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
  {
    // We only accept initial pose estimates in the global frame
    if(msg->header.frame_id != m_globalFrameId)
    {
      ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
      msg->header.frame_id.c_str(),
      m_globalFrameId.c_str());
      return;
    }

    // Transform into the global frame
    tf::Pose pose;
    tf::poseMsgToTF(msg->pose.pose, pose);
    double x = pose.getOrigin().x();
    double y = pose.getOrigin().y();
    int i,j;

    // Logging the pose change
    ROS_INFO("Setting pose: %.3f %.3f %.3f", pose.getOrigin().x(), pose.getOrigin().y(), getYawFromTf(pose));
    ROS_INFO("Distance to closest Edge = %f", s_g->getDistanceToClosestEdge(x , y, i, j));
    ROS_INFO("Vertices of the closest Edge = %d, %d", i,j);
    ROS_INFO("Distance to closest Manhole = %f", s_g->getDistanceToClosestManhole(x, y));

    // Initialize the filter
    setInitialPose(pose, m_initXDev, m_initYDev, m_initADev);
  }

  //! 3D point-cloud callback
  void manholeDetectedCallback(const std_msgs::BoolConstPtr& msg)
  {
    manhole_hist.push_back(msg->data);
  }

  void wallInfoCallback(const wall_detector::WallInfoConstPtr &msg) {
    // last_info = *msg;
    if (fabs(msg->angle) < 0.06) {
      last_yaw = 0.9*msg->angle;
      last_relative_time = ros::Time::now();
      float x,y,a,xv,yv,av,xycov;
      computeVar(x,y,a,xv,yv,av,xycov);
      double detection_x = 1.5;
      double man_angle_1 = s_g->getClosestEdgeAngle( x + detection_x*cos(a), y + detection_x * sin(a));
      double rel_angle = a - man_angle_1;

      while (fabs(rel_angle) > M_PI / 2) {
        rel_angle -=  (std::signbit(rel_angle))? -M_PI :M_PI;
      }
      // ROS_INFO("Catched angle measurement. Angle = %f. Relative angle = %f", last_yaw, rel_angle);
    }

  }

  void groundCallback(const manhole_detector::ManholeConstPtr &msg) {
     ground = *msg;
     ROS_INFO("Ground truth received. Performing update.");
     updateParticles(GROUND_TRUTH);
      // Re-compute global TF according to new weight of samples
    computeGlobalTfAndPose();

    //Do the resampling
    m_nUpdates = 0;
    resample();
    publishParticles();
    m_posecovPub.publish(m_lastPoseCov);
  }

  void update(bool detected_manhole) {
    if(!predictParticles())
    {
      ROS_ERROR("ParticleFilterSewer::manholeDetectedCallback --> Prediction error!");
      return;
    }

    float x,y,a,xv,yv,av,xycov;
    computeVar(x,y,a,xv,yv,av,xycov);

    if (traj_file_open) {
      traj_file << x << "\t" << y << "\t" << a <<"\t"<< ros::Time::now().sec << "." << ros::Time::now().nsec<<  std::endl;
    }

    // Perform different particle update based on current point-cloud and available measurements
    if (detected_manhole) {
      ROS_INFO("Performing Update with Manhole");
      updateParticles(MANHOLE);
    } else if (isFork(x,y)) {
      ROS_INFO("Performing update with fork");
      updateParticles(FORK);
    } else if (last_relative_time - ros::Time::now() < ros::Duration(0,100000000L) && fabs(last_yaw) < 5) {

      if (gsl_rng_uniform(m_randomValue) < m_angularRate) {
        ROS_INFO("Performing angular update. ");
        updateParticles(ANGULAR);

      } else {
        ROS_INFO("Performing regular update, but detected yaw.");
        updateParticles(REGULAR);
      }
    } else {
      ROS_INFO("Performing regular update");
      updateParticles(REGULAR);
    }

    // Re-compute global TF according to new weight of samples
    computeGlobalTfAndPose();

    //Do the resampling if needed
    m_nUpdates++;
    if(m_nUpdates > m_resampleInterval)
    {
      m_nUpdates = 0;
      resample();
    }

    /*wt = 0; NOTE: this was the implementation of importance resampling
    for(int i=0; i<(int)m_p.size(); i++)
      wt += m_p[i].w*m_p[i].w;
    float nEff = 1/wt;
    if(nEff < ((float)m_p.size())/10.0)
      resample();*/
    //resample();

    m_doUpdate = false;

    // Publish particles
    publishParticles();
    m_posecovPub.publish(m_lastPoseCov);
  }

  //!This function implements the PF prediction stage. Translation in X, Y and Z
  //!in meters and yaw angle incremenet in rad
  //! TODO: In a first stage the predict stage will remain equal to the amcl_3d but without the z
  virtual bool predictParticles()
  {
    // Compute odometric trasnlation and rotation since last update
    tf::StampedTransform odomTf;
    try
    {
      m_tfListener.waitForTransform(m_odomFrameId, m_baseFrameId, ros::Time(0), ros::Duration(0.1));
      m_tfListener.lookupTransform(m_odomFrameId, m_baseFrameId, ros::Time(0), odomTf);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s",ex.what());
      return false;
    }

    // Extract current robot roll and pitch
    double yaw, r, p;
    odomTf.getBasis().getRPY(r, p, yaw);

    // Perform particle prediction based on odometry
    float delta_x, delta_y;
    double delta_r, delta_p, delta_a;
    tf::Transform T = m_lastOdomTf.inverse()*odomTf;
    delta_x = T.getOrigin().getX();
    delta_y = T.getOrigin().getY();
    T.getBasis().getRPY(delta_r, delta_p, delta_a);

    float xDev, yDev, aDev;
    xDev = fabs(delta_x*m_odomXMod);
    yDev = fabs(delta_y*m_odomYMod);
    aDev = fabs(delta_a*m_odomAMod) + fabs(m_odomANoise);

    //Make a prediction for all particles according to the odometry
    for(int i=0; i<(int)m_p.size(); i++)
    {
      float sa = sin(m_p[i].a);
      float ca = cos(m_p[i].a);
      float randX = delta_x + gsl_ran_gaussian(m_randomValue, xDev);
      float randY = delta_y + gsl_ran_gaussian(m_randomValue, yDev);
      m_p[i].x += ca*randX - sa*randY;
      m_p[i].y += sa*randX + ca*randY;
      m_p[i].a += delta_a + gsl_ran_gaussian(m_randomValue, aDev);
    }

    m_lastOdomTf = odomTf;

    return true;
  }

  // Update Particles taking into account
  // No input is necessary --> we will get the closest Manhole, which will be used for weighting purposes(with some dispersion)
  void updateParticles(const UpdateType mode)
  {
    double wt = 0.0;

    for(int i=0; i<(int)m_p.size(); i++)
    {
      double tx = m_p[i].x;
      double ty = m_p[i].y;
      double ta = m_p[i].a;
      // Evaluate the weight of the range sensors

      switch (mode) {
        case MANHOLE:
          m_p[i].w = computeManholeWeight(tx, ty);
          break;
        case FORK:
          m_p[i].w = computeForkWeight(tx, ty);
          break;
        case ANGULAR:
          m_p[i].w = computeAngularWeight(tx, ty, ta);
          break;
        case YAW:
          m_p[i].w = computeEdgeWeight(tx, ty);
          m_p[i].w += angular_weight * computeAngularWeight(tx, ty, ta);
          break;
        case GROUND_TRUTH:
          m_p[i].w = computeGroundTruthWeight(tx, ty, ground.local_pose.x, ground.local_pose.y);
          break;
        default:
          m_p[i].w = computeEdgeWeight(tx, ty); // Compute weight as a function of the distance to the closest edge
      }

      //Increase the summatory of weights
      wt += m_p[i].w;
    }

    //Normalize all weights
    wt = 1.0 / wt;
    for(int i=0; i<(int)m_p.size(); i++)
    {
      m_p[i].w *= wt;
    }
  }

  double computeEdgeWeight(double x, double y) {
    int i,j;
    double dist = s_g->getDistanceToClosestEdge(x,y,i, j);

    return edgeConst1*exp(-dist*dist*edgeConst2);
  }

  double computeForkWeight(double x, double y) {
    int i,j;
    double dist = s_g->getDistanceToClosestEdge(x,y,i,j);
    return forkConst1*exp(-dist*dist*forkConst2);

  }

  double computeManholeWeight(double x, double y) {
    double dist = s_g->getDistanceToClosestManhole(x, y);
    return manholeConst1*exp(-dist*dist*manholeConst2) + m_manholeThres;
  }

  double computeGroundTruthWeight(double x, double y, double g_x, double g_y) {
    double dist2 = (x-g_x)*(x-g_x) + (y-g_y)*(y-g_y);
    return manholeConst1*exp(-dist2*manholeConst2) + m_manholeThres;
  }

  double computeAngularWeight(double tx, double ty, double ta) {
    double ret = 0.0;
    double detection_x = 1.5;
    double man_angle_1 = s_g->getClosestEdgeAngle( tx + detection_x*cos(ta), ty + detection_x * sin(ta));
    double rel_angle = ta - man_angle_1;

    while (fabs(rel_angle) > M_PI / 2) {
      rel_angle -=  (std::signbit(rel_angle))? -M_PI :M_PI;
    }
    while (fabs(last_yaw) > M_PI / 2) {
      last_yaw -=  (std::signbit(last_yaw))? -M_PI :M_PI;
    }

    // ROS_INFO("ComputeAngularWeight: Estimated rel angle = %f\t Rel angle from particle = %f", last_yaw, rel_angle);
    double angular_error = rel_angle - last_yaw; // The angles should be opposite (their sum should be zero)

    ret = angleConst1*exp(-angular_error*angular_error*angleConst2);

    return ret;
  }

  //! Set the initial pose of the particle filter
  void setInitialPose(tf::Pose initPose, float xDev, float yDev, float aDev)
  {
    // Resize particle set
    m_p.resize(m_maxParticles);

    // Sample the given pose
    tf::Vector3 t = initPose.getOrigin();
    float a = getYawFromTf(initPose);
    float dev = std::max(xDev, yDev);
    float gaussConst1 = 1./(dev*sqrt(2*M_PI));
    float gaussConst2 = 1./(2*dev*dev);
    float dist = 0.0, wt = 0.0;
    m_p[0].x = t.x();
    m_p[0].y = t.y();
    m_p[0].a = a;
    m_p[0].w = gaussConst1;
    wt = m_p[0].w;
    for(int i=1; i<(int)m_p.size(); i++)
    {
      m_p[i].x = t.x() + gsl_ran_gaussian(m_randomValue, xDev);
      m_p[i].y = t.y() + gsl_ran_gaussian(m_randomValue, yDev);
      m_p[i].a = a + gsl_ran_gaussian(m_randomValue, aDev);
      dist = sqrt((m_p[i].x - t.x())*(m_p[i].x - t.x()) + (m_p[i].y - t.y())*(m_p[i].y - t.y()) );
      m_p[i].w = gaussConst1*exp(-dist*dist*gaussConst2);
      wt += m_p[i].w;
    }
    for(int i=0; i<(int)m_p.size(); i++)
      m_p[i].w /= wt;

    // Extract TFs for future updates
    bool initialized = false;
    while (!initialized) {
      try
      {
        m_tfListener.waitForTransform(m_odomFrameId, m_baseFrameId, ros::Time(0), ros::Duration(15.0));
        m_tfListener.lookupTransform(m_odomFrameId, m_baseFrameId, ros::Time(0), m_lastOdomTf);
        initialized = true;
      }
      catch (tf::TransformException ex)
      {
        ROS_ERROR("%s",ex.what());
      }
      sleep(2);
    }
    computeGlobalTfAndPose();
    m_doUpdate = false;
    m_init = true;

    // Publish particles
    publishParticles();
  }

  //! Return yaw from a given TF
  float getYawFromTf(tf::Pose& pose)
  {
    double yaw, pitch, roll;

    pose.getBasis().getRPY(roll, pitch, yaw);

    return (float)yaw;
  }

  //! resample the set of particules using low-variance sampling
  int resample()
  {
    int i, m;
    float r, u, c, factor;
    std::vector<Particle> newP;

    //Initialize data and vectors
    newP.resize(m_p.size());
    factor = 1.0/((float)m_p.size());
    i = 0;
    c = m_p[0].w;
    r = factor * gsl_rng_uniform(m_randomValue);

    //Do resamplig
    for(m=0; m<m_p.size(); m++)
    {
      u = std::fma(factor,m, r);
      while(u > c)
      {
        i++;
        c += m_p[i].w;
      }
      newP[m] = m_p[i];
      newP[m].w = factor;
    }

    //Asign the new particles set
    m_p = newP;

    return 0;
  }

  // Computes TF from odom to global frames
  void computeGlobalTfAndPose()
  {
    // Compute mean value from particles
    float mx, my, ma, xv, yv, av, xycov;
    computeVar(mx, my, ma, xv, yv, av, xycov);
    Particle p;
    p.x = mx;
    p.y = my;
    p.a = ma;
    p.w = 0.0;

    // Compute the TF from odom to global
    std::cout << "New TF:\n\t" << p.x << ", " << p.y << std::endl;
    m_lastGlobalTf = tf::Transform(tf::Quaternion(0.0, 0.0, sin(p.a*0.5), cos(p.a*0.5)), tf::Vector3(p.x, p.y, 0.0))*m_lastOdomTf.inverse();
    m_lastPoseCov.header.stamp = ros::Time::now();
    m_lastPoseCov.header.seq++;
    geometry_msgs::Quaternion &orientation = m_lastPoseCov.pose.pose.orientation;
    orientation.w = m_lastGlobalTf.getRotation().getW();
    orientation.x = m_lastGlobalTf.getRotation().getX();
    orientation.y = m_lastGlobalTf.getRotation().getY();
    orientation.z = m_lastGlobalTf.getRotation().getZ();
    geometry_msgs::Point &position = m_lastPoseCov.pose.pose.position;
    position.x = mx; position.y = my; position.z = 0.0;
    boost::array<double, 36> &Cov = m_lastPoseCov.pose.covariance;
    Cov[0] = xv; Cov[7] = yv; Cov[35] = av;
    Cov[1] = Cov[6] = xycov;
  }

  void computeVar(float &mX, float &mY, float &mA, float &var_x, float &var_y, float &var_a, float &cov_x_y) {
    // Compute mean value from particles
    var_x = mX = 0.0;
    var_y = mY = 0.0;
    var_a = mA = 0.0;
    cov_x_y = 0.0;
    for(int i=0; i<m_p.size(); i++)
    {
      mX += m_p[i].w * m_p[i].x;
      mY += m_p[i].w * m_p[i].y;
      mA += m_p[i].w * m_p[i].a;
    }
    for(int i=0; i<m_p.size(); i++)
    {
      var_x += m_p[i].w * (m_p[i].x-mX) * (m_p[i].x-mX);
      var_y += m_p[i].w * (m_p[i].y-mY) * (m_p[i].y-mY);
      var_a += m_p[i].w * (m_p[i].a-mA) * (m_p[i].a-mA);
      cov_x_y += m_p[i].w * (m_p[i].x-mX) * (m_p[i].y-mY);

    }
  }

  void computeDev(float &mX, float &mY, float &mA, float &devX, float &devY, float &devA)
  {
    // Compute mean value from particles
    devX = mX = 0.0;
    devY = mY = 0.0;
    devA = mA = 0.0;
    for(int i=0; i<m_p.size(); i++)
    {
      mX += m_p[i].w * m_p[i].x;
      mY += m_p[i].w * m_p[i].y;
      mA += m_p[i].w * m_p[i].a;
    }
    for(int i=0; i<m_p.size(); i++)
    {
      devX += m_p[i].w * (m_p[i].x-mX) * (m_p[i].x-mX);
      devY += m_p[i].w * (m_p[i].y-mY) * (m_p[i].y-mY);
      devA += m_p[i].w * (m_p[i].a-mA) * (m_p[i].a-mA);
    }
    devX = sqrt(devX);
    devY = sqrt(devY);
    devA = sqrt(devA);
  }

  bool checkManhole() {
    int n_trues = 0;

    for (unsigned int i = 0;i < manhole_hist.size(); i++) {

      if (manhole_hist[i])
        n_trues++;
    }

    ROS_INFO("Number of detections %d. Number of positive: %d", (int)manhole_hist.size(), n_trues);

    manhole_hist.clear();

    return n_trues >= m_min_manhole_detected;
  }

  //! Indicates if the filter was initialized
  bool m_init;

  //! Particles
  std::vector<Particle> m_p;

  //! Particles roll and pich (given by IMU)
  double m_roll, m_pitch;

  //! Number of particles in the filter
  int m_maxParticles;
  int m_minParticles;

  //! Odometry characterization
  double m_odomXMod, m_odomYMod, m_odomZMod, m_odomAMod, m_odomANoise;
  double m_initX, m_initY, m_initA;
  double m_initXDev, m_initYDev, m_initADev;

  //! Resampling control
  int m_nUpdates;
  int m_resampleInterval;

  //! Yaw estimation
  double last_yaw;
  ros::Time last_relative_time;
  double angular_weight;

  //! Thresholds for filter updating
  double m_dTh, m_aTh, m_tTh;
  tf::StampedTransform m_lastOdomTf;
  tf::Transform m_lastGlobalTf;
  geometry_msgs::PoseWithCovarianceStamped m_lastPoseCov;
  bool m_doUpdate;
  double m_updateRate;
  double m_angularRate;

  //! Node parameters
  std::string m_detectManholeTopic;
  std::string m_baseFrameId;
  std::string m_odomFrameId;
  std::string m_globalFrameId;
  std::string m_inOdomTfTopic;

  //! ROS msgs and data
  ros::NodeHandle m_nh, lnh;
  tf::TransformBroadcaster m_tfBr;
  tf::TransformListener m_tfListener;
  ros::Subscriber m_detect_manhole_Sub, m_initialPoseSub, m_odomTfSub, m_wall_info_sub, m_ground_sub;
  ros::Publisher m_posesPub, m_graphPub, m_gpsPub, m_posecovPub;
  ros::Timer updateTimer;

  // Sewer stuff
  std::string m_sewer_graph_file;
  sewer_graph::SewerGraph *s_g;

  //! Random number generator
  const gsl_rng_type *m_randomType;
  gsl_rng *m_randomValue;

  boost::variate_generator<boost::mt19937, boost::normal_distribution<> > m_randVar;

  std::vector<bool> manhole_hist;
  int m_min_manhole_detected;
  double m_edgeDev, m_manholeDev, m_manholeThres, m_angleDev;
  double edgeConst1, edgeConst2;
  double manholeConst1, manholeConst2;
  double angleConst1, angleConst2;
  double m_forkDev, forkConst1, forkConst2, m_fork_dist;

  manhole_detector::Manhole ground; // Store the ground truth data

  //For saving trajetory
  std::ofstream traj_file;
  std::string traj_filename;
  bool traj_file_open;
};

#endif


//  Copyright (c) 2006, Giovanni P. Deretta
//
//  This code may be used under either of the following two licences:
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
//  THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE. OF SUCH DAMAGE.
//
//  Or:
//
//  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)

#include <iostream>
#include <boost/variant.hpp>
#include <boost/variant/get.hpp>
#include <boost/variant/recursive_variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>
#include <boost/optional.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/mpl/begin_end.hpp>
#include <boost/mpl/deref.hpp>
#include <boost/coroutine/coroutine.hpp>

/**
 * Solve the 'same fringe problem' using coroutines.
 * Given two binary trees, they have the same fringe
 * if all leafs, read from left to right are equals.
 * This is the classical coroutine demonstration problem,
 * because it is hard to solve in O(N) (with best case O(1))
 * without using coroutines.
 * see http://c2.com/cgi/wiki?CoRoutine
 * NOTE: this solution is an almost verbatim port of the lua solution from
 * the wiki.
 */
namespace coroutines = boost::coroutines;
using coroutines::coroutine;

namespace meta {
  /**
   * Compact compile-time description of binary trees of ints.
   */
  template<typename Left, typename Right>
  class node{
    typedef Left left;
    typedef Right right;
  };

  template<int A>
  struct leaf {
    enum {value = A};
  };

  typedef
  node<node<leaf<0>, leaf<1> >, node<leaf<0>, node<leaf<5>, leaf<7> > > >
  tree_a; // fringe: 0 1 0 5 7

  typedef
  node<leaf<0>, node<leaf<1>, node<node<leaf<0>, leaf<5> >, leaf<7> > > >
  tree_b; // fringe: 0 1 0 5 7

  typedef
  node<leaf<1>, node<leaf<7>, node<node<leaf<5>, leaf<4> >, leaf<7> > > >
  tree_c; // fringe: 1 7 5 4 7
}

typedef int leaf;
struct node;
typedef boost::variant<boost::recursive_wrapper<node>,  leaf> tree_element;

typedef boost::tuple<tree_element, tree_element> node_;
struct node : node_ {
  template<typename A, typename B>
  node(const A& a, const B& b) :
    node_(a, b) {}
};

bool is_leaf(const tree_element& x) {
  return x.which() == 1;
}

template<typename Left, typename Right>
tree_element make_tree(meta::node<Left, Right> const&) {
  return tree_element(node(make_tree(Left()),
			   make_tree(Right())));
}

template<int A>
tree_element make_tree(meta::leaf<A> const&) {
  return tree_element(A);
}

typedef coroutine<boost::optional<leaf>(tree_element)> coroutine_type;

boost::optional<leaf>  tree_leaves(coroutine_type::self& self, tree_element tree) {
  if (is_leaf(tree)) {
    self.yield(boost::get<leaf>(tree));
  } else {
    tree_leaves(self, boost::get<node>(tree).get<0>());
    tree_leaves(self, boost::get<node>(tree).get<1>());
  }
  return boost::optional<leaf>();
}

bool same_fringe(tree_element tree1, tree_element tree2) {
  coroutine_type tree_leaves_a(tree_leaves);
  coroutine_type tree_leaves_b(tree_leaves);
  boost::optional<leaf> tmp1, tmp2;
  while ((tmp1 = tree_leaves_a(tree1)) && (tmp2=tree_leaves_b(tree2)))
    if (!(tmp1 == tmp2)) return false;
  return true;
}

int main() {
  std::cout<<"same_fringe tree_a, tree_a "<<same_fringe
    (make_tree(meta::tree_a()), make_tree(meta::tree_a()))<<"\n";
  std::cout<<"same_fringe tree_a, tree_b "<<same_fringe
    (make_tree(meta::tree_a()), make_tree(meta::tree_b()))<<"\n";
  std::cout<<"same_fringe tree_a, tree_c "<<same_fringe
    (make_tree(meta::tree_a()), make_tree(meta::tree_c()))<<"\n";
}


/*=================================================================================
 *	                    Copyleft! 2018 William Yu
 *          Some rights reserved：CC(creativecommons.org)BY-NC-SA
 *                      Copyleft! 2018 William Yu
 *      版权部分所有，遵循CC(creativecommons.org)BY-NC-SA协议授权方式使用
 *
 * Filename                :
 * Description             : 视觉SLAM十四讲/ch5 学习记录
 * Reference               : 分切点云
 * Programmer(s)           : William Yu, windmillyucong@163.com
 * Company                 : HUST, DMET国家重点实验室FOCUS团队
 * Modification History	   : ver1.0, 2019.01.02, William Yu

=================================================================================*/

/// Include Files
#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp>  // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

/// Function Definitions

/**
 * @function main
 * @author William Yu
 * @brief
 */
int main( int argc, char** argv )
{
    // 相机内参
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;

    cout<<"读入点云..."<<endl;
    pcl::PCDReader reader;
    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;
    PointCloud::Ptr pointCloud( new PointCloud );
    reader.read ("../output/map.pcd", *pointCloud);
    int size_pointCloud = pointCloud->points.size()/10;//只是用前面的一些点，使用全部点会报错，原因不明。应该是内存溢出。
    std::cout << "PointCloud before filtering has: " << size_pointCloud << " data points." << std::endl;

    cv::Mat color(480,640,CV_8UC3,cv::Scalar(0,0,0) );
    cv::Mat depth(480,640,CV_8UC1,cv::Scalar(255));

    //--每个世界坐标转换到像素坐标
    for ( size_t p_i=0; p_i<540920; ++p_i )
    {
        PointT p = pointCloud->points[p_i];//由点云中载入点

        //内参修正
        Eigen::Vector3d point;
        point[0] = p.x;
        point[1] = p.y;
        point[2] = p.z;

        unsigned int d = depthScale * point[2];
        int u = fx * point[0] / point[2] + cx;
        int v = fy * point[1] / point[2] + cy;
        int d_rawdata = depth.ptr<unsigned short> (v)[u];
        std::cout << p_i << std::endl;
        // std::cout << " u:" << u
        // << " v:" << v
        // << " d:" << d
        // << std::endl;
        // std::cout << " b:" << (int)p.b
        // << " g:" << (int)p.g
        // << " r:" << (int)p.r
        // << std::endl;

        //如果新点z轴更高，跳过
        if ( d_rawdata < d ) continue; //取下侧，仰视图
        //如果新点更低，更新RGBD
        depth.ptr<unsigned short> (v)[u] = d; // 深度值，其实深度值根本获取不出来
        color.data[ v*color.step+u*color.channels() ] = p.b;
        color.data[ v*color.step+u*color.channels()+1 ] = p.g;
        color.data[ v*color.step+u*color.channels()+2 ] = p.r;

        //释放资源
        //delete []PointCloud;
    }

    cv::imshow("splitimg",color);

    char key = (char)cv::waitKey(0);
    if( key == 27 || key == 'q' || key == 'Q' ) // 'ESC'
        return 0;
}


/**
 * @file   global.hpp
 * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com)
 * @date   May 22, 2019
 * @brief
 * @section LICENSE
 *
 *  BSD-3-Clause License
 *
 * @copyright Copyright (C) 2020 Intel Corporation
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name of the copyright holder nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * 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.
 *
**/
#ifndef RRT_GLOBAL_HPP_
#define RRT_GLOBAL_HPP_

#include <Eigen/Core>
#include <vector>
#include <stdint.h>

namespace rrt
{

typedef enum {kMax=0, kMin, kNIndex}eIndex;
typedef enum {kFov=0, kMap, kNTypes}eDirection;

const double k_rrt_solution_error = 0.1;

#ifndef finline
  #ifdef WIN32
    #define finline __forceinline
  #else
    #define finline inline __attribute__((always_inline))
  #endif
#endif

class 	Node;

typedef Eigen::Vector3d 								Point;
typedef std::vector<Point> 								PointList;
typedef Eigen::MatrixXd 								Matrix;
typedef std::vector<Node>								Nodes;

static Matrix CalculateRotationMatrix(const double &roll, const double &pitch, const double &yaw)
{
	Matrix Rz(3,3);
	Rz<< 	cos(yaw), -sin(yaw), 0.0,
			sin(yaw), cos(yaw), 0.0,
			0.0, 0.0, 1.0;

	Matrix Rx(3,3);
	Rx<< 		1.0, 0.0, 0.0,
			0.0, cos(roll), -sin(roll),
			0.0, sin(roll), cos(roll);

	Matrix Ry(3,3);
	Ry<< 	cos(pitch), 0.0, sin(pitch),
			 0.0, 1.0, 0.0,
			 -sin(pitch), 0.0, cos(pitch);
	return Rz*Ry*Rx;
}
static Matrix CalculateRotationMatrix(const double &theta)
{
	Matrix Rz(2,2);
	Rz<< 	cos(theta), -sin(theta),
			sin(theta), cos(theta);
	return Rz;
}

} /* namespace rrt */

#endif /* RRT_GLOBAL_HPP_ */