#include <iostream>
#include <string>
#include <boost/make_shared.hpp>
#include <pcl/console/time.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>

#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>

#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/fpfh_omp.h>//包含fpfh加速计算的omp多核并行计算

#include <pcl/registration/ia_ransac.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>

#include <pcl/visualization/pcl_visualizer.h>

//216行  pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
using namespace std;
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;

//convenient typedefs
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;

typedef pcl::FPFHSignature33 FPFHT;
typedef pcl::PointCloud<FPFHT> FPFHCloud;

const float VOXEL_GRID_SIZE = 0.01;
const double radius_normal=30;
const double radius_feature=30;
const double max_sacia_iterations=1000;
const double min_correspondence_dist=0.01;
const double max_correspondence_dist=1000;
// This is a tutorial so we can afford having global variables
pcl::visualization::PCLVisualizer *p;
//its left and right viewports
int vp_1, vp_2;

pcl::console::TicToc timecal;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
//convenient structure to handle our pointclouds
struct PCD
{
  PointCloud::Ptr cloud;
  std::string f_name;

  PCD() : cloud (new PointCloud) {};
};

struct PCDComparator
{
  bool operator () (const PCD& p1, const PCD& p2)
  {
    return (p1.f_name < p2.f_name);
  }
};

// Define a new point representation for < x, y, z, curvature >
class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
{
  using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;

public:
  MyPointRepresentation ()
  {
    // Define the number of dimensions
    nr_dimensions_ = 4;
  }

  // Override the copyToFloatArray method to define our feature vector
  virtual void copyToFloatArray (const PointNormalT &p, float * out) const
  {
    // < x, y, z, curvature >
    out[0] = p.x;
    out[1] = p.y;
    out[2] = p.z;
    out[3] = p.curvature;
  }
};

void voxelFilter(PointCloud::Ptr &cloud_in, PointCloud::Ptr &cloud_out, float gridsize){
	pcl::VoxelGrid<PointT> vox_grid;
	vox_grid.setLeafSize(gridsize, gridsize, gridsize);
	vox_grid.setInputCloud(cloud_in);
  vox_grid.filter(*cloud_out);
  cout << "PointCloud before voxelfiltering: " << cloud_in->width * cloud_in->height
  << " data points (" << pcl::getFieldsList (*cloud_in) << ")."<<endl;
  cout << "PointCloud after voxelfiltering: " << cloud_out->width * cloud_out->height
  << " data points (" << pcl::getFieldsList (*cloud_out) << ").\n"<<endl;
	return;
}

pcl::PointCloud<pcl::Normal>::Ptr getNormals(PointCloud::Ptr cloud, double radius)
{
    pcl::PointCloud<pcl::Normal>::Ptr normalsPtr (new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<PointT,pcl::Normal> norm_est;
    norm_est.setInputCloud(cloud);
    norm_est.setSearchMethod(tree);
    norm_est.setKSearch(radius);
    //norm_est.setRadiusSearch(radius);
    norm_est.compute(*normalsPtr);
    return normalsPtr;

}

FPFHCloud::Ptr getFeatures(PointCloud::Ptr cloud,pcl::PointCloud<pcl::Normal>::Ptr normals,double radius)
{
    FPFHCloud::Ptr features (new FPFHCloud);
    pcl::FPFHEstimationOMP<PointT,pcl::Normal,FPFHT> fpfh_est;
    fpfh_est.setNumberOfThreads(4);
    fpfh_est.setInputCloud(cloud);
    fpfh_est.setInputNormals(normals);
    fpfh_est.setSearchMethod(tree);
    fpfh_est.setKSearch(radius);
   // fpfh_est.setRadiusSearch(radius);
    fpfh_est.compute(*features);
    return features;
}

//sac_ia配准
void sac_ia_align(PointCloud::Ptr source,PointCloud::Ptr target,PointCloud::Ptr finalcloud,Eigen::Matrix4f init_transform,
   int max_sacia_iterations,double min_correspondence_dist,double max_correspondence_dist)
{

  vector<int> indices1;
  vector<int> indices2;
  PointCloud::Ptr sourceds (new PointCloud);
  PointCloud::Ptr targetds (new PointCloud);
  pcl::removeNaNFromPointCloud(*source,*source,indices1);
  pcl::removeNaNFromPointCloud(*target,*target,indices2);
//降采样
  voxelFilter(source,sourceds,VOXEL_GRID_SIZE);
  voxelFilter(target,targetds,VOXEL_GRID_SIZE);
  cout<<"1"<<endl;
//计算法向量
  pcl::PointCloud<pcl::Normal>::Ptr source_normal (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<pcl::Normal>::Ptr target_normal (new pcl::PointCloud<pcl::Normal>);
  source_normal=getNormals(sourceds,radius_normal);
  target_normal=getNormals(targetds,radius_normal);
  cout<<"2"<<endl;
//计算FPFH特征
  FPFHCloud::Ptr source_feature (new FPFHCloud);
  FPFHCloud::Ptr target_feature (new FPFHCloud);
  source_feature=getFeatures(sourceds,source_normal,radius_feature);
  target_feature=getFeatures(targetds,target_normal,radius_feature);
  cout<<"3"<<endl;
//SAC-IA配准
  pcl::SampleConsensusInitialAlignment<PointT,PointT,FPFHT> sac_ia;
  Eigen::Matrix4f final_transformation;
  sac_ia.setInputSource(targetds);
  sac_ia.setSourceFeatures(target_feature);
  sac_ia.setInputTarget(sourceds);
  sac_ia.setTargetFeatures(source_feature);
  sac_ia.setMaximumIterations(max_sacia_iterations);
  sac_ia.setMinSampleDistance(min_correspondence_dist);
  sac_ia.setMaxCorrespondenceDistance(max_correspondence_dist);

  PointCloud::Ptr output (new PointCloud);
  timecal.tic();
  sac_ia.align(*output);
  cout<<"Finished SAC_IA Initial Regisration in "<<timecal.toc()<<"ms\n"<<endl;
  init_transform=sac_ia.getFinalTransformation();
  pcl::transformPointCloud(*target,*finalcloud,init_transform);
}
////////////////////////////////
//显示配准结果，刚性变换矩阵
void print4x4Matrix(Eigen::Matrix4f &matrix)
{

  printf ("Rotation matrix:\n");
  printf ("    | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));
  printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));
  printf ("    | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));
  printf ("Translation vector :\n");
  printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));
}

////////////////////////////////////////////////////////////////////////////////
/** \brief Display source and target on the first viewport of the visualizer
 *
 */
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
  p->removePointCloud ("vp1_target");
  p->removePointCloud ("vp1_source");

  PointCloudColorHandlerCustom<PointT> tgt_h (cloud_target, 0, 255, 0);
  PointCloudColorHandlerCustom<PointT> src_h (cloud_source, 255, 0, 0);
  p->addPointCloud (cloud_target, tgt_h, "vp1_target", vp_1);
  p->addPointCloud (cloud_source, src_h, "vp1_source", vp_1);

  PCL_INFO ("Press q to begin the registration.\n");
  p-> spin();
}

////////////////////////////////////////////////////////////////////////////////
/** \brief Display source and target on the second viewport of the visualizer
 *
 */
void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, const PointCloudWithNormals::Ptr cloud_source)
{
  p->removePointCloud ("source");
  p->removePointCloud ("target");

  PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler (cloud_target, "curvature");
  if (!tgt_color_handler.isCapable ())
      PCL_WARN ("Cannot create curvature color handler!");

  PointCloudColorHandlerGenericField<PointNormalT> src_color_handler (cloud_source, "curvature");
  if (!src_color_handler.isCapable ())
      PCL_WARN ("Cannot create curvature color handler!");

  p->addPointCloud (cloud_target, tgt_color_handler, "target", vp_2);
  p->addPointCloud (cloud_source, src_color_handler, "source", vp_2);

  p->spinOnce();
}

////////////////////////////////////////////////////////////////////////////////
/** \brief Load a set of PCD files that we want to register together
  * \param argc the number of arguments (pass from main ())
  * \param argv the actual command line arguments (pass from main ())
  * \param models the resultant vector of point cloud datasets
  */
void loadData (int argc, char **argv, std::vector<PCD, Eigen::aligned_allocator<PCD> > &models)
{
  std::string extension (".pcd");
  // Suppose the first argument is the actual test model
  for (int i = 1; i < argc; i++)
  {
    std::string fname = std::string (argv[i]);
    // Needs to be at least 5: .plot
    if (fname.size () <= extension.size ())
      continue;

    std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);

    //check that the argument is a pcd file
    if (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0)
    {
      // Load the cloud and saves it into the global list of models
      PCD m;
      m.f_name = argv[i];
      pcl::io::loadPCDFile (argv[i], *m.cloud);
      //remove NAN points from the cloud
      std::vector<int> indices;
      pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);

      models.push_back (m);
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
/** \brief Align a pair of PointCloud datasets and return the result
  * \param cloud_src the source PointCloud
  * \param cloud_tgt the target PointCloud
  * \param output the resultant aligned source PointCloud
  * \param final_transform the resultant transform between source and target
  */
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
{
  //
  // Downsample for consistency and speed
  // \note enable this for large datasets
  PointCloud::Ptr src (new PointCloud);
  PointCloud::Ptr tgt (new PointCloud);
  pcl::VoxelGrid<PointT> grid;
  if (downsample)
  {
    grid.setLeafSize (0.05, 0.05, 0.05);
    grid.setInputCloud (cloud_src);
    grid.filter (*src);

    grid.setInputCloud (cloud_tgt);
    grid.filter (*tgt);
  }
  else
  {
    src = cloud_src;
    tgt = cloud_tgt;
  }

  // Compute surface normals and curvature
  PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);
  PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);

  pcl::NormalEstimation<PointT, PointNormalT> norm_est;

  norm_est.setSearchMethod (tree);
  norm_est.setKSearch (30);
  norm_est.setInputCloud (src);
  norm_est.compute (*points_with_normals_src);
  pcl::copyPointCloud (*src, *points_with_normals_src);

  norm_est.setInputCloud (tgt);
  norm_est.compute (*points_with_normals_tgt);
  pcl::copyPointCloud (*tgt, *points_with_normals_tgt);
  MyPointRepresentation point_representation;
  // ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
  float alpha[4] = {1.0, 1.0, 1.0, 1.0};
  point_representation.setRescaleValues (alpha);

  //
  // Align
  pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
  reg.setTransformationEpsilon (1e-6);
  //reg.setEuclideanFitnessEpsilon(1);
  // Set the maximum distance between two correspondences (src<->tgt) to 10cm
  // Note: adjust this based on the size of your datasets
  reg.setMaxCorrespondenceDistance (1);
  //reg.RANSACOutlierRejectionThreshold(1.5);
  reg.setMaximumIterations (1000);
  reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));
  reg.setInputSource (points_with_normals_src);
  reg.setInputTarget (points_with_normals_tgt);

  // Run the same optimization in a loop and visualize the results
  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
  PointCloudWithNormals::Ptr reg_result = points_with_normals_src;

  for (int i = 0; i < 5; ++i)
  {
    points_with_normals_src = reg_result;// save cloud for visualization purpose
    reg.setInputSource (points_with_normals_src);
    timecal.tic();
    reg.align (*reg_result);
    //cout<<"Applied %ICP "<<time.toc()<<" ms\n";
    PCL_INFO("TIME:Applied num %d ICP in %f ms\n",i+1,timecal.toc());
    Ti = reg.getFinalTransformation () * Ti;

		//if the difference between this transformation and the previous one
		//is smaller than the threshold, refine the process by reducing
		//the maximal correspondence distance
    if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
      reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);

    prev = reg.getLastIncrementalTransformation ();

    // visualize current state
    showCloudsRight(points_with_normals_tgt, points_with_normals_src);
  }

  // Get the transformation from target to source
  targetToSource = Ti.inverse();

  final_transform = targetToSource;
  std::cout << "\nICP has converged, score is " << reg.getFitnessScore () << std::endl;
  //std::cout <<icp.getFinalTransformation ()<<std::endl;
  print4x4Matrix(final_transform);

  // Transform target back in source frame
  pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);

  p->removePointCloud ("source");
  p->removePointCloud ("target");

  PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 255, 0, 0);
  PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 0, 255, 0);
  p->addPointCloud (output, cloud_tgt_h, "target", vp_2);
  p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);

	PCL_INFO ("Press q to continue the registration.\n");
  p->spin ();

  p->removePointCloud ("source");
  p->removePointCloud ("target");

  //add the source to the transformed target
  //*output += *cloud_src;

 }

/* ---[ */
int main (int argc, char** argv)
{
  // Load data
  std::vector<PCD, Eigen::aligned_allocator<PCD> > data;
  loadData (argc, argv, data);

  // Check user input
  if (data.empty ())
  {
    PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);
    PCL_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");
    return (-1);
  }
  PCL_INFO ("Loaded %d datasets.", (int)data.size ());

  // Create a PCLVisualizer object
  p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");
  p->createViewPort (0.0, 0, 0.5, 1.0, vp_1);
  p->createViewPort (0.5, 0, 1.0, 1.0, vp_2);

	PointCloud::Ptr result (new PointCloud), source, target;
  Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;

  for (size_t i = 1; i < data.size (); ++i)
  {
    source = data[i-1].cloud;
    target = data[i].cloud;

    // Add visualization data
    showCloudsLeft(source, target);

    PointCloud::Ptr temp (new PointCloud);
    PointCloud::Ptr final (new PointCloud);
    final=data[0].cloud;
    PointCloud::Ptr init_result (new PointCloud);
    *init_result = *target;
    Eigen::Matrix4f init_transform = Eigen::Matrix4f::Identity ();
    PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());
    sac_ia_align(source,target,init_result,init_transform,max_sacia_iterations,min_correspondence_dist,max_correspondence_dist);
    pairAlign (source, init_result, temp, pairTransform, true);
    pairTransform*=init_transform;

    pcl::transformPointCloud (*temp, *result, GlobalTransform);
    *final+=*result;

    GlobalTransform = GlobalTransform * pairTransform;

		//save aligned pair, transformed into the first cloud's frame
    std::stringstream ss;
    ss << i << ".pcd";
    pcl::io::savePCDFile (ss.str (), *final, true);

  }
}
/* ]--- */


/*
 * Copyright (c) 2020, Eberty Alves
 */

// C++ standard library
#include <bits/stdc++.h>

// Point cloud library
#include <pcl/common/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>

// Boost
#include <boost/program_options.hpp>

// Typedefs
typedef pcl::PointXYZRGBNormal PointT;
typedef pcl::PointCloud<PointT> PointC;

int main(int argc, char* argv[]) {
  pcl::console::setVerbosityLevel(pcl::console::L_ERROR);

  try {
    // Declaration of variables
    std::string input_file_name;
    std::string output_file_name;
    std::string transform_file;

    // Parse command-line options
    namespace po = boost::program_options;

    // Define command-line options
    po::options_description desc("Options");
    desc.add_options()
    ("help,h", "Print help message")
    ("input,i", po::value<std::string>(&input_file_name)->required(), "Input file (.ply)")
    ("output,o", po::value<std::string>(&output_file_name)->required(), "Output file (.ply)")
    ("transform,t", po::value<std::string>(&transform_file)->required(), "File containing a 4x4 transformation matrix");

    // Use a parser to evaluate the command line
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    // Store the command-line options evaluated by the parser
    if (vm.count("help")) {
      std::cout << "Transforms Point cloud." << std::endl << std::endl;
      std::cout << desc << std::endl;
      return 0;
    }

    if (vm.count("input") && vm.count("output") && vm.count("transform")) {
      input_file_name = vm["input"].as<std::string>();
      output_file_name = vm["output"].as<std::string>();
      transform_file = vm["transform"].as<std::string>();
    } else {
      throw std::string("Correct mode of use: " + std::string(argv[0]) +
                        " -i input.ply -o output.ply -t transform_file.txt");
    }

    PointC::Ptr source_cloud(new PointC);
    PointC::Ptr transformed_cloud(new PointC);

    // Load the point cloud data from disk
    if (pcl::io::loadPLYFile<PointT>(input_file_name, *source_cloud) == -1) {
      throw std::string("Couldn't load input file");
    }

    // Load the transform matrix data from txt file
    double matrix[4][4];
    std::ifstream file(transform_file);
    if (file.is_open()) {
      for (size_t i = 0; i < 4; i++) {
        for (size_t j = 0; j < 4; j++) {
          if (!(file >> matrix[i][j])) {
            throw std::string("Error on read transform file: " + transform_file);
          }
        }
      }
    } else {
      throw std::string("Unable to open file: " + transform_file);
    }

    // Transform
    Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
    for (size_t i = 0; i < 4; i++) {
      for (size_t j = 0; j < 4; j++) {
        transform(i, j) = matrix[i][j];
      }
    }
    pcl::transformPointCloudWithNormals(*source_cloud, *transformed_cloud, transform);

    pcl::io::savePLYFileBinary(output_file_name, *transformed_cloud);

    return 0;
  } catch (boost::program_options::error& msg) {
    std::cout << "ERROR: " << msg.what() << std::endl;
  } catch (std::string msg) {
    std::cout << msg << std::endl;
  }

  return -1;
}


/***
 *  $Id$
 **
 *  File: graph_dijkstra.hpp
 *  Created: May 9, 2012
 *
 *  Author: Olga Wodo, Baskar Ganapathysubramanian
 *  Copyright (c) 2012 Olga Wodo, Baskar Ganapathysubramanian
 *  See accompanying LICENSE.
 *
 *  This file is part of GraSPI.
 */

#ifndef GRAPH_DIJKSTRA_HPP
#define GRAPH_DIJKSTRA_HPP

#include "graspi_types.hpp"
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/filtered_graph.hpp>

namespace graspi {

  template<typename Pred>
  inline void determine_shortest_distances(graph_t*G,
					   const edge_weights_t& W,
					   vertex_t source,
					   const Pred& pred,
					   std::vector<float>& d){
      boost::filtered_graph<graph_t, Pred> FG(*G,pred);
      unsigned int n = boost::num_vertices(*G);
      std::vector<vertex_t> p(n);
      std::fill(d.begin(), d.end(), 0.0);
      for (unsigned int i = 0; i < n; ++i) p[i] = i;

      boost::dijkstra_shortest_paths(FG, source,
				     boost::predecessor_map(&p[0])
				     .
				     distance_map(&d[0]).weight_map(W));
  }
}//graspi-namespace

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_ALLBITS_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_ALLBITS_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Allbits Allbits (function template)

  Generates a constant with all bits set to 1.

  @headerref{<boost/simd/constant/allbits.hpp>}

  @par Description

  1.  @code
      template<typename T> T Allbits();
      @endcode

  2.  @code
      template<typename T> T Allbits( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T with all bits set to 1.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to

  - `Nan<T>()` if @c T models IEEEValue
  - `T(~0)` if @c T models IntegerValue

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/allbits.hpp>
#include <boost/simd/constant/simd/allbits.hpp>

#endif


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee 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.
 */

#include <ekf_bag/ekf_bag_csv.h>

#include <camera/camera_params.h>
#include <Eigen/Core>

namespace ekf_bag {

Eigen::Vector3f QuatToEuler(const geometry_msgs::Quaternion & q) {
  Eigen::Vector3f euler;
  float q2q2 = q.y * q.y;
  euler.x() = atan2(2 * (q.x * q.w + q.y * q.z), 1 - 2 * (q.x * q.x + q2q2));
  float arg = std::max(-1.0, std::min(1.0, 2 * (q.y * q.w - q.x * q.z)));
  euler.y() = asin(arg);
  euler.z() = atan2(2 * (q.x * q.y + q.z * q.w), 1 - 2 * (q2q2 + q.z * q.z));
  return euler;
}

Eigen::Vector3f QuatToEuler(const Eigen::Quaternionf & q) {
  Eigen::Vector3f euler;
  float q2q2 = q.y() * q.y();
  euler.x() = atan2(2 * (q.x() * q.w() + q.y() * q.z()), 1 - 2 * (q.x() * q.x() + q2q2));
  float arg = std::max(-1.0f, std::min(1.0f, 2 * (q.y() * q.w() - q.x() * q.z())));
  euler.y() = asin(arg);
  euler.z() = atan2(2 * (q.x() * q.y() + q.z() * q.w()), 1 - 2 * (q2q2 + q.z() * q.z()));
  return euler;
}

EkfBagCsv::EkfBagCsv(const char* bagfile, const char* mapfile, const char* csvfile) :
          EkfBag(bagfile, mapfile),
          start_time_set_(false) {
  // virtual function has to be called in subclass since not initialized in superclass
  config_reader::ConfigReader config;
  ReadParams(&config);

  f_ = fopen(csvfile, "w");
  if (f_ == NULL) {
    fprintf(stderr, "Failed to open file %s.", csvfile);
    exit(0);
  }
}

EkfBagCsv::~EkfBagCsv(void) {
  fclose(f_);
}

void EkfBagCsv::ReadParams(config_reader::ConfigReader* config) {
  EkfBag::ReadParams(config);

  camera::CameraParameters cam_params(config, "nav_cam");
  tracked_of_.SetCameraParameters(cam_params);
}

void EkfBagCsv::UpdateEKF(const ff_msgs::EkfState & s) {
  EkfBag::UpdateEKF(s);

  if (!start_time_set_) {
    start_time_ = s.header.stamp;
    start_time_set_ = true;
  }
  fprintf(f_, "EKF %g ", (s.header.stamp - start_time_).toSec());
  fprintf(f_, "%g %g %g ", s.pose.position.x, s.pose.position.y, s.pose.position.z);
  Eigen::Vector3f euler = QuatToEuler(s.pose.orientation);
  fprintf(f_, "%g %g %g ", euler.x(), euler.y(), euler.z());
  fprintf(f_, "%g %g %g ", s.velocity.x, s.velocity.y, s.velocity.z);
  fprintf(f_, "%g %g %g ", s.omega.x, s.omega.y, s.omega.z);
  fprintf(f_, "%g %g %g ", s.accel.x, s.accel.y, s.accel.z);
  fprintf(f_, "%g %g %g ", s.accel_bias.x, s.accel_bias.y, s.accel_bias.z);
  fprintf(f_, "%g %g %g ", s.gyro_bias.x, s.gyro_bias.y, s.gyro_bias.z);
  fprintf(f_, "%d ", s.confidence);
  fprintf(f_, "%d ", s.status);
  fprintf(f_, "%d %d", s.ml_count, s.of_count);
  for (int i = 0; i < 15; i++)
    fprintf(f_, " %g", s.cov_diag[i]);
  for (int i = 0; i < 50; i++)
    fprintf(f_, " %g", s.ml_mahal_dists[i]);
  fprintf(f_, "\n");
}

void EkfBagCsv::UpdateOpticalFlow(const ff_msgs::Feature2dArray & of) {
  EkfBag::UpdateOpticalFlow(of);

  if (!start_time_set_)
    return;
  float t = (of.header.stamp - start_time_).toSec();
  tracked_of_.UpdateFeatures(of, t);
  fprintf(f_, "OF %g ", t);
  fprintf(f_, "%d ", static_cast<int>(of.feature_array.size()));
  for (auto it = tracked_of_.begin(); it != tracked_of_.end(); it++) {
    const auto & a = (*it).second;
    fprintf(f_, "%d %g %g %g ", a.id, a.time, a.x, a.y);
  }
  fprintf(f_, "\n");
}

void EkfBagCsv::UpdateSparseMap(const ff_msgs::VisualLandmarks & vl) {
  EkfBag::UpdateSparseMap(vl);

  if (!start_time_set_)
    return;
  const camera::CameraParameters & params = map_.GetCameraParameters();
  fprintf(f_, "VL %g ", (vl.header.stamp - start_time_).toSec());
  fprintf(f_, "%d ", static_cast<int>(vl.landmarks.size()));
  Eigen::Vector3f trans = ekf_.GetNavCamToBody().translation().cast<float>();
  Eigen::Quaternionf q1(0, trans.x(), trans.y(), trans.z());
  Eigen::Quaternionf q2(vl.pose.orientation.w, vl.pose.orientation.x, vl.pose.orientation.y, vl.pose.orientation.z);
  Eigen::Quaternionf cam_to_body_q(ekf_.GetNavCamToBody().rotation().cast<float>());
  cam_to_body_q.w() = -cam_to_body_q.w();
  q2 = q2 * cam_to_body_q;
  Eigen::Quaternionf t = (q2 * q1) * q2.conjugate();
  Eigen::Vector3f r(vl.pose.position.x, vl.pose.position.y, vl.pose.position.z);
  r -= Eigen::Vector3f(t.x(), t.y(), t.z());
  fprintf(f_, "%g %g %g ", r.x(), r.y(), r.z());
  Eigen::Vector3f euler = QuatToEuler(q2);
  fprintf(f_, "%g %g %g ", euler.x(), euler.y(), euler.z());
  for (unsigned int i = 0; i < vl.landmarks.size(); i++) {
    Eigen::Vector2d input(vl.landmarks[i].u, vl.landmarks[i].v);
    Eigen::Vector2d output;
    params.Convert<camera::UNDISTORTED_C, camera::DISTORTED_C>(input, &output);
    fprintf(f_, "%g %g %g %g %g ", output.x(), output.y(), vl.landmarks[i].x, vl.landmarks[i].y, vl.landmarks[i].z);
  }
  fprintf(f_, "\n");
}

void EkfBagCsv::UpdateGroundTruth(const geometry_msgs::PoseStamped & gt) {
  EkfBag::UpdateGroundTruth(gt);

  if (f_ == NULL || !start_time_set_)
    return;
  fprintf(f_, "GT %g ", (gt.header.stamp - start_time_).toSec());
  fprintf(f_, "%g %g %g ", gt.pose.position.x, gt.pose.position.y, gt.pose.position.z);
  Eigen::Vector3f euler = QuatToEuler(gt.pose.orientation);
  fprintf(f_, "%g %g %g\n", euler.x(), euler.y(), euler.z());
}

}  // namespace ekf_bag


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      References for deltaV computation function:
 *          Melman J. Trajectory optimization for a mission to Neptune and Triton, MSc thesis
 *              report, Delft University of Technology, 2007.
 *          Musegaas, P., Optimization of Space Trajectories Including Multiple Gravity Assists and
 *              Deep Space Maneuvers, MSc thesis report, Delft University of Technology, 2012.
 *              [unpublished so far].
 *      Reference for unpowered gravity assist propagation function:
 *          Conway, B.A., Spacecraft Trajectory Optimization, Chapter 7, Cambridge University
 *              Press, 2010.
 *      Reference for powered gravity assist propagation function:
 *          Musegaas, P., Optimization of Space Trajectories Including Multiple Gravity Assists and
 *              Deep Space Maneuvers, MSc thesis report, Delft University of Technology, 2012.
 *              [unpublished so far].
 *
 *    Notes
 *      Gravity assist and swing-by are two different words for the same thing. The delta-V that is
 *      computed for a powered swing-by has not been proven to be the optimum (lowest) to achieve
 *      the desired geometry of incoming and outgoing hyperbolic legs. Some literature research will
 *      have to be done to look at the alternatives.
 *
 *      Note that the exact implementation of Newton Raphson as root finder should be updated if
 *      someone would want to use a different root finding technique.
 *
 *      Note that by default a velocity effect deltaV of less than 1 micrometer/second is deemed
 *      negligable in this code. This value can be set though.
 *
 */

#include <cmath>

#include <boost/bind.hpp>

#include <Eigen/Dense>

#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"

#include "Tudat/Astrodynamics/MissionSegments/gravityAssist.h"
#include "Tudat/Mathematics/BasicMathematics/functionProxy.h"
#include "Tudat/Mathematics/RootFinders/bisection.h"

namespace tudat
{
namespace mission_segments
{

using namespace root_finders;

//! Calculate deltaV of a gravity assist.
double gravityAssist( const double centralBodyGravitationalParameter,
                      const Eigen::Vector3d& centralBodyVelocity,
                      const Eigen::Vector3d& incomingVelocity,
                      const Eigen::Vector3d& outgoingVelocity,
                      const double smallestPeriapsisDistance,
                      const bool useEccentricityInsteadOfPericenter,
                      const double speedTolerance,
                      RootFinderPointer rootFinder )
{
    using basic_mathematics::UnivariateProxyPointer;
    using basic_mathematics::UnivariateProxy;

    // Compute incoming and outgoing hyperbolic excess velocity.
    const Eigen::Vector3d incomingHyperbolicExcessVelocity
            = incomingVelocity - centralBodyVelocity;
    const Eigen::Vector3d outgoingHyperbolicExcessVelocity
            = outgoingVelocity - centralBodyVelocity;

    // Compute absolute values of the hyperbolic excess velocities.
    const double absoluteIncomingExcessVelocity = incomingHyperbolicExcessVelocity.norm( );
    const double absoluteOutgoingExcessVelocity = outgoingHyperbolicExcessVelocity.norm( );

    // Compute bending angle.
    double bendingAngle = linear_algebra::computeAngleBetweenVectors(
                            incomingHyperbolicExcessVelocity, outgoingHyperbolicExcessVelocity );

    // Compute maximum achievable bending angle.
    const double maximumBendingAngle =
            std::asin( 1.0 / ( 1.0 + ( smallestPeriapsisDistance *
                                       absoluteIncomingExcessVelocity *
                                       absoluteIncomingExcessVelocity /
                                       centralBodyGravitationalParameter ) ) ) +
            std::asin( 1.0 / ( 1.0 + ( smallestPeriapsisDistance *
                                       absoluteOutgoingExcessVelocity *
                                       absoluteOutgoingExcessVelocity /
                                       centralBodyGravitationalParameter ) ) );

    // Initialize bending effect deltaV, which is zero, unless extra bending angle is required.
    double bendingEffectDeltaV = 0.0;

    // Initialize velocity effect delta V parameter.
    double velocityEffectDeltaV = 0.0;

    // Check if an additional bending angle is required. If so, the additional bending angle
    // maneuver has to be performed. Also the pericenter radius will be the minimum pericenter
    // radius to obtain the largest possible bending angle 'for free'. Hence no root finding is
    // required for this case. As noted above, this may not be ideal for all cases. (for cases
    // in which the excess velocities are relatively small)
    if ( bendingAngle > maximumBendingAngle )
    {
        // Compute required extra bending angle that cannot be delivered by an unpowered swing-by.
        const double extraBendingAngle = bendingAngle - maximumBendingAngle;

        // Compute necessary delta-V due to bending-effect.
        bendingEffectDeltaV = 2.0 * std::min( absoluteIncomingExcessVelocity,
                                              absoluteOutgoingExcessVelocity ) *
                                    std::sin( extraBendingAngle / 2.0 );

        // This means the pericenter radius is now equal to the smallest pericenter radius, to
        // ensure the largest possible bending angle.
        const double pericenterRadius = smallestPeriapsisDistance;

        // Compute semi-major axis of hyperbolic legs.
        const double incomingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteIncomingExcessVelocity /
                                             absoluteIncomingExcessVelocity;
        const double outgoingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteOutgoingExcessVelocity /
                                             absoluteOutgoingExcessVelocity;

        // Compute incoming hyperbolic leg eccentricity.
        const double incomingEccentricity = 1 - pericenterRadius / incomingSemiMajorAxis;

        // Compute outgoing hyperbolic leg eccentricity.
        const double outgoingEccentricity = 1 - pericenterRadius / outgoingSemiMajorAxis;

        // Compute incoming and outgoing velocities at periapsis.
        const double incomingVelocityAtPeriapsis = absoluteIncomingExcessVelocity *
                std::sqrt( ( incomingEccentricity + 1.0 ) / ( incomingEccentricity - 1.0 ) );
        const double outgoingVelocityAtPeriapsis = absoluteOutgoingExcessVelocity *
                std::sqrt( ( outgoingEccentricity + 1.0 ) / ( outgoingEccentricity - 1.0 ) );

        // Compute necessary delta-V due to velocity-effect.
        velocityEffectDeltaV = std::fabs( incomingVelocityAtPeriapsis -
                                          outgoingVelocityAtPeriapsis );
    }

    else if ( ( std::fabs( absoluteIncomingExcessVelocity - absoluteOutgoingExcessVelocity )
                <= speedTolerance ) )
    {
        // In this case no maneuver has to be performed. Hence no iteration is performed, and the
        // delta V is simply kept at 0.0.
    }

    // Here the required maneuver to patch the incoming and outgoing excess velocities is
    // calculated. In this implementation, the eccentricity will be used as iteration parameter.
    else if ( useEccentricityInsteadOfPericenter )
    {
        // Compute semi-major axis of hyperbolic legs.
        const double incomingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteIncomingExcessVelocity /
                                             absoluteIncomingExcessVelocity;
        const double outgoingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteOutgoingExcessVelocity /
                                             absoluteOutgoingExcessVelocity;

        // Set the gravity assist function with the variables to perform root finder calculations.
        EccentricityFindingFunctions eccentricityFindingFunctions( incomingSemiMajorAxis,
                                                                   outgoingSemiMajorAxis,
                                                                   bendingAngle );

        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = std::make_shared< UnivariateProxy >(
                    std::bind( &EccentricityFindingFunctions::
                                 computeIncomingEccentricityFunction,
                                 eccentricityFindingFunctions, std::placeholders::_1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, std::bind(
                                      &EccentricityFindingFunctions::
                                      computeFirstDerivativeIncomingEccentricityFunction,
                                      eccentricityFindingFunctions, std::placeholders::_1 ) );

        // Initialize incoming eccentricity.
        double incomingEccentricity = TUDAT_NAN;

        // Set initial guess of the variable computed in Newton-Rapshon method.
        if ( ( absoluteOutgoingExcessVelocity / absoluteIncomingExcessVelocity ) < 100.0 )
        {
            // In these cases the very low estimate (which is given under else) may in some cases
            // result in no convergence. Hence a higher value of 1.01 is necessary. This will not
            // result in 'going through' 1.0 as mentioned below, because the eccentricity in these
            // cases is always high!
            try
            {
                incomingEccentricity = rootFinder->execute( rootFunction, 1.0 + 1.0e-2 );
            }
            catch(std::runtime_error)
            {
                root_finders::RootFinderPointer rootFinder_temp
                  = std::make_shared< root_finders::Bisection >( 1.0e-12, 1000 ) ;
                incomingEccentricity = rootFinder_temp->execute( rootFunction, 1.0 + 1.0e-2 );

            }
        }

        else
        {
            // This is set to a value that is close to 1.0. This is more robust than higher values,
            // because for those higher values Newton Raphson sometimes 'goes through' 1.0. This
            // results in NaN values for the derivative of the eccentricity finding function.
            try
            {
                incomingEccentricity = rootFinder->execute( rootFunction, 1.0 + 1.0e-10 );
            }
            catch(std::runtime_error)
            {
                root_finders::RootFinderPointer rootFinder_temp
                  = std::make_shared< root_finders::Bisection >( 1.0e-12, 1000 ) ;
                incomingEccentricity = rootFinder_temp->execute( rootFunction, 1.0 + 1.0e-10 );

            }
        }

        // Compute outgoing hyperbolic leg eccentricity.
        const double outgoingEccentricity = 1.0 - ( incomingSemiMajorAxis /
                                                    outgoingSemiMajorAxis ) *
                                            ( 1.0 - incomingEccentricity );

        // Compute incoming and outgoing velocities at periapsis.
        const double incomingVelocityAtPeriapsis = absoluteIncomingExcessVelocity *
                std::sqrt( ( incomingEccentricity + 1.0 ) / ( incomingEccentricity - 1.0 ) );
        const double outgoingVelocityAtPeriapsis = absoluteOutgoingExcessVelocity *
                std::sqrt( ( outgoingEccentricity + 1.0 ) / ( outgoingEccentricity - 1.0 ) );

        // Compute necessary delta-V due to velocity-effect.
        velocityEffectDeltaV = std::fabs( incomingVelocityAtPeriapsis -
                                          outgoingVelocityAtPeriapsis );
    }

    // Here the required maneuver to patch the incoming and outgoing excess velocities is
    // calculated. In this implementation, the pericenter radius will be used as iteration
    // parameter.
    else
    {
        // Compute semi-major axis of hyperbolic legs. This is the absolute semi-major axis, because
        // it will otherwisely result in the root of a negative function for various cases during
        // the rootfinding process.
        const double absoluteIncomingSemiMajorAxis = 1.0 * centralBodyGravitationalParameter /
                                                     absoluteIncomingExcessVelocity /
                                                     absoluteIncomingExcessVelocity;
        const double absoluteOutgoingSemiMajorAxis = 1.0 * centralBodyGravitationalParameter /
                                                     absoluteOutgoingExcessVelocity /
                                                     absoluteOutgoingExcessVelocity;

        // Set the gravity assist function with the variables to perform root finder calculations.
        PericenterFindingFunctions pericenterFindingFunctions( absoluteIncomingSemiMajorAxis,
                                                               absoluteOutgoingSemiMajorAxis,
                                                               bendingAngle);

        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = std::make_shared< UnivariateProxy >(
                    std::bind( &PericenterFindingFunctions::computePericenterRadiusFunction,
                                 pericenterFindingFunctions, std::placeholders::_1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, std::bind( &PericenterFindingFunctions::
                                                   computeFirstDerivativePericenterRadiusFunction,
                                                   pericenterFindingFunctions, std::placeholders::_1 ) );

        // Set pericenter radius based on result of Newton-Raphson root-finding algorithm.
        const double pericenterRadius = rootFinder->execute( rootFunction,
                                                             smallestPeriapsisDistance );

        // Compute incoming hyperbolic leg eccentricity.
        const double incomingEccentricity = 1.0 + pericenterRadius / absoluteIncomingSemiMajorAxis;

        // Compute outgoing hyperbolic leg eccentricity.
        const double outgoingEccentricity = 1.0 + pericenterRadius / absoluteOutgoingSemiMajorAxis;

        // Compute incoming and outgoing velocities at periapsis.
        const double incomingVelocityAtPeriapsis = absoluteIncomingExcessVelocity *
                std::sqrt( ( incomingEccentricity + 1.0 ) / ( incomingEccentricity - 1.0 ) );
        const double outgoingVelocityAtPeriapsis = absoluteOutgoingExcessVelocity *
                std::sqrt( ( outgoingEccentricity + 1.0 ) / ( outgoingEccentricity - 1.0 ) );

        // Compute necessary delta-V due to velocity-effect.
        velocityEffectDeltaV = std::fabs( incomingVelocityAtPeriapsis -
                                          outgoingVelocityAtPeriapsis );
    }

    // Compute and return the total delta-V.
    return bendingEffectDeltaV + velocityEffectDeltaV;
}

//! Propagate an unpowered gravity assist.
Eigen::Vector3d gravityAssist( const double centralBodyGravitationalParameter,
                               const Eigen::Vector3d& centralBodyVelocity,
                               const Eigen::Vector3d& incomingVelocity,
                               const double rotationAngle,
                               const double pericenterRadius )
{
    // Calculate the incoming velocity.
    const Eigen::Vector3d relativeIncomingVelocity = incomingVelocity - centralBodyVelocity;
    const double absoluteRelativeIncomingVelocity = relativeIncomingVelocity.norm( );

    // Calculate the eccentricity and bending angle.
    const double eccentricity = 1.0 + pericenterRadius / centralBodyGravitationalParameter *
                            absoluteRelativeIncomingVelocity * absoluteRelativeIncomingVelocity;
    const double bendingAngle = 2.0 * std::asin ( 1.0 / eccentricity );

    // Calculate the unit vectors.
    const Eigen::Vector3d unitVector1 = relativeIncomingVelocity /
                                        absoluteRelativeIncomingVelocity;
    const Eigen::Vector3d unitVector2 = unitVector1.cross( centralBodyVelocity ).normalized( );
    const Eigen::Vector3d unitVector3 = unitVector1.cross( unitVector2 );

    // Calculate the relative outgoing velocity.
    const Eigen::Vector3d relativeOutgoingVelocity = absoluteRelativeIncomingVelocity *
            ( std::cos( bendingAngle ) * unitVector1 + std::sin( bendingAngle ) *
              std::cos( rotationAngle ) * unitVector2 + std::sin( bendingAngle ) *
              std::sin( rotationAngle ) * unitVector3 );

    // Add the relative outgoing velocity to the swing-by body velocity and return it.
    return centralBodyVelocity + relativeOutgoingVelocity;
}

//! Propagate a powered gravity assist.
Eigen::Vector3d gravityAssist( const double centralBodyGravitationalParameter,
                               const Eigen::Vector3d& centralBodyVelocity,
                               const Eigen::Vector3d& incomingVelocity,
                               const double rotationAngle,
                               const double pericenterRadius,
                               const double deltaV )
{
    // Calculate the incoming velocity.
    const Eigen::Vector3d relativeIncomingVelocity = incomingVelocity - centralBodyVelocity;
    const double absoluteRelativeIncomingVelocity = relativeIncomingVelocity.norm( );

    // Calculate the incoming eccentricity and bending angle.
    const double incomingEccentricity = 1.0 + pericenterRadius /
                                        centralBodyGravitationalParameter *
                                        absoluteRelativeIncomingVelocity *
                                        absoluteRelativeIncomingVelocity;
    const double incomingBendingAngle = std::asin ( 1.0 / incomingEccentricity );

    // Calculate the pericenter velocities.
    const double incomingPericenterVelocity = std::sqrt( absoluteRelativeIncomingVelocity *
                                                         absoluteRelativeIncomingVelocity *
                                                         ( incomingEccentricity + 1.0 ) /
                                                         ( incomingEccentricity - 1.0 ) );
    const double outgoingPericenterVelocity = incomingPericenterVelocity + deltaV;

    // Calculate magnitude of the absolute relative outgoing velocity.
    const double absoluteRelativeOutgoingVelocity =
            std::sqrt( outgoingPericenterVelocity * outgoingPericenterVelocity -
                       2.0 * centralBodyGravitationalParameter / pericenterRadius );

    // Calculate the remaining bending angles.
    const double outgoingBendingAngle =
            std::asin ( 1.0 / ( 1.0 + absoluteRelativeOutgoingVelocity *
                                absoluteRelativeOutgoingVelocity * pericenterRadius /
                                centralBodyGravitationalParameter ) );
    const double bendingAngle = incomingBendingAngle + outgoingBendingAngle;

    // Calculate the unit vectors.
    const Eigen::Vector3d unitVector1 = relativeIncomingVelocity /
                                        absoluteRelativeIncomingVelocity;
    const Eigen::Vector3d unitVector2 = unitVector1.cross( centralBodyVelocity ).normalized( );
    const Eigen::Vector3d unitVector3 = unitVector1.cross( unitVector2 );

    // Calculate the relative outgoing velocity.
    const Eigen::Vector3d relativeOutgoingVelocity = absoluteRelativeOutgoingVelocity *
            ( std::cos( bendingAngle ) * unitVector1 + std::sin( bendingAngle ) *
              std::cos( rotationAngle ) * unitVector2 + std::sin( bendingAngle ) *
              std::sin( rotationAngle ) * unitVector3 );

    // Add the relative outgoing velocity to the swing-by body velocity and return it.
    return centralBodyVelocity + relativeOutgoingVelocity;
}

//! Compute pericenter radius function.
double PericenterFindingFunctions::computePericenterRadiusFunction( const double pericenterRadius )
{
    return std::asin( absoluteIncomingSemiMajorAxis_ / ( absoluteIncomingSemiMajorAxis_ +
                                                         pericenterRadius ) ) +
           std::asin( absoluteOutgoingSemiMajorAxis_ / ( absoluteOutgoingSemiMajorAxis_ +
                                                         pericenterRadius ) ) - bendingAngle_;
}

//! Compute first-derivative of the pericenter radius function.
double PericenterFindingFunctions::computeFirstDerivativePericenterRadiusFunction(
        const double pericenterRadius )
{
    return -absoluteIncomingSemiMajorAxis_ / ( absoluteIncomingSemiMajorAxis_ + pericenterRadius )
            / std::sqrt( ( pericenterRadius + 2.0 * absoluteIncomingSemiMajorAxis_ )
                         * pericenterRadius ) -
           absoluteOutgoingSemiMajorAxis_ / ( absoluteOutgoingSemiMajorAxis_ + pericenterRadius )
            / std::sqrt( ( pericenterRadius + 2.0 * absoluteOutgoingSemiMajorAxis_ ) *
                         pericenterRadius );
}

//! Compute incoming eccentricity function.
double EccentricityFindingFunctions::computeIncomingEccentricityFunction(
        const double incomingEccentricity )
{
    return std::asin( 1.0 / incomingEccentricity )
            + std::asin( 1.0 / ( 1.0 - incomingSemiMajorAxis_ / outgoingSemiMajorAxis_ *
                                 ( 1.0 - incomingEccentricity ) ) ) - bendingAngle_;
}

//! Compute first-derivative of the incoming eccentricity function.
double EccentricityFindingFunctions::computeFirstDerivativeIncomingEccentricityFunction(
        const double incomingEccentricity )
{
    const double eccentricitySquareMinusOne_ = incomingEccentricity * incomingEccentricity - 1.0;
    const double semiMajorAxisRatio_ = incomingSemiMajorAxis_ / outgoingSemiMajorAxis_ ;
    const double bParameter_ = 1.0 - semiMajorAxisRatio_ * ( 1.0 - incomingEccentricity );

    return -1.0 / ( incomingEccentricity * std::sqrt( eccentricitySquareMinusOne_ ) ) -
            semiMajorAxisRatio_ / ( bParameter_ * std::sqrt( bParameter_ * bParameter_ - 1.0 ) );
}

} // namespace mission_segments
} // namespace tudat


/**************************************************************
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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.
 *
 *************************************************************/

#ifndef  _USE_MATH_DEFINES
#define  _USE_MATH_DEFINES  // needed by Visual C++ for math constants
#endif
#include <math.h>

#include <comphelper/processfactory.hxx>
#include <comphelper/regpathhelper.hxx>
#include <cppuhelper/servicefactory.hxx>
#include <cppuhelper/bootstrap.hxx>
#include <com/sun/star/lang/XMultiServiceFactory.hpp>
#include <com/sun/star/lang/XInitialization.hpp>
#include <com/sun/star/registry/XSimpleRegistry.hpp>

#include <ucbhelper/contentbroker.hxx>
#include <ucbhelper/configurationkeys.hxx>

#include <vcl/window.hxx>
#include <vcl/svapp.hxx>
#include <vcl/msgbox.hxx>
#include <vcl/unowrap.hxx>
#include <vcl/bitmap.hxx>
#include <vcl/bmpacc.hxx>

#include <basegfx/polygon/b2dlinegeometry.hxx>
#include <basegfx/polygon/b2dpolypolygon.hxx>
#include <basegfx/polygon/b2dpolypolygontools.hxx>
#include <basegfx/polygon/b2dpolypolygonrasterconverter.hxx>
#include <basegfx/polygon/b2dpolygontools.hxx>
#include <basegfx/polygon/b2dpolygon.hxx>
#include <basegfx/numeric/ftools.hxx>
#include <basegfx/range/b2irange.hxx>
#include <basegfx/vector/b2isize.hxx>
#include <basegfx/point/b2ipoint.hxx>

#include <basebmp/color.hxx>
#include <basebmp/scanlineformats.hxx>
#include <basebmp/bitmapdevice.hxx>
#include <basebmp/debug.hxx>

#include <rtl/bootstrap.hxx>

#include <vigra/metaprogramming.hxx>
#include <vigra/static_assert.hxx>
#include <vigra/basicimageview.hxx>

#include <boost/static_assert.hpp>
#include <algorithm>
#include <iostream>
#include <fstream>

using namespace ::com::sun::star;

namespace
{

/// template meta function: add const qualifier, if given 2nd type has it
template<typename A, typename B> struct clone_const
{
    typedef B type;
};
template<typename A, typename B> struct clone_const<const A,B>
{
    typedef const B type;
};

template< class DestIterator, class DestAccessor > class Renderer :
        public basegfx::B2DPolyPolygonRasterConverter
{
private:
    typename DestIterator::value_type fillColor_;
    typename DestIterator::value_type clearColor_;
    DestIterator                      begin_;
    DestAccessor                      accessor_;

public:
    Renderer(const basegfx::B2DPolyPolygon&     rPolyPolyRaster,
             typename DestIterator::value_type  fillColor,
             typename DestIterator::value_type  clearColor,
             DestIterator                       begin,
             DestIterator                       end,
             DestAccessor                       accessor ) :
        B2DPolyPolygonRasterConverter(rPolyPolyRaster,
                                      basegfx::B2DRange(0,0,
                                                        end.x - end.x,
                                                        begin.y - begin.y )),
        fillColor_( fillColor ),
        clearColor_( clearColor ),
        begin_( begin ),
        accessor_( accessor )
    {
    }

    virtual void span(const double& rfXLeft,
                      const double& rfXRight,
                      sal_Int32 	nY,
                      bool 			bOn )
    {
        DestIterator currIter( begin_ + vigra::Diff2D(0,nY) );
        typename DestIterator::row_iterator rowIter( currIter.rowIterator() +
                                                     basegfx::fround(rfXLeft) );
        typename DestIterator::row_iterator rowEnd( currIter.rowIterator() +
                                                    basegfx::fround(rfXRight) );
        if( bOn )
            while( rowIter != rowEnd )
            {
                accessor_.set(fillColor_, rowIter);
                ++rowIter;
            }
        else
            while( rowIter != rowEnd )
            {
                accessor_.set(accessor_(rowIter)*clearColor_, rowIter);
                ++rowIter;
            }
    }
};

template< class DestIterator, class DestAccessor >
    std::auto_ptr< Renderer< DestIterator, DestAccessor > > makeRenderer(
        const basegfx::B2DPolyPolygon&                          rPolyPolyRaster,
        typename DestIterator::value_type                       fillColor,
        typename DestIterator::value_type                       clearColor,
        vigra::triple<DestIterator, DestIterator, DestAccessor> dest )
{
    return std::auto_ptr< Renderer< DestIterator, DestAccessor > >(
        new Renderer< DestIterator, DestAccessor >(rPolyPolyRaster,
                                                   fillColor,
                                                   clearColor,
                                                   dest.first,
                                                   dest.second,
                                                   dest.third));
}

// changed semantics re. DirectionSelector<StridedArrayTag>: stride
// now counts in raw bytes!
template< typename T > class StridedArrayIterator
{
public:
    typedef typename clone_const<T, unsigned char>::type  internal_type;

    StridedArrayIterator(int stride, T* ptr = 0) :
        stride_(stride),
        current_(reinterpret_cast<internal_type*>(ptr))
    {}

    /// Copy from other StridedArrayIterator, plus given offset
    StridedArrayIterator( StridedArrayIterator const& rSrc,
                          int                         offset ) :
        stride_(rSrc.stride_),
        current_(reinterpret_cast<internal_type*>(
                     reinterpret_cast<T*>(rSrc.current_)+offset))
    {}

    void operator++() {current_ += stride_; }
    void operator++(int) {current_ += stride_; }
    void operator--() {current_ -= stride_; }
    void operator--(int) {current_ -= stride_; }
    void operator+=(int dy) {current_ += dy*stride_; }
    void operator-=(int dy) {current_ -= dy*stride_; }

    bool operator==(StridedArrayIterator const & rhs) const
    { return (current_ == rhs.current_); }

    bool operator!=(StridedArrayIterator const & rhs) const
    { return (current_ != rhs.current_); }

    bool operator<(StridedArrayIterator const & rhs) const
    { return (current_ < rhs.current_); }

    bool operator<=(StridedArrayIterator const & rhs) const
    { return (current_ <= rhs.current_); }

    bool operator>(StridedArrayIterator const & rhs) const
    { return (current_ > rhs.current_); }

    bool operator>=(StridedArrayIterator const & rhs) const
    { return (current_ >= rhs.current_); }

    int operator-(StridedArrayIterator const & rhs) const
    { return (current_ - rhs.current_) / stride_; }

    T* operator()() const
    { return reinterpret_cast<T*>(current_); }

    T* operator()(int d) const
    { return reinterpret_cast<T*>(current_ + d*stride_); }

    int            stride_;
    internal_type* current_;
};

/// template meta function: remove const qualifier from plain type
template <typename T> struct remove_const
{
    typedef T type;
};
template <typename T> struct remove_const<const T>
{
    typedef T type;
};

/// returns true, if given number is strictly less than 0
template< typename T > inline bool is_negative( T x )
{
    return x < 0;
}

/// Overload for ints (branch-free)
inline bool is_negative( int x )
{
    // force logic shift (result for signed shift right is undefined)
    return static_cast<unsigned int>(x) >> (sizeof(int)*8-1);
}

/// Get bitmask for data at given intra-word position, for given bit depth
template< typename data_type, int bits_per_pixel, bool MsbFirst, typename difference_type > inline data_type get_mask( difference_type d )
{
    BOOST_STATIC_ASSERT(bits_per_pixel > 0);
    BOOST_STATIC_ASSERT(sizeof(data_type)*8 % bits_per_pixel == 0);
    BOOST_STATIC_ASSERT(sizeof(data_type)*8 / bits_per_pixel > 1);
    BOOST_STATIC_ASSERT(vigra::TypeTraits<data_type>::isPOD::asBool);

    const unsigned int nIntraWordPositions( sizeof(data_type)*8 / bits_per_pixel );

    //      create bits_per_pixel 1s      shift to intra-word position
    return ((~(~0 << bits_per_pixel)) << bits_per_pixel*(MsbFirst ?
                                                         (nIntraWordPositions-1 - (d % nIntraWordPositions)) :
                                                         (d % nIntraWordPositions)));
}

template< int num_intraword_positions, int bits_per_pixel, bool MsbFirst, typename difference_type > inline difference_type get_shift( difference_type remainder )
{
    return bits_per_pixel*(MsbFirst ?
                           (num_intraword_positions - 1 - remainder) :
                           remainder);
}

template< typename Datatype,
          typename Valuetype,
          int      bits_per_pixel,
          bool     MsbFirst > class PackedPixelColumnIterator
{
public:
    // no reference, no index_reference type here
    typedef Datatype                                    data_type;
    typedef Valuetype                                   value_type;
    typedef int                                         difference_type;
    typedef image_traverser_tag                         iterator_category;

    typedef typename remove_const<data_type>::type      mask_type;
    typedef data_type*                                  pointer;
    typedef StridedArrayIterator< data_type >           MoveY;

    enum {
        /** The number of pixel within a single data_type value
         */
        num_intraword_positions=sizeof(data_type)*8/bits_per_pixel,
        /** Bit mask for one pixel (least significant bits)
         */
        bit_mask=~(~0 << bits_per_pixel)
    };

private:
    MoveY           y;
    mask_type       mask_;
    difference_type shift_;

    void inc()
    {
        ++y;
    }

    void dec()
    {
        --y;
    }

    bool equal( PackedPixelColumnIterator const & rhs ) const
    {
        return rhs.y == y;
    }

    bool less( PackedPixelColumnIterator const & rhs ) const
    {
        return y < rhs.y;
    }

public:
    PackedPixelColumnIterator() :
        y(0),
        mask_( get_mask<data_type, bits_per_pixel, MsbFirst, difference_type>(0) ),
        shift_( get_shift<num_intraword_positions, bits_per_pixel, MsbFirst, difference_type>(0) )
    {}

    PackedPixelColumnIterator( const MoveY& base, difference_type remainder ) :
        y(base),
        mask_( get_mask<data_type, bits_per_pixel, MsbFirst>(remainder) ),
        shift_( get_shift<num_intraword_positions, bits_per_pixel, MsbFirst>(remainder) )
    {}

    PackedPixelColumnIterator& operator+=( difference_type d )
    {
        y += d;
        return *this;
    }

    PackedPixelColumnIterator& operator-=( difference_type d )
    {
        y -= d;
        return *this;
    }

    PackedPixelColumnIterator operator+( difference_type d )
    {
        PackedPixelColumnIterator res(*this);
        res += d;
        return res;
    }

    PackedPixelColumnIterator operator-( difference_type d )
    {
        PackedPixelColumnIterator res(*this);
        res -= d;
        return res;
    }

    PackedPixelColumnIterator& operator++()
    {
        inc();
        return *this;
    }

    PackedPixelColumnIterator& operator--()
    {
        dec();
        return *this;
    }

    PackedPixelColumnIterator operator++(int)
    {
        PackedPixelColumnIterator res(*this);
        res.inc();
        return res;
    }

    PackedPixelColumnIterator operator--(int)
    {
        PackedPixelColumnIterator res(*this);
        res.dec();
        return res;
    }

    bool operator==(PackedPixelColumnIterator const & rhs) const
    {
        return equal( rhs );
    }

    bool operator!=(PackedPixelColumnIterator const & rhs) const
    {
        return !equal( rhs );
    }

    bool operator<(PackedPixelColumnIterator const & rhs) const
    {
        return less(rhs);
    }

    bool operator<=(PackedPixelColumnIterator const & rhs) const
    {
        return !less(rhs);
    }

    bool operator>(PackedPixelColumnIterator const & rhs) const
    {
        return rhs.less(*this);
    }

    bool operator>=(PackedPixelColumnIterator const & rhs) const
    {
        return !rhs.less(*this);
    }

    difference_type operator-(PackedPixelColumnIterator const & rhs) const
    {
        return y - rhs.y;
    }

    value_type get() const
    {
        // TODO(Q3): use traits to get unsigned type for data_type (if
        // not already)
        return static_cast<unsigned int>(*y() & mask_) >> shift_;
    }

    value_type get(difference_type d) const
    {
        // TODO(Q3): use traits to get unsigned type for data_type (if
        // not already)
        return static_cast<unsigned int>(*y(d) & mask_) >> shift_;
    }

    void set( value_type v ) const
    {
        const value_type pixel_value( (v << shift_) & mask_ );
        *y() = (*y() & ~mask_) | pixel_value;
    }

    void set( value_type v, difference_type d ) const
    {
        const value_type pixel_value( (v << shift_) & mask_ );
        *y(d) = (*y(d) & ~mask_) | pixel_value;
    }
};

template< typename Datatype,
          typename Valuetype,
          int      bits_per_pixel,
          bool     MsbFirst > class PackedPixelRowIterator
{
public:
    // no reference, no index_reference type here
    typedef Datatype                                    data_type;
    typedef Valuetype                                   value_type;
    typedef int                                         difference_type;
    typedef image_traverser_tag                         iterator_category;

    typedef typename remove_const<data_type>::type      mask_type;
    typedef data_type*                                  pointer;

    enum {
        /** The number of pixel within a single data_type value
         */
        num_intraword_positions=sizeof(data_type)*8/bits_per_pixel,
        /** Bit mask for one pixel (least significant bits)
         */
        bit_mask=~(~0 << bits_per_pixel)
    };

private:
    pointer         data_;
    mask_type       mask_;
    difference_type remainder_;

    void update_mask()
    {
        mask_ = get_mask<data_type, bits_per_pixel, MsbFirst>(remainder_);
    }

    void inc()
    {
        const difference_type newValue( remainder_ + 1 );
        const difference_type data_offset( newValue / num_intraword_positions );

        data_ += data_offset;
        remainder_ = newValue % num_intraword_positions;

        const mask_type shifted_mask(
            MsbFirst ?
            // TODO(Q3): use traits to get unsigned type for data_type
            // (if not already)
            static_cast<unsigned int>(mask_) >> bits_per_pixel :
            mask_ << bits_per_pixel );

        // data_offset is 0 for shifted mask, and 1 for wrapped-around mask
        mask_ = (1-data_offset)*shifted_mask + data_offset*(MsbFirst ?
                                                            bit_mask << bits_per_pixel*(num_intraword_positions-1) :
                                                            bit_mask);
    }

    void dec()
    {
        const difference_type newValue( remainder_ - 1 );
        const bool            isNegative( is_negative(newValue) );
        const difference_type newRemainder( newValue % num_intraword_positions );

        // calc  data_ += newValue / num_intraword_positions;
        //       remainder_ = newRemainder;
        // for newValue >= 0, and
        //       data_ += newValue / num_intraword_positions - 1;
        //       remainder_ = num_intraword_positions - newRemainder;
        // (to force remainder_ to be positive).
        // This is branch-free, if is_negative() is branch-free
        const difference_type data_offset( newValue / num_intraword_positions - isNegative );
        data_     += data_offset;
        remainder_ = newRemainder + isNegative*num_intraword_positions;

        const mask_type shifted_mask(
            MsbFirst ?
            mask_ << bits_per_pixel :
            // TODO(Q3): use traits to get unsigned type for data_type
            // (if not already)
            static_cast<unsigned int>(mask_) >> bits_per_pixel );

        // data_offset is 0 for shifted mask, and 1 for wrapped-around mask
        mask_ = (1-data_offset)*shifted_mask + data_offset*(MsbFirst ?
                                                            bit_mask :
                                                            bit_mask << bits_per_pixel*(num_intraword_positions-1));
    }

    bool equal( PackedPixelRowIterator const & rhs ) const
    {
        return rhs.data_ == data_ && rhs.remainder_ == remainder_;
    }

    bool less( PackedPixelRowIterator const & rhs ) const
    {
        return data_ == rhs.data_ ?
            (remainder_ < rhs.remainder_) :
            (data_ < rhs.data_);
    }

public:
    PackedPixelRowIterator() :
        data_(0),
        mask_( get_mask<data_type, bits_per_pixel, MsbFirst, difference_type>(0) ),
        remainder_(0)
    {}

    explicit PackedPixelRowIterator( pointer base ) :
        data_(base),
        mask_( get_mask<data_type, bits_per_pixel, MsbFirst, difference_type>(0) ),
        remainder_(0)
    {}

    PackedPixelRowIterator& operator+=( difference_type d )
    {
        const difference_type newValue( remainder_ + d );

        data_ += newValue / num_intraword_positions;
        remainder_ = newValue % num_intraword_positions;
        update_mask();

        return *this;
    }

    PackedPixelRowIterator& operator-=( difference_type d )
    {
        const difference_type newValue( remainder_ - d );
        const bool            isNegative( is_negative(newValue) );
        const difference_type newRemainder( newValue % num_intraword_positions );

        // calc  data_ += newValue / num_intraword_positions;
        //       remainder_ = newRemainder;
        // for newValue >= 0, and
        //       data_ += newValue / num_intraword_positions - 1;
        //       remainder_ = num_intraword_positions - newRemainder;
        // (to force remainder_ to be positive).
        // This is branch-free, if is_negative() is branch-free
        data_     += newValue / num_intraword_positions - isNegative;
        remainder_ = newRemainder + isNegative*(num_intraword_positions - 2*newRemainder);
        update_mask();

        return *this;
    }

    PackedPixelRowIterator operator+( difference_type d )
    {
        PackedPixelRowIterator res(*this);
        res += d;
        return res;
    }

    PackedPixelRowIterator operator-( difference_type d )
    {
        PackedPixelRowIterator res(*this);
        res -= d;
        return res;
    }

    PackedPixelRowIterator& operator++()
    {
        inc();
        return *this;
    }

    PackedPixelRowIterator& operator--()
    {
        dec();
        return *this;
    }

    PackedPixelRowIterator operator++(int)
    {
        PackedPixelRowIterator res(*this);
        res.inc();
        return res;
    }

    PackedPixelRowIterator operator--(int)
    {
        PackedPixelRowIterator res(*this);
        res.dec();
        return res;
    }

    bool operator==(PackedPixelRowIterator const & rhs) const
    {
        return equal( rhs );
    }

    bool operator!=(PackedPixelRowIterator const & rhs) const
    {
        return !equal( rhs );
    }

    bool operator<(PackedPixelRowIterator const & rhs) const
    {
        return less(rhs);
    }

    bool operator<=(PackedPixelRowIterator const & rhs) const
    {
        return !less(rhs);
    }

    bool operator>(PackedPixelRowIterator const & rhs) const
    {
        return rhs.less(*this);
    }

    bool operator>=(PackedPixelRowIterator const & rhs) const
    {
        return !rhs.less(*this);
    }

    difference_type operator-(PackedPixelRowIterator const & rhs) const
    {
        return (data_ - rhs.data_)*num_intraword_positions + (remainder_ - rhs.remainder_);
    }

    value_type get() const
    {
        // TODO(Q3): use traits to get unsigned type for data_type (if
        // not already)
        return static_cast<unsigned int>(*data_ & mask_) >>
            get_shift<num_intraword_positions, bits_per_pixel, MsbFirst>(remainder_);
    }

    value_type get(difference_type d) const
    {
        PackedPixelRowIterator tmp(*this);
        tmp += d;
        return tmp.get();
    }

    void set( value_type v ) const
    {
        const value_type pixel_value(
            (v <<
             get_shift<num_intraword_positions, bits_per_pixel, MsbFirst>(remainder_))
            & mask_ );
        *data_ = (*data_ & ~mask_) | pixel_value;
    }

    void set( value_type v, difference_type d ) const
    {
        PackedPixelRowIterator tmp(*this);
        tmp += d;
        tmp.set(v);
    }
};

template< typename Datatype,
          typename Valuetype,
          int      bits_per_pixel,
          bool     MsbFirst > class PackedPixelIterator
{
public:
    // no reference, no index_reference type here
    typedef Datatype                                    data_type;
    typedef Valuetype                                   value_type;
    typedef vigra::Diff2D                               difference_type;
    typedef image_traverser_tag                         iterator_category;
    typedef PackedPixelRowIterator<data_type,
                                   value_type,
                                   bits_per_pixel,
                                   MsbFirst>            row_iterator;
    typedef PackedPixelColumnIterator<data_type,
                                      value_type,
                                      bits_per_pixel,
                                      MsbFirst>         column_iterator;

    typedef data_type*                                  pointer;
    typedef int                                         MoveX;
    typedef StridedArrayIterator< data_type >           MoveY;

    enum {
        /** The number of pixel within a single data_type value
         */
        num_intraword_positions=sizeof(data_type)*8/bits_per_pixel,
        /** Bit mask for one pixel (least significant bits)
         */
        bit_mask=~(~0 << bits_per_pixel)
    };

    // TODO(F2): direction of iteration (ImageIterator can be made to
    // run backwards)

private:
    pointer current() const
    {
        return y() + (x / num_intraword_positions);
    }

    pointer current(int dx, int dy) const
    {
        return y(dy) + ((x+dx)/num_intraword_positions);
    }

    bool equal(PackedPixelIterator const & rhs) const
    {
        return (x == rhs.x) && (y == rhs.y);
    }

public:
    PackedPixelIterator() :
        x(0),
        y(0)
    {}

    PackedPixelIterator(pointer base, int ystride) :
        x(0),
        y(ystride,base)
    {}

    bool operator==(PackedPixelIterator const & rhs) const
    {
        return equal(rhs);
    }

    bool operator!=(PackedPixelIterator const & rhs) const
    {
        return !equal(rhs);
    }

    difference_type operator-(PackedPixelIterator const & rhs) const
    {
        return difference_type(x - rhs.x, y - rhs.y);
    }

    MoveX x;
    MoveY y;

    PackedPixelIterator & operator+=(difference_type const & s)
    {
        x += s.x;
        y += s.y;
        return *this;
    }

    PackedPixelIterator & operator-=(difference_type const & s)
    {
        x -= s.x;
        y -= s.y;
        return *this;
    }

    PackedPixelIterator operator+(difference_type const & s) const
    {
        PackedPixelIterator ret(*this);
        ret += s;
        return ret;
    }

    PackedPixelIterator operator-(difference_type const & s) const
    {
        PackedPixelIterator ret(*this);
        ret -= s;
        return ret;
    }

    row_iterator rowIterator() const
    {
        return row_iterator(current());
    }

    column_iterator columnIterator() const
    {
        return column_iterator(MoveY(y,
                                     x / num_intraword_positions),
                               x % num_intraword_positions);
    }

    value_type get() const
    {
        const int remainder( x() % num_intraword_positions );

        // TODO(Q3): use traits to get unsigned type for data_type (if
        // not already)
        return (static_cast<unsigned int>(*current() &
                                          get_mask<data_type, bits_per_pixel, MsbFirst>(remainder))
                >> (MsbFirst ?
                    (num_intraword_positions - remainder) :
                    remainder));
    }

    value_type get(difference_type const & d) const
    {
        const int remainder( x(d.x) % num_intraword_positions );

        // TODO(Q3): use traits to get unsigned type for data_type (if
        // not already)
        return (static_cast<unsigned int>(*current(d.x,d.y) &
                                          get_mask<data_type, bits_per_pixel, MsbFirst>(remainder))
                >> get_shift<num_intraword_positions, bits_per_pixel, MsbFirst>(remainder));
    }

    void set( value_type v ) const
    {
        const int remainder( x() % num_intraword_positions );
        const int mask( get_mask<data_type, bits_per_pixel, MsbFirst>(remainder) );
        const value_type pixel_value(
            (v <<
             get_shift<num_intraword_positions, bits_per_pixel, MsbFirst>(remainder))
            & mask );
        pointer p = current();
        *p = (*p & ~mask) | pixel_value;
    }

    void set( value_type v, difference_type const & d ) const
    {
        const int remainder( x(d.x) % num_intraword_positions );
        const int mask( get_mask<data_type, bits_per_pixel, MsbFirst>(remainder) );
        const value_type pixel_value(
            (v <<
             get_shift<num_intraword_positions, bits_per_pixel, MsbFirst>(remainder))
             & mask );
        pointer p = current(d.x,d.y);
        *p = (*p & ~mask) | pixel_value;
    }
};

/** Access (possibly packed-pixel) data via palette indirection
 */
template< typename Valuetype, typename Datatype > class PaletteImageAccessor
{
  public:
    typedef Valuetype                                   value_type;
    typedef Datatype                                    data_type;
    typedef typename remove_const<data_type>::type      count_type;

private:
    const BitmapColor* palette;
    count_type         num_entries;

    double norm( BitmapColor const& rLHS,
                 BitmapColor const& rRHS ) const
    {
        // convert RGBValue's linear space to a normed linear space
        return sqrt(
            vigra::sq(rLHS.GetRed()-rRHS.GetRed()) +
            vigra::sq(rLHS.GetGreen()-rRHS.GetGreen()) +
            vigra::sq(rLHS.GetBlue()-rRHS.GetBlue()) );
    }

    data_type find_best_match(value_type const& v) const
    {
        // TODO(F3): not generic!!!
        const BitmapColor aTmpCol(v.red(),
                                  v.green(),
                                  v.blue());

        // TODO(P3): use table-based/octree approach here!
        const BitmapColor* best_entry;
        const BitmapColor* palette_end( palette+num_entries );
        if( (best_entry=std::find( palette, palette_end, aTmpCol)) != palette_end )
            return best_entry-palette;

        // TODO(F3): HACK. Need palette traits, and an error function
        // here. We blatantly assume value_type is a normed linear
        // space.
        const BitmapColor* curr_entry( palette );
        best_entry = curr_entry;
        while( curr_entry != palette_end )
        {
            if( norm(*curr_entry,*best_entry) > norm(*curr_entry,aTmpCol) )
                best_entry = curr_entry;

            ++curr_entry;
        }

        return best_entry-palette;
    }

    value_type toCol( BitmapColor const& rCol ) const
    {
        return value_type(rCol.GetRed(),rCol.GetGreen(),rCol.GetBlue());
    }

public:
    PaletteImageAccessor() :
        palette(0),
        num_entries(0)
    {}

    PaletteImageAccessor( const BitmapColor* pPalette,
                          data_type          entries ) :
        palette(pPalette),
        num_entries(entries)
    {}

    template< class Iterator >
    value_type operator()(Iterator const& i) const { return toCol(palette[i.get()]); }
    value_type operator()(data_type const* i) const { return toCol(palette[*i]); }

    template< class Iterator, class Difference >
    value_type operator()(Iterator const& i, Difference const& diff) const
    {
        return toCol(palette[i.get(diff)]);
    }

    template< typename V, class Iterator >
    void set(V const& value, Iterator const& i) const
    {
        i.set(
            find_best_match(
                vigra::detail::RequiresExplicitCast<value_type>::cast(value) ));
    }

    template< typename V, class Iterator, class Difference >
    void set(V const& value, Iterator const& i, Difference const& diff) const
    {
        i.set(
            find_best_match(
                vigra::detail::RequiresExplicitCast<value_type>::cast(value)),
            diff );
    }
};

}

class TestApp : public Application
{
public:
	virtual void Main();
	virtual USHORT	Exception( USHORT nError );
};

class TestWindow : public Dialog
{
	public:
		TestWindow() : Dialog( (Window *) NULL )
		{
			SetText( rtl::OUString::createFromAscii( "VIGRA test" ) );
			SetSizePixel( Size( 1024, 1024 ) );
			EnablePaint( true );
			Show();
		}
		virtual ~TestWindow() {}
        virtual void MouseButtonUp( const MouseEvent& /*rMEvt*/ )
		{
			//TODO: do something cool
            EndDialog();
        }
		virtual void Paint( const Rectangle& rRect );
};

static basegfx::B2IPoint project( const basegfx::B2IPoint& rPoint )
{
	const double angle_x = M_PI / 6.0;
	const double angle_z = M_PI / 6.0;

	// transform planar coordinates to 3d
	double x = rPoint.getX();
	double y = rPoint.getY();
	//double z = 0;

	// rotate around X axis
	double x1 = x;
	double y1 = y * cos( angle_x );
	double z1 = y * sin( angle_x );

	// rotate around Z axis
	double x2 = x1 * cos( angle_z ) + y1 * sin( angle_z );
	//double y2 = y1 * cos( angle_z ) - x1 * sin( angle_z );
	double z2 = z1;

	//return basegfx::B2IPoint( (sal_Int32)3*x2, (sal_Int32)3*z2 );
	return basegfx::B2IPoint( (sal_Int32)(6*x2), (sal_Int32)(6*z2) );
}

static basebmp::Color approachColor( const basebmp::Color& rFrom, const basebmp::Color& rTo )
{
	basebmp::Color aColor;
	UINT8 nDiff;
	// approach red
	if( rFrom.getRed() < rTo.getRed() )
	{
		nDiff = rTo.getRed() - rFrom.getRed();
		aColor.setRed( rFrom.getRed() + ( nDiff < 10 ? nDiff : 10 ) );
	}
	else if( rFrom.getRed() > rTo.getRed() )
	{
		nDiff = rFrom.getRed() - rTo.getRed();
		aColor.setRed( rFrom.getRed() - ( nDiff < 10 ? nDiff : 10 ) );
	}
	else
		aColor.setRed( rFrom.getRed() );

	// approach Green
	if( rFrom.getGreen() < rTo.getGreen() )
	{
		nDiff = rTo.getGreen() - rFrom.getGreen();
		aColor.setGreen( rFrom.getGreen() + ( nDiff < 10 ? nDiff : 10 ) );
	}
	else if( rFrom.getGreen() > rTo.getGreen() )
	{
		nDiff = rFrom.getGreen() - rTo.getGreen();
		aColor.setGreen( rFrom.getGreen() - ( nDiff < 10 ? nDiff : 10 ) );
	}
	else
		aColor.setGreen( rFrom.getGreen() );

	// approach blue
	if( rFrom.getBlue() < rTo.getBlue() )
	{
		nDiff = rTo.getBlue() - rFrom.getBlue();
		aColor.setBlue( rFrom.getBlue() + ( nDiff < 10 ? nDiff : 10 ) );
	}
	else if( rFrom.getBlue() > rTo.getBlue() )
	{
		nDiff = rFrom.getBlue() - rTo.getBlue();
		aColor.setBlue( rFrom.getBlue() - ( nDiff < 10 ? nDiff : 10 ) );
	}
	else
		aColor.setBlue( rFrom.getBlue() );

	return aColor;
}

#define DELTA 5.0

void TestWindow::Paint( const Rectangle& /*rRect*/ )
{
    basegfx::B2ISize aTestSize(1000,1000);
    basebmp::BitmapDeviceSharedPtr pDevice( basebmp::createBitmapDevice( aTestSize,
                                                                         false,
                                                                         basebmp::Format::THIRTYTWO_BIT_TC_MASK ));

    {
        ::rtl::OUString aSvg;
        basegfx::B2DPolyPolygon aPoly;

        basegfx::tools::importFromSvgD( aPoly,
                                        ::rtl::OUString::createFromAscii(
                                            "m0 0 h7 v7 h-7 z" ) );
        basegfx::tools::importFromSvgD( aPoly,
                                        ::rtl::OUString::createFromAscii(
                                            "m2 2 h3 v3 h-3 z" ) );

        pDevice->fillPolyPolygon(
            aPoly,
            basebmp::Color(0xFFFFFFFF),
            basebmp::DrawMode_PAINT );
    }

#if 0
    {
        basebmp::BitmapDeviceSharedPtr pMask( basebmp::createBitmapDevice( aTestSize,
                                                                           false,
                                                                           basebmp::Format::ONE_BIT_MSB_GREY ));

        const basegfx::B2IPoint aPt111(10,10);
        const basegfx::B2IPoint aPt222(0,10);
        const basebmp::Color aCol333(0xFFFFFFFF);
        pMask->drawLine( aPt111, aPt222, aCol333, basebmp::DrawMode_PAINT );

        ::rtl::OUString aSvg = ::rtl::OUString::createFromAscii(
            "m 0 0 h5 l5 5 v5 h-5 l-5-5 z" );
        basegfx::B2DPolyPolygon aPoly;
        basegfx::tools::importFromSvgD( aPoly, aSvg );
        pMask->clear(basebmp::Color(0xFFFFFFFF));
        pMask->drawPolygon(
            aPoly.getB2DPolygon(0),
            basebmp::Color(0),
            basebmp::DrawMode_PAINT );

        basebmp::BitmapDeviceSharedPtr pSubsetDevice =
            basebmp::subsetBitmapDevice( pDevice,
                                         basegfx::B2IRange(3,3,7,7) );

        const basegfx::B2IPoint aPt1(0,0);
        const basegfx::B2IPoint aPt2(1,9);
        const basebmp::Color aCol(0xFFFFFFFF);
        pDevice->drawLine( aPt1, aPt2, aCol, basebmp::DrawMode_PAINT, pMask );
    }

    {
        const basebmp::Color aCol(0xFFFFFFFF);
        basegfx::B2DPolygon aRect = basegfx::tools::createPolygonFromRect(
            basegfx::B2DRange( 0,0,1001,1001 ));
        pDevice->drawPolygon( aRect, aCol, basebmp::DrawMode_PAINT );

        const basegfx::B2IPoint aPt1(0,0);
        const basegfx::B2IPoint aPt2(0,800);
        pDevice->drawLine( aPt1, aPt2, aCol, basebmp::DrawMode_PAINT );

        const basegfx::B2IPoint aPt3(0,1001);
        pDevice->drawLine( aPt1, aPt3, aCol, basebmp::DrawMode_PAINT );
    }
#endif

    {
        pDevice->clear(basebmp::Color(0));

        basegfx::B2IPoint aCenter( aTestSize.getX()/2,
                                   aTestSize.getY()/2 );
        //basegfx::B2IPoint aP1( aTestSize.getX()/48, 0), aP2( aTestSize.getX()/40, 0 ), aPoint;
        //basegfx::B2IPoint aP1( aTestSize.getX()/7, 0), aP2( aTestSize.getX()/6, 0 ), aPoint;
        //basegfx::B2IPoint aP1( aTestSize.getX()/5, 0), aP2( aTestSize.getX()/4, 0 ), aPoint;
        basegfx::B2IPoint aP1( aTestSize.getX()/12, 0), aP2( aTestSize.getX()/11, 0 ), aPoint;

        double sind = sin( DELTA*M_PI/180.0 );
        double cosd = cos( DELTA*M_PI/180.0 );
        double factor = 1 + (DELTA/1000.0);
        int n=0;
        basebmp::Color aLineColor( 0, 0, 0 );
        basebmp::Color aApproachColor( 0, 0, 200 );
        while ( aP2.getX() < aCenter.getX() && n++ < 680 )
        {
            aLineColor = approachColor( aLineColor, aApproachColor );

            // switch aproach color
            if( aApproachColor == aLineColor )
            {
                if( aApproachColor.getRed() )
                    aApproachColor = basebmp::Color( 0, 0, 200 );
                else if( aApproachColor.getGreen() )
                    aApproachColor = basebmp::Color( 200, 0, 0 );
                else
                    aApproachColor = basebmp::Color( 0, 200, 0 );
            }

            basegfx::B2DPolygon aPoly;
            aPoly.append( basegfx::B2DPoint(project( aP1 ) + aCenter) );
            aPoly.append( basegfx::B2DPoint(project( aP2 ) + aCenter) );
            pDevice->fillPolyPolygon(
                basegfx::tools::createAreaGeometryForPolygon(
                    aPoly,
//                    std::max(1,n/30),
//                    std::max(1,n/60),
                    std::max(1,n/30),
                    basegfx::tools::B2DLINEJOIN_NONE),
                aLineColor,
                basebmp::DrawMode_PAINT);

            aPoint.setX( (int)((((double)aP1.getX())*cosd - ((double)aP1.getY())*sind)*factor) );
            aPoint.setY( (int)((((double)aP1.getY())*cosd + ((double)aP1.getX())*sind)*factor) );
            aP1 = aPoint;
            aPoint.setX( (int)((((double)aP2.getX())*cosd - ((double)aP2.getY())*sind)*factor) );
            aPoint.setY( (int)((((double)aP2.getY())*cosd + ((double)aP2.getX())*sind)*factor) );
            aP2 = aPoint;
        }
    }

    Bitmap aBitmap( Size(aTestSize.getX(),
                         aTestSize.getY()), 24 );

    // Fill bitmap with generated content
    {
        ScopedBitmapWriteAccess pWriteAccess( aBitmap.AcquireWriteAccess(),
                                              aBitmap );
        for( int y=0; y<aTestSize.getY(); ++y )
            for( int x=0; x<aTestSize.getX(); ++x )
                pWriteAccess->SetPixel(y,x,
                                       Color(pDevice->getPixelData(basegfx::B2IPoint(x,y))) );
    }

    DrawBitmap( Point(), aBitmap );
}

USHORT TestApp::Exception( USHORT nError )
{
	switch( nError & EXC_MAJORTYPE )
	{
		case EXC_RSCNOTLOADED:
			Abort( String::CreateFromAscii( "Error: could not load language resources.\nPlease check your installation.\n" ) );
			break;
	}
	return 0;
}

void TestApp::Main()
{
	//-------------------------------------------------
	// create the global service-manager
	//-------------------------------------------------
    uno::Reference< lang::XMultiServiceFactory > xFactory;
    try
    {
        uno::Reference< uno::XComponentContext > xCtx = ::cppu::defaultBootstrap_InitialComponentContext();
        xFactory = uno::Reference< lang::XMultiServiceFactory >(  xCtx->getServiceManager(),
                                                                  uno::UNO_QUERY );
        if( xFactory.is() )
            ::comphelper::setProcessServiceFactory( xFactory );
    }
    catch( uno::Exception& )
    {
    }

    if( !xFactory.is() )
    {
        OSL_TRACE( "Could not bootstrap UNO, installation must be in disorder. Exiting.\n" );
        exit( 1 );
    }

    // Create UCB.
    uno::Sequence< uno::Any > aArgs( 2 );
	aArgs[ 0 ] <<= rtl::OUString::createFromAscii( UCB_CONFIGURATION_KEY1_LOCAL );
	aArgs[ 1 ] <<= rtl::OUString::createFromAscii( UCB_CONFIGURATION_KEY2_OFFICE );
    ::ucb::ContentBroker::initialize( xFactory, aArgs );

	TestWindow pWindow;
	pWindow.Execute();

    // clean up UCB
	::ucb::ContentBroker::deinitialize();
}

TestApp aDemoApp;