#ifndef RLSS_INTERNAL_UTIL_HPP
#define RLSS_INTERNAL_UTIL_HPP

#include <Eigen/Geometry>
#include <boost/functional/hash/hash_fwd.hpp>
#include <memory>
#include <Eigen/Dense>
#include <queue>
#include <functional>
#include <stdexcept>
#include <absl/strings/str_cat.h>
#include <iostream>
#include <fstream>
#include <qp_wrappers/cplex.hpp>
#include <qp_wrappers/gurobi.hpp>
#include <qp_wrappers/qpoases.hpp>
#include <qp_wrappers/osqp.hpp>
#include <splx/curve/PiecewiseCurve.hpp>
#include <lp_wrappers/cplex.hpp>
#include <lp_wrappers/gurobi.hpp>

#define RLSS_HARD_QP_SOLVER CPLEX
#define RLSS_SOFT_QP_SOLVER CPLEX
#define RLSS_SVM_QP_SOLVER qpOASES
#define RLSS_HP_PRUNING_LP_SOLVER GUROBI

namespace rlss {

#ifdef ENABLE_RLSS_DEBUG_MESSAGES
namespace internal {
    // methods for debugging
    template<typename T>
    void debug_message_internal(bool first, const T& message) {
        if(first) {
            std::cout << "[DEBUG] ";
        }
        std::cout << message << std::endl;
    }

    template<typename T, typename... Args>
    void debug_message_internal(bool first, const T& message, const Args&... args) {
        if(first) {
            std::cout << "[DEBUG] ";
        }
        std::cout << message;
        debug_message_internal(false, args...);
    }
} // namespace internal

    template<typename... Args>
    void debug_message(const Args&... args) {
        internal::debug_message_internal(true, args...);
    }
#else
    template<typename... Args>
    void debug_message(const Args&... args) {

    }
#endif

namespace internal {
    namespace debug {
        namespace colors {
            constexpr char RESET[] = "\033[0m";
            constexpr char RED[] = "\033[31m";
            constexpr char GREEN[] = "\033[32m";
        }
    }
}

namespace internal {

template<typename T, unsigned int DIM>
using AlignedBox = Eigen::AlignedBox<T, DIM>;

template<typename T, unsigned int DIM>
using Hyperplane = Eigen::Hyperplane<T, DIM>;

template<typename T, unsigned int DIM>
using VectorDIM = Eigen::Matrix<T, DIM, 1>;

template<typename T, unsigned int DIM>
using StdVectorVectorDIM = std::vector<VectorDIM<T,DIM>,
                            Eigen::aligned_allocator<VectorDIM<T,DIM>>>;

template<typename T>
using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;

template<typename T>
using Row = Eigen::Matrix<T, 1, Eigen::Dynamic>;

template<typename T>
using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

template<typename T, unsigned int R, unsigned int C>
using MatrixRC = Eigen::Matrix<T, R, C>; // row column

// method to get corner points of the Eigen AlignedBox passed as argument
template<typename T, unsigned int DIM>
StdVectorVectorDIM<T, DIM> cornerPoints(const AlignedBox<T, DIM>& box) {
    StdVectorVectorDIM<T, DIM> pts(1<<DIM);
    for(unsigned int i = 0; i < (1<<DIM); i++) {
        for(unsigned int d = 0; d < DIM; d++) {
            pts[i](d) = (i & (1<<d)) ? box.min()(d) : box.max()(d);
        }
    }
    return pts;
}

// method to return hash value based on vector coordinates passed
template<typename T, unsigned int DIM>
class VectorDIMHasher {
public:
    std::size_t operator()(const VectorDIM<T, DIM>& vec) const {
        std::size_t seed = 0;
        for(unsigned int d = 0; d < DIM; d++) {
            boost::hash_combine(seed, vec(d));
        }
        return seed;
    }
};

// linear interpolation of two vector points. Returns a vector container of
// points based on number of interpolated points passed
template <typename T, unsigned int DIM>
StdVectorVectorDIM<T, DIM> linearInterpolate(
    const VectorDIM<T, DIM>& start,
    const VectorDIM<T, DIM>& end,
    std::size_t num_points
) {

    using StdVectorVectorDIM = StdVectorVectorDIM<T, DIM>;
    using VectorDIM = VectorDIM<T, DIM>;

    if(num_points < 2) {
        throw std::domain_error(
            absl::StrCat
            (
                "linear interpolate can't have less than 2 number of points",
                " given: ",
                num_points
            )
        );
    }

    StdVectorVectorDIM result(num_points);

    VectorDIM step_vec = (end - start) / (num_points - 1);

    result[0] = start;
    for(std::size_t step = 1; step < num_points - 1; step++) {
        result[step] = start + step * step_vec;
    }
    result.back() = end;

    return result;

}

// method to check if the discrete path search algorithm splits the path segments
// to the minimum required number of segments
template <typename T, unsigned int DIM>
StdVectorVectorDIM<T, DIM> bestSplitSegments(
    const StdVectorVectorDIM<T, DIM>& segments,
    std::size_t num_pieces
) {
    // if current number of segments is larger than the number of pieces, throws error
    if(num_pieces + 1 < segments.size()) {
        throw std::domain_error(
            absl::StrCat
            (
                "nothing to split since num_pieces=",
                num_pieces,
                "is less than current number of segments ",
                segments.size() - 1
            )
        );
    }

    using VectorDIM = VectorDIM<T, DIM>;
    using StdVectorVectorDIM = StdVectorVectorDIM<T, DIM>;

    // if segements is currently of size 1, return a vector of size num_pieces + 1 with
    // all of values of that segment
    if(segments.size() == 1) {
        return StdVectorVectorDIM(num_pieces + 1, segments[0]);
    }

    // define segment priority queue element
    struct SegmentPQElem {
        VectorDIM start;
        VectorDIM end;
        T length;
        unsigned int num_pieces;
        std::size_t segment_idx;

        SegmentPQElem(const VectorDIM& s, const VectorDIM& e, std::size_t i)
                :   start(s),
                    end(e),
                    length((s-e).norm()),
                    num_pieces(1),
                    segment_idx(i)
        {

        }

        // operator to compare the average length of the segment
        bool operator<(const SegmentPQElem& rhs) const {
            return this->length / this->num_pieces
                        < rhs.length / rhs.num_pieces;
        }
    };

    std::priority_queue
    <
        SegmentPQElem,
        std::vector<SegmentPQElem>
    > pq;

    // initialize and store segmentPQelements in PQ
    // store current segment point and next segment point and the index of that segment
    for(std::size_t i = 0; i < segments.size()-1; i++) {
        pq.push({segments[i], segments[i+1], i});
    }

    // enters iteration only if there are excess segments
    // remove excess segments, such that the segments vector container size equals to the
    // number of segments to be generated
    for(std::size_t p_count = segments.size() - 1;
            p_count < num_pieces; p_count++) {
        SegmentPQElem t = pq.top();
        pq.pop();
        t.num_pieces++;
        pq.push(t);
    }

    // iteratively process the priority queue to store into the segment pq elements vector
    std::vector<SegmentPQElem> segment_pqelems;
    while(!pq.empty()) {
        segment_pqelems.push_back(pq.top());
        pq.pop();
    }

    // sort the segments based on their segment length from shortest to longest
    std::sort(
            segment_pqelems.begin(),
            segment_pqelems.end(),
            [] (const SegmentPQElem& lhs, const SegmentPQElem& rhs) -> bool {
                return lhs.segment_idx < rhs.segment_idx;
            }
    );

    StdVectorVectorDIM result;

    // interpolate every segment and split them into the number of pieces that segment
    // is to be interpolated, and store the segment points into the vector
    for(const SegmentPQElem& pqelem: segment_pqelems) {
        StdVectorVectorDIM interpolate
            = linearInterpolate<T, DIM>(
                    pqelem.start,
                    pqelem.end,
                    pqelem.num_pieces + 1
        );

        // append to the back of results vector
        result.insert(result.end(), interpolate.begin(), interpolate.end() - 1);
    }

    result.push_back(segments.back());
    return  result;
}

template<typename T, unsigned int DIM>
StdVectorVectorDIM<T, DIM> firstSegmentFix(
    const StdVectorVectorDIM<T, DIM>& segments
) {
    assert(segments.size() > 1);

    StdVectorVectorDIM<T, DIM> result;
    result.push_back(segments[0]);
    result.push_back((segments[0] + segments[1]) / 2);
    for(std::size_t i = 1; i < segments.size(); i++) {
        result.push_back(segments[i]);
    }
    return result;
}
// shift hyperplane hp creating hyperplane shp
// such that whenever the center_of_mass of the robot
// is to the negative side of the shp,
// the collision shape box of the
// robot is to the negative side of the hyperplane hp.
// box should be the bounding box of the collision shape at given
// center_of_mass
template<typename T, unsigned int DIM>
Hyperplane<T, DIM> shiftHyperplane(
    const VectorDIM<T, DIM>& center_of_mass,
    const AlignedBox<T, DIM>& box,
    const Hyperplane<T, DIM>& hp
) {
    using Hyperplane = Hyperplane<T, DIM>;
    using StdVectorVectorDIM = StdVectorVectorDIM<T, DIM>;
    Hyperplane shp {hp.normal(), std::numeric_limits<T>::lowest()};

    StdVectorVectorDIM corner_points = cornerPoints<T, DIM>(box);

    for(const auto& pt : corner_points) {
        shp.offset()
                = std::max(shp.offset(),
                           hp.normal().dot(pt - center_of_mass) + hp.offset()
        );
    }

    return shp;
}

/*
* Buffer 'box' so that when center_of_mass is inside the buffered box
* com_box corresponding to robot with center of mass 'center_of_mass'
* is inside the 'box'
*/
template<typename T, unsigned int DIM>
AlignedBox<T, DIM> bufferAlignedBox(
    const VectorDIM<T, DIM>& center_of_mass,
    const AlignedBox<T, DIM>& com_box,
    const AlignedBox<T, DIM>& box
) {
    return AlignedBox<T, DIM>(box.min() + (center_of_mass - com_box.min()),
                      box.max() + (center_of_mass - com_box.max()));
}

/*
 * Prunes hyperplanes h in hps such that if the space S bounded by the bounding
 * box bbox and the hyperplane h contains the space S' bounded by the bounding
 * box bbox and an hyperplane h' in hps (that is S' is a subset of S).
 */
template<typename T, unsigned int DIM>
std::vector<Hyperplane<T, DIM>> pruneHyperplanes(
        const std::vector<Hyperplane<T,DIM>>& hps,
        const AlignedBox<T, DIM>& bbox) {

    using Hyperplane = Hyperplane<T, DIM>;
    using AlignedBox = AlignedBox<T, DIM>;
    using LP = LPWrappers::Problem<T>;
    using LPEngine = LPWrappers::RLSS_HP_PRUNING_LP_SOLVER::Engine<T>;
    using Vector = typename LPEngine::Vector;

    std::vector<Hyperplane> result = hps;
    LP lp(DIM, 2);
    LPEngine solver;

    for(unsigned int d = 0; d < DIM; d++) {
        lp.set_var_limits(d, bbox.min()(d), bbox.max()(d));
    }

    Vector c(DIM);
    for(unsigned int d = 0; d < DIM; d++) c(d) = 1;
    lp.add_c(c);

    for(std::size_t i = 0; i < result.size(); i++) {
        bool should_remove_i = false;

        lp.set_constraint(1, result[i].normal().transpose(),
                result[i].offset(), LP::infinity);

        for(std::size_t j = 0; j < result.size(); j++) {
            if(i == j) continue;

            lp.set_constraint(0, result[j].normal().transpose(),
                    LP::minus_infinity, result[j].offset());

            Vector soln;
            auto ret = solver.init(lp, soln);
            if(ret != LPWrappers::OptReturnType::Optimal) {
                should_remove_i = true;
                break;
            }
        }

        if(should_remove_i) {
            std::swap(result[i], result.back());
            result.pop_back();
            i--;
        }
    }

    return result;
}

} // namespace internal

// ellipsoid class
template<typename T, unsigned int DIM>
class Ellipsoid {
public:
    using VectorDIM = rlss::internal::VectorDIM<T, DIM>;
    using MatrixDIMDIM = rlss::internal::MatrixRC<T, DIM, DIM>;
    using AlignedBox = rlss::internal::AlignedBox<T, DIM>;
    using Index = Eigen::Index;

    Ellipsoid(const VectorDIM& cnt, const MatrixDIMDIM& m)
        : center(cnt), mtr(m) {
        mtrInvSq = mtr.inverse();
        mtrInvSq = mtrInvSq * mtrInvSq;

    }

    AlignedBox boundingBox() const {
        // https://members.loria.fr/samuel.hornus/ellipsoid-bbox.html
        VectorDIM delta;
        for(Index r = 0; r < DIM; r++) {
            delta(r) = mtr.row(r).norm();
        }

        debug_message(center - delta, center + delta);

        return AlignedBox(center - delta, center + delta);
    }

    bool intersects(const AlignedBox& box) const {
        if(box.contains(this->center)) {
            return true;
        }

        auto crnr = internal::cornerPoints<T, DIM>(box);
        for(const auto& pt: crnr) {
            VectorDIM d = pt - center;
            if(d.transpose() * mtrInvSq * d <= 1) {
                return true;
            }
        }
        return false;
    }

private:
    VectorDIM center;
    // affine transformation matrix that transforms unit sphere
    // at origin to the ellipsoid at origin
    MatrixDIMDIM mtr;
    MatrixDIMDIM mtrInvSq;
};

} // namespace rlss

#endif // RLSS_INTERNAL_UTIL_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2015 Muhammad Junaid Muzammil <mjunaidmuzammil@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_RANDOM_THREEFRY_HPP
#define BOOST_COMPUTE_RANDOM_THREEFRY_HPP

#include <algorithm>

#include <boost/compute/types.hpp>
#include <boost/compute/buffer.hpp>
#include <boost/compute/kernel.hpp>
#include <boost/compute/context.hpp>
#include <boost/compute/program.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>
#include <boost/compute/utility/program_cache.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/discard_iterator.hpp>

namespace boost {
namespace compute {

/// \class threefry_engine
/// \brief Threefry pseudorandom number generator.
template<class T = uint_>
class threefry_engine
{
public:
    static const size_t threads = 1024;
    typedef T result_type;

    /// Creates a new threefry_engine and seeds it with \p value.
    explicit threefry_engine(command_queue &queue)
        : m_context(queue.get_context())
    {
        // setup program
        load_program();
    }

    /// Creates a new threefry_engine object as a copy of \p other.
    threefry_engine(const threefry_engine<T> &other)
        : m_context(other.m_context),
          m_program(other.m_program)
    {
    }

    /// Copies \p other to \c *this.
    threefry_engine<T>& operator=(const threefry_engine<T> &other)
    {
        if(this != &other){
            m_context = other.m_context;
            m_program = other.m_program;
        }

        return *this;
    }

    /// Destroys the threefry_engine object.
    ~threefry_engine()
    {
    }

private:
    /// \internal_
    void load_program()
    {
        boost::shared_ptr<program_cache> cache =
            program_cache::get_global_cache(m_context);
        std::string cache_key =
            std::string("threefry_engine_32x2");

        // Copyright 2010-2012, D. E. Shaw Research.
        // 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 D. E. Shaw Research 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
        // OWNER 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.
        const char source[] =
            "#define THREEFRY2x32_DEFAULT_ROUNDS 20\n"
            "#define SKEIN_KS_PARITY_32 0x1BD11BDA\n"

            "enum r123_enum_threefry32x2 {\n"
            "    R_32x2_0_0=13,\n"
            "    R_32x2_1_0=15,\n"
            "    R_32x2_2_0=26,\n"
            "    R_32x2_3_0= 6,\n"
            "    R_32x2_4_0=17,\n"
            "    R_32x2_5_0=29,\n"
            "    R_32x2_6_0=16,\n"
            "    R_32x2_7_0=24\n"
            "};\n"

            "static uint RotL_32(uint x, uint N)\n"
            "{\n"
            "    return (x << (N & 31)) | (x >> ((32-N) & 31));\n"
            "}\n"

            "struct r123array2x32 {\n"
            "    uint v[2];\n"
            "};\n"
            "typedef struct r123array2x32 threefry2x32_ctr_t;\n"
            "typedef struct r123array2x32 threefry2x32_key_t;\n"

            "threefry2x32_ctr_t threefry2x32_R(unsigned int Nrounds, threefry2x32_ctr_t in, threefry2x32_key_t k)\n"
            "{\n"
            "    threefry2x32_ctr_t X;\n"
            "    uint ks[3];\n"
            "    uint  i; \n"
            "    ks[2] =  SKEIN_KS_PARITY_32;\n"
            "    for (i=0;i < 2; i++) {\n"
            "        ks[i] = k.v[i];\n"
            "        X.v[i]  = in.v[i];\n"
            "        ks[2] ^= k.v[i];\n"
            "    }\n"
            "    X.v[0] += ks[0]; X.v[1] += ks[1];\n"
            "    if(Nrounds>0){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_0_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>1){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_1_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>2){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_2_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>3){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_3_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>3){\n"
            "        X.v[0] += ks[1]; X.v[1] += ks[2];\n"
            "        X.v[1] += 1;\n"
            "    }\n"
            "    if(Nrounds>4){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_4_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>5){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_5_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>6){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_6_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>7){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_7_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>7){\n"
            "        X.v[0] += ks[2]; X.v[1] += ks[0];\n"
            "        X.v[1] += 2;\n"
            "    }\n"
            "    if(Nrounds>8){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_0_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>9){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_1_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>10){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_2_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>11){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_3_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>11){\n"
            "        X.v[0] += ks[0]; X.v[1] += ks[1];\n"
            "        X.v[1] += 3;\n"
            "    }\n"
            "    if(Nrounds>12){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_4_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>13){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_5_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>14){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_6_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>15){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_7_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>15){\n"
            "        X.v[0] += ks[1]; X.v[1] += ks[2];\n"
            "        X.v[1] += 4;\n"
            "    }\n"
            "    if(Nrounds>16){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_0_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>17){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_1_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>18){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_2_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>19){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_3_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>19){\n"
            "        X.v[0] += ks[2]; X.v[1] += ks[0];\n"
            "        X.v[1] += 5;\n"
            "    }\n"
            "    if(Nrounds>20){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_4_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>21){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_5_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>22){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_6_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>23){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_7_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>23){\n"
            "        X.v[0] += ks[0]; X.v[1] += ks[1];\n"
            "        X.v[1] += 6;\n"
            "    }\n"
            "    if(Nrounds>24){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_0_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>25){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_1_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>26){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_2_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>27){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_3_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>27){\n"
            "        X.v[0] += ks[1]; X.v[1] += ks[2];\n"
            "        X.v[1] += 7;\n"
            "    }\n"
            "    if(Nrounds>28){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_4_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>29){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_5_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>30){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_6_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>31){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_7_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>31){\n"
            "        X.v[0] += ks[2]; X.v[1] += ks[0];\n"
            "        X.v[1] += 8;\n"
            "    }\n"
            "    return X;\n"
            "}\n"

            "__kernel void generate_rng(__global uint *ctr, __global uint *key, const uint offset) {\n"
            "    threefry2x32_ctr_t in;\n"
            "    threefry2x32_key_t k;\n"
            "    const uint i = get_global_id(0);\n"
            "    in.v[0] = ctr[2 * (offset + i)];\n"
            "    in.v[1] = ctr[2 * (offset + i) + 1];\n"
            "    k.v[0] = key[2 * (offset + i)];\n"
            "    k.v[1] = key[2 * (offset + i) + 1];\n"
            "    in = threefry2x32_R(20, in, k);\n"
            "    ctr[2 * (offset + i)] = in.v[0];\n"
            "    ctr[2 * (offset + i) + 1] = in.v[1];\n"
            "}\n";

        m_program = cache->get_or_build(cache_key, std::string(), source, m_context);
    }

public:

    /// Generates Threefry random numbers using both the counter and key values, and then stores
    /// them to the range [\p first_ctr, \p last_ctr).
    template<class OutputIterator>
    void generate(OutputIterator first_ctr, OutputIterator last_ctr, OutputIterator first_key, OutputIterator last_key, command_queue &queue) {
        const size_t size_ctr = detail::iterator_range_size(first_ctr, last_ctr);
        const size_t size_key = detail::iterator_range_size(first_key, last_key);
        if(!size_ctr || !size_key || (size_ctr != size_key)) {
            return;
        }
        kernel rng_kernel = m_program.create_kernel("generate_rng");

        rng_kernel.set_arg(0, first_ctr.get_buffer());
        rng_kernel.set_arg(1, first_key.get_buffer());
        size_t offset = 0;

        for(;;){
            size_t count = 0;
            size_t size = size_ctr/2;
            if(size > threads){
                count = (std::min)(static_cast<size_t>(threads), size - offset);
            }
            else {
                count = size;
            }
            rng_kernel.set_arg(2, static_cast<const uint_>(offset));
            queue.enqueue_1d_range_kernel(rng_kernel, 0, count, 0);

            offset += count;

            if(offset >= size){
                break;
            }

        }
    }

    template<class OutputIterator>
    void generate(OutputIterator first_ctr, OutputIterator last_ctr, command_queue &queue) {
        const size_t size_ctr = detail::iterator_range_size(first_ctr, last_ctr);
        if(!size_ctr) {
            return;
        }
        boost::compute::vector<uint_> vector_key(size_ctr, m_context);
        vector_key.assign(size_ctr, 0, queue);
        kernel rng_kernel = m_program.create_kernel("generate_rng");

        rng_kernel.set_arg(0, first_ctr.get_buffer());
        rng_kernel.set_arg(1, vector_key);
        size_t offset = 0;

        for(;;){
            size_t count = 0;
            size_t size = size_ctr/2;
            if(size > threads){
                count = (std::min)(static_cast<size_t>(threads), size - offset);
            }
            else {
                count = size;
            }
            rng_kernel.set_arg(2, static_cast<const uint_>(offset));
            queue.enqueue_1d_range_kernel(rng_kernel, 0, count, 0);

            offset += count;

            if(offset >= size){
                break;
            }

        }
    }
private:
    context m_context;
    program m_program;
};

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_RANDOM_THREEFRY_HPP


/*!
 * @file swp_surfvolhydrogen_primary.cpp
 * @author Robert I A Patterson Robert.Patterson@wias-berlin.de
 *

  Copyright (C) 2012 Robert I A Patterson

  File purpose:
    Implementation of the SurfVolHydrogenPrimary class declared in the
    swp_surfvol_primary.h header file.
    @brief Implementation of particle described by its volume and surface area.

  Licence:
    This file is part of "sweepc".

    sweepc is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

  Contact:
    Dr Markus Kraft
    Dept of Chemical Engineering
    University of Cambridge
    New Museums Site
    Pembroke Street
    Cambridge
    CB2 3RA
    UK

    Email:       mk306@cam.ac.uk
    Website:     http://como.cheng.cam.ac.uk
*/

#include "swp_primary.h"
#include "swp_surfvolhydrogen_primary.h"
#include "swp_aggmodel_type.h"
#include "swp_model_factory.h"

#include <stdexcept>
#include <boost/random/poisson_distribution.hpp>

using namespace Sweep;
using namespace Sweep::AggModels;

// CONSTRUCTORS AND DESTRUCTORS.

// Default constructor (protected).
SurfVolHydrogenPrimary::SurfVolHydrogenPrimary(void)
: iH(1u)
{
}

// Initialising constructor.
SurfVolHydrogenPrimary::SurfVolHydrogenPrimary(double time, const Sweep::ParticleModel &model)
: Primary(time, model)
, iH(1u)
{
}

// Copy constructor.
SurfVolHydrogenPrimary::SurfVolHydrogenPrimary(const SurfVolHydrogenPrimary &copy)
: iH(1u)
{
    *this = copy;
}

// Stream-reading constructor.
SurfVolHydrogenPrimary::SurfVolHydrogenPrimary(std::istream &in, const Sweep::ParticleModel &model)
: iH(1u)
{
    Deserialize(in, model);
}

// Default destructor.
SurfVolHydrogenPrimary::~SurfVolHydrogenPrimary()
{
    releaseMem();
}

// OPERATOR OVERLOADS.

// Assignment operator (Primary RHS).
SurfVolHydrogenPrimary &SurfVolHydrogenPrimary::operator=(const Primary &rhs)
{
    // Attempt to cast the primary to a SurfVolHydrogenPrimary.  This will
    // throw an exception if the cast fails.
    operator=(dynamic_cast<const SurfVolHydrogenPrimary&>(rhs));

    return *this;
}

// Assignment operator (SurfVolHydrogenPrimary RHS).
SurfVolHydrogenPrimary &SurfVolHydrogenPrimary::operator=(const SurfVolHydrogenPrimary &rhs)
{
    // First copy everything for spherical primaries.
    Primary::operator=(rhs);

    return *this;
}

// AGGREGATION MODEL.

// Returns the aggregation model which this primary describes.
AggModels::AggModelType SurfVolHydrogenPrimary::AggID(void) const {return AggModels::SurfVolHydrogen_ID;}

// BASIC DERIVED PARTICLE PROPERTIES.

// Calculates the derived properties from the unique properties.  This
// function is broadly similar to the version in the spherical Primary
// class except that it uses the surface-volume model.  Therefore the
// surface area is not altered and the collision diameter is calculated
// using the arithmetic mean function.
void SurfVolHydrogenPrimary::UpdateCache(void)
{
    // Store the correct surface area.
    const double s = m_surf;

    // Pretend that the primary is spherical and set the cache
    // accordingly.  This will set m_surf the the surface are
    // of an equivalent volume sphere.
    Primary::UpdateCache();

    // The surface area is now set to that of a sphere. Set
    // correct surface area (minimum is spherical surface area).
    m_surf = std::max(s, m_surf);

    // Calculate diameters.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol; // TODO:  Correct expression for Dmob.
}

// Returns the equivalent spherical particle surface area.
double SurfVolHydrogenPrimary::SphSurfaceArea(void) const {return PI * std::pow(6.0 * m_vol / PI, TWO_THIRDS);}

// Returns the number of primary particles if the aggregate is assumed
// to consist of mono-sized primaries.
unsigned int SurfVolHydrogenPrimary::PP_Count(void) const
{
    // Note the minimum number of primary particles must be 1.
    return std::max(1u, (unsigned int)((m_surf * m_surf * m_surf) /
                                       (36.0 * PI * m_vol * m_vol)));
}

// Returns the primary particle diameter if the aggregate is assumed
// to consist of mono-sized primaries.
double SurfVolHydrogenPrimary::PP_Diameter(void) const
{
    // This should always be <= equiv. sphere diameter.
    return 6.0 * m_vol / m_surf;
}

// OPERATIONS.

// Adjusts the primary with the given composition and
// tracker values changes n times.  If the particle cannot be adjust
// n times, then this function returns the number of times
// it was adjusted.
unsigned int SurfVolHydrogenPrimary::Adjust(const fvector &dcomp, const fvector &dvalues, rng_type &rng,
                                    unsigned int n)
{
    // Calculate change in volume.
    double dvol = 0.0;
    for (unsigned int i=0; i!=dcomp.size(); ++i) {
        dvol += dcomp[i] * m_pmodel->Components(i)->MolWt() /
                m_pmodel->Components(i)->Density();
    }
    dvol *= (double)n / NA;

    // Calculate change in surface area.
    double invRadius = 0.0;
    if (dvol > 0.0) {
        // Inverse growth radius.
        invRadius = sqrt(4.0 * PI / m_surf);
    } else {
        // Inverse oxidation radius.
        invRadius = m_surf / (3.0 * m_vol);
    }

    // Save new and old surface areas
    const double oldS = m_surf;
    const double s = m_surf + (2.0 * dvol * invRadius);

    // Adjust the particle assuming that it is spherical.
    Primary::Adjust(dcomp, dvalues, rng, n);

    // Maintain the concentration of H per unit surface area during oxidation
    if(dvol < 0.0) {
        fvector comp = Composition();
        //comp[iH] = static_cast<int>(s * comp[iH] / oldS + 0.5);
        comp[iH] = s * comp[iH] / oldS;
        SetComposition(comp);
    }

    // Set correct surface area, which was incorrectly set by
    // Primary::Adjust.
    m_surf    = std::max(s, m_surf);

    // This has a knock-on affect of changing the collision diameter.
    // Note, we can avoid recalling UpdateCache() here, because only
    // a couple of cached values will have changed.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol;

    return n;
}

/*!
 * Combines this primary with another.
 *
 * \param[in]       rhs         Particle to add to current instance
 * \param[in,out]   rng         Random number generator
 *
 * \return      Reference to the current instance after rhs has been added
 */
SurfVolHydrogenPrimary &SurfVolHydrogenPrimary::Coagulate(const Primary &rhs, rng_type &rng)

{
    // Store the resultant surface area.
    const double s = m_surf + rhs.SurfaceArea();

    // Perform the coagulation.
    Primary::Coagulate(rhs, rng);

    // The spherical particle Coagulate() routine has set the
    // surface area incorrectly.  We now replace the surface area
    // to the correct point-contact value.
    m_surf    = std::max(m_surf, s);

    // This has a knock-on affect of changing the collision diameter.
    // Note, we can avoid recalling UpdateCache() here, because only
    // a couple of cached values will have changed.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol;

    return *this;
}

/*!
 * Combines this primary with another.
 *
 * \param[in]       rhs         Particle to add to current instance
 * \param[in,out]   rng         Random number generator
 *
 * \return      Reference to the current instance after rhs has been added
 */
SurfVolHydrogenPrimary &SurfVolHydrogenPrimary::Fragment(const Primary &rhs, rng_type &rng)

{
    // Store the resultant surface area.
    const double s = m_surf + rhs.SurfaceArea();

    // Perform the coagulation.
    Primary::Fragment(rhs, rng);

    // The spherical particle Coagulate() routine has set the
    // surface area incorrectly.  We now replace the surface area
    // to the correct point-contact value.
    m_surf    = std::max(m_surf, s);

    // This has a knock-on affect of changing the collision diameter.
    // Note, we can avoid recalling UpdateCache() here, because only
    // a couple of cached values will have changed.
    m_dcol = (m_diam + sqrt(m_surf / PI)) * 0.5;
    m_dmob = m_dcol;

    return *this;
}

// This routine sinters the Primary for the given length of
// time using the provided sintering model.
void SurfVolHydrogenPrimary::Sinter(double dt, Cell &sys,
                            const Processes::SinteringModel &model,
                            rng_type &rng,
                            double wt)
{
  throw std::runtime_error("sintering not implemented at present for SurfVolHydrogenPrimary");
}

// READ/WRITE/COPY.

// Returns a copy of the model data.
SurfVolHydrogenPrimary *const SurfVolHydrogenPrimary::Clone(void) const
{
    return new SurfVolHydrogenPrimary(*this);
}

/*!
 * \return      Number of surface Hydrogens (active sites)
 */
double SurfVolHydrogenPrimary::GetSites() const
{
    return Composition()[iH];
}

// Writes the object to a binary stream.
void SurfVolHydrogenPrimary::Serialize(std::ostream &out) const
{
    if (out.good()) {
        // Output the version ID (=0 at the moment).
        const unsigned int version = 0;
        out.write((char*)&version, sizeof(version));

        // Output base class.
        Primary::Serialize(out);
    } else {
        throw std::invalid_argument("Output stream not ready (Sweep, SurfVolHydrogenPrimary::Serialize).");
    }
}

// Reads the object from a binary stream.
void SurfVolHydrogenPrimary::Deserialize(std::istream &in, const Sweep::ParticleModel &model)
{
    if (in.good()) {
        // Read the output version.  Currently there is only one
        // output version, so we don't do anything with this variable.
        // Still needs to be read though.
        unsigned int version = 0;
        in.read(reinterpret_cast<char*>(&version), sizeof(version));

        switch (version) {
            case 0:
                // Read base class.
                Primary::Deserialize(in, model);
                break;
            default:
                throw std::runtime_error("Serialized version number is invalid (Sweep, SurfVolHydrogenPrimary::Deserialize).");
        }
    } else {
        throw std::invalid_argument("Input stream not ready (Sweep, SurfVolHydrogenPrimary::Deserialize).");
    }
}


#include <fstream>
#include <filesystem>

#include <Eigen/Geometry>
#include <utility>

#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/common/norms.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>

#include "analysis.h"
#include "filter.h"

#define N_BINS 4

namespace fs = std::filesystem;

std::pair<float, float> calculate_rotation_and_translation_errors(const Eigen::Matrix4f &transformation,
                                                                  const Eigen::Matrix4f &transformation_gt) {
    Eigen::Matrix3f rotation_diff = transformation.block<3, 3>(0, 0).inverse() * transformation_gt.block<3, 3>(0, 0);
    Eigen::Vector3f translation_diff = transformation.block<3, 1>(0, 3) - transformation_gt.block<3, 1>(0, 3);
    float rotation_error = Eigen::AngleAxisf(rotation_diff).angle();
    float translation_error = translation_diff.norm();
    return {rotation_error, translation_error};
}

inline float dist2(const PointN &p1, const PointN &p2) {
    return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
};

float calculate_point_cloud_rmse(const PointNCloud::ConstPtr &pcd,
                                 const Eigen::Matrix4f &transformation,
                                 const Eigen::Matrix4f &transformation_gt) {
    PointNCloud pcd_transformed;
    Eigen::Matrix4f transformation_diff = transformation.inverse() * transformation_gt;
    pcl::transformPointCloud(*pcd, pcd_transformed, transformation_diff);

    float rmse = 0.f;
    for (int i = 0; i < pcd->size(); ++i) {
        rmse += dist2(pcd->points[i], pcd_transformed.points[i]);
    }
    rmse = std::sqrt(rmse / (float) pcd->size());
    return rmse;
}

float calculate_overlap_rmse(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                             const Eigen::Matrix4f &transformation,
                             const Eigen::Matrix4f &transformation_gt,
                             float inlier_threshold) {
    PointNCloud src_aligned, src_aligned_gt;
    pcl::transformPointCloud(*src, src_aligned, transformation);
    pcl::transformPointCloud(*src, src_aligned_gt, transformation_gt);

    pcl::KdTreeFLANN<PointN> tree_tgt;
    tree_tgt.setInputCloud(tgt);

    int overlap_size = 0;
    float rmse = 0.f;
    pcl::Indices nn_indices;
    std::vector<float> nn_dists;
    for (int i = 0; i < src->size(); ++i) {
        tree_tgt.nearestKSearch(src_aligned_gt[i], 1, nn_indices, nn_dists);
        if (nn_dists[0] < inlier_threshold * inlier_threshold) {    // ith point in overlap
            rmse += dist2(src_aligned.points[i], src_aligned_gt.points[i]);
            overlap_size++;
        }
    }
    if (overlap_size != 0) {
        rmse = std::sqrt(rmse / (float)overlap_size);
    } else {
        rmse = std::numeric_limits<float>::quiet_NaN();
    }
    return rmse;

}

float calculate_correspondence_uniformity(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                          const pcl::Correspondences &correct_correspondences,
                                          const AlignmentParameters &parameters,
                                          const Eigen::Matrix4f &transformation_gt) {
    PointNCloud::Ptr src_aligned(new PointNCloud);
    pcl::transformPointCloud(*src, *src_aligned, transformation_gt);
    pcl::KdTreeFLANN<PointN>::Ptr tree(new pcl::KdTreeFLANN<PointN>());
    tree->setInputCloud(tgt);

    pcl::Indices indices;
    std::vector<float> distances;
    // calculate bbox of overlapping area
    PointN min_point, max_point;
    float error_thr = parameters.distance_thr_coef * parameters.voxel_size;
    for (int i = 0; i < src_aligned->size(); ++i) {
        (float) tree->nearestKSearch(*src_aligned, i, 1, indices, distances);
        const PointN &p_src(src_aligned->points[i]), p_tgt(tgt->points[indices[0]]);
        if (std::sqrt(distances[0]) < error_thr && std::isfinite(p_src.normal_x) && std::isfinite(p_tgt.normal_x)) {
            min_point.x = std::min(min_point.x, p_src.x);
            min_point.y = std::min(min_point.y, p_src.y);
            min_point.z = std::min(min_point.z, p_src.z);
            max_point.x = std::max(max_point.x, p_src.x);
            max_point.y = std::max(max_point.y, p_src.y);
            max_point.z = std::max(max_point.z, p_src.z);
        }
    }
    int count[3][N_BINS][N_BINS]{0};

    for (auto const &corr: correct_correspondences) {
        const auto &point = src_aligned->points[corr.index_query];
        if (pointInBoundingBox(point, min_point, max_point)) {
            int bin[3];
            bin[0] = std::floor((point.x - min_point.x) / (max_point.x - min_point.x) * N_BINS);
            bin[1] = std::floor((point.y - min_point.y) / (max_point.y - min_point.y) * N_BINS);
            bin[2] = std::floor((point.z - min_point.z) / (max_point.z - min_point.z) * N_BINS);
            // count 3D points projected to YZ, ZX, XY and fallen in 2D bin
            for (int k = 0; k < 3; ++k) {
                count[k][bin[(k + 1) % 3]][bin[(k + 2) % 3]]++;
            }
        }
    }
    float entropy[3]{0.f};
    float n = correct_correspondences.size();
    for (int k = 0; k < 3; ++k) {
        for (int i = 0; i < N_BINS; ++i) {
            for (int j = 0; j < N_BINS; ++j) {
                float p = (float) count[k][i][j] / n;
                if (p == 0.f) continue;
                entropy[k] -= p * std::log(p);
            }
        }
        entropy[k] /= std::log((float) (N_BINS * N_BINS));
    }
    return std::cbrt(entropy[0] * entropy[1] * entropy[2]);
}

float calculate_normal_difference(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                  const AlignmentParameters &parameters, const Eigen::Matrix4f &transformation_gt) {
    PointNCloud::Ptr src_aligned(new PointNCloud);
    pcl::transformPointCloud(*src, *src_aligned, transformation_gt);
    pcl::KdTreeFLANN<PointN>::Ptr tree(new pcl::KdTreeFLANN<PointN>());
    tree->setInputCloud(tgt);

    pcl::Indices indices;
    std::vector<float> distances;
    float error_thr = parameters.distance_thr_coef * parameters.voxel_size;
    float difference = 0.f;
    int n_points_overlap = 0;
    for (int i = 0; i < src_aligned->size(); ++i) {
        (float) tree->nearestKSearch(*src_aligned, i, 1, indices, distances);
        const PointN &p_src(src_aligned->points[i]), p_tgt(tgt->points[indices[0]]);
        if (std::sqrt(distances[0]) < error_thr && std::isfinite(p_src.normal_x) && std::isfinite(p_tgt.normal_x)) {
            float cos = std::clamp(p_src.normal_x * p_tgt.normal_x + p_src.normal_y * p_tgt.normal_y +
                                   p_src.normal_z * p_tgt.normal_z, -1.f, 1.f);
            difference += std::abs(std::acos(cos));
            n_points_overlap++;
        }
    }
    return difference / (float) n_points_overlap;
}

void buildCorrectCorrespondences(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                 const pcl::Correspondences &correspondences,
                                 pcl::Correspondences &correct_correspondences,
                                 const Eigen::Matrix4f &transformation_gt, float error_threshold) {
    correct_correspondences.clear();
    correct_correspondences.reserve(correspondences.size());

    PointNCloud input_transformed;
    input_transformed.resize(src->size());
    pcl::transformPointCloud(*src, input_transformed, transformation_gt);

    for (const auto &correspondence: correspondences) {
        PointN source_point(input_transformed.points[correspondence.index_query]);
        PointN target_point(tgt->points[correspondence.index_match]);
        float e = pcl::L2_Norm(source_point.data, target_point.data, 3);
        if (e < error_threshold) {
            correct_correspondences.push_back(correspondence);
        }
    }
}

AlignmentAnalysis::AlignmentAnalysis(const AlignmentParameters &parameters,
                                     const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                     const pcl::Correspondences &correspondences,
                                     int iterations, const Eigen::Matrix4f &transformation, double time) {
    parameters_ = parameters;
    src_ = src;
    tgt_ = tgt;
    correspondences_ = correspondences;
    iterations_ = iterations;
    transformation_ = transformation;
    time_ = time;
    has_converged_ = true;
    metric_estimator_ = getMetricEstimator(parameters);
    metric_estimator_->setSourceCloud(src);
    metric_estimator_->setTargetCloud(tgt);
    metric_estimator_->setCorrespondences(correspondences);
    metric_estimator_->setInlierThreshold(parameters.distance_thr_coef * parameters.voxel_size);
}

void AlignmentAnalysis::start(const Eigen::Matrix4f &transformation_gt, const std::string &testname) {
    testname_ = testname;
    float error_thr = parameters_.distance_thr_coef * parameters_.voxel_size;
    transformation_gt_ = transformation_gt;

    buildCorrectCorrespondences(src_, tgt_, correspondences_, correct_correspondences_, transformation_gt_, error_thr);
    metric_estimator_->buildInlierPairsAndEstimateMetric(transformation_, inlier_pairs_, rmse_, fitness_);
    metric_estimator_->buildCorrectInlierPairs(inlier_pairs_, correct_inlier_pairs_, transformation_gt_);
    pcd_error_ = calculate_point_cloud_rmse(src_, transformation_, transformation_gt_);
    overlap_error_ = calculate_overlap_rmse(src_, tgt_, transformation_, transformation_gt_, error_thr);
    normal_diff_ = calculate_normal_difference(src_, tgt_, parameters_, transformation_gt_);
    corr_uniformity_ = calculate_correspondence_uniformity(src_, tgt_, correct_correspondences_,
                                                           parameters_, transformation_gt_);
    std::tie(r_error_, t_error_) = calculate_rotation_and_translation_errors(transformation_, transformation_gt_);

    print();
    save(testname);
}

void AlignmentAnalysis::print() {
    // Print results
    printf("\n");
    printTransformation(transformation_);
    printTransformation(transformation_gt_);
    pcl::console::print_info("fitness: %0.7f\n", fitness_);
    pcl::console::print_info("inliers_rmse: %0.7f\n", rmse_);
    pcl::console::print_info("correct inliers: %i/%i\n", correct_inlier_pairs_.size(), inlier_pairs_.size());
    pcl::console::print_info("correct correspondences: %i/%i\n",
                             correct_correspondences_.size(), correspondences_.size());
    pcl::console::print_info("rotation error: %0.7f\n", r_error_);
    pcl::console::print_info("translation error: %0.7f\n", t_error_);
    pcl::console::print_info("point cloud mean error: %0.7f\n", pcd_error_);
    pcl::console::print_info("normal mean difference: %0.7f\n", normal_diff_);
    pcl::console::print_info("uniformity of correct correspondences' distribution: %0.7f\n", corr_uniformity_);
}

void AlignmentAnalysis::save(const std::string &testname) {
    // Save test parameters and results
    std::string filepath = constructPath("test", "results", "csv", false);
    bool file_exists = std::filesystem::exists(filepath);
    std::fstream fout;
    if (!file_exists) {
        fout.open(filepath, std::ios_base::out);
    } else {
        fout.open(filepath, std::ios_base::app);
    }
    if (fout.is_open()) {
        if (!file_exists) {
            printAnalysisHeader(fout);
        }
        fout << *this;
        fout.close();
    } else {
        perror(("error while opening file " + filepath).c_str());
    }
}

void printAnalysisHeader(std::ostream &out) {
    out << "version,descriptor,testname,fitness,rmse,correspondences,correct_correspondences,inliers,correct_inliers,";
    out << "voxel_size,normal_radius_coef,feature_radius_coef,distance_thr_coef,edge_thr,";
    out << "iteration,matching,randomness,filter,threshold,n_random,r_err,t_err,pcd_err,use_normals,";
    out << "normal_diff,corr_uniformity,lrf,metric,time,overlap_rmse\n";
}

std::ostream &operator<<(std::ostream &stream, const AlignmentAnalysis &analysis) {
    stream << VERSION << "," << analysis.parameters_.descriptor_id << "," << analysis.testname_ << ","
           << analysis.fitness_ << "," << analysis.rmse_ << ",";
    stream << analysis.correspondences_.size() << "," << analysis.correct_correspondences_.size() << ",";
    stream << analysis.inlier_pairs_.size() << "," << analysis.correct_inlier_pairs_.size() << ",";
    stream << analysis.parameters_.voxel_size << ",";
    stream << analysis.parameters_.normal_radius_coef << ",";
    stream << analysis.parameters_.feature_radius_coef << ",";
    stream << analysis.parameters_.distance_thr_coef << ",";
    stream << analysis.parameters_.edge_thr_coef << ",";
    stream << analysis.iterations_ << ",";
    stream << analysis.parameters_.matching_id << ",";
    stream << analysis.parameters_.randomness << ",";
    auto func = getUniquenessFunction(analysis.parameters_.func_id);
    if (func != nullptr) {
        stream << analysis.parameters_.func_id << "," << UNIQUENESS_THRESHOLD << "," << N_RANDOM_FEATURES << ",";
    } else {
        stream << ",,,";
    }
    stream << analysis.r_error_ << "," << analysis.t_error_ << "," << analysis.pcd_error_ << ",";
    stream << analysis.parameters_.use_normals << "," << analysis.normal_diff_ << ",";
    stream << analysis.corr_uniformity_ << "," << analysis.parameters_.lrf_id << ",";
    stream << analysis.parameters_.metric_id << "," << analysis.time_ << "," << analysis.overlap_error_ << "\n";
    return stream;
}


// __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__

#include <vw/Math/Vector.h>
#include <vw/FileIO/DiskImageResourceGDAL.h>
#include <vw/FileIO/FileUtils.h>
#include <vw/Cartography/Datum.h>
#include <vw/Cartography/GeoReference.h>
#include <asp/Camera/RPCModel.h>
#include <asp/Core/Common.h>

#include <gdal.h>
#include <gdal_priv.h>

#include <boost/smart_ptr/scoped_ptr.hpp>
#include <boost/smart_ptr/shared_ptr.hpp>

using namespace vw;

namespace asp {

  void RPCModel::initialize(DiskImageResourceGDAL* resource) {
    // Extract the datum (by means of georeference)
    cartography::GeoReference georef;
    cartography::read_georeference(georef, *resource);
    m_datum = georef.datum();

    // Extract RPC Info
    boost::shared_ptr<GDALDataset> dataset = resource->get_dataset_ptr();
    if (!dataset)
      vw_throw(NotFoundErr() << "RPCModel: Could not read data. No file has been opened.");

    GDALRPCInfo gdal_rpc;
    if (!GDALExtractRPCInfo(dataset->GetMetadata("RPC"),
                            &gdal_rpc))
      vw_throw(NotFoundErr() << "RPCModel: GDAL resource appears not to have RPC metadata.");

    // Copy information over to our data structures.
    m_lonlatheight_offset = Vector3(gdal_rpc.dfLONG_OFF,
                                    gdal_rpc.dfLAT_OFF,
                                    gdal_rpc.dfHEIGHT_OFF);
    m_lonlatheight_scale = Vector3(gdal_rpc.dfLONG_SCALE,
                                   gdal_rpc.dfLAT_SCALE,
                                   gdal_rpc.dfHEIGHT_SCALE);
    m_xy_offset = Vector2(gdal_rpc.dfSAMP_OFF,   gdal_rpc.dfLINE_OFF);
    m_xy_scale  = Vector2(gdal_rpc.dfSAMP_SCALE, gdal_rpc.dfLINE_SCALE);

    m_line_num_coeff   = CoeffVec(gdal_rpc.adfLINE_NUM_COEFF);
    m_line_den_coeff   = CoeffVec(gdal_rpc.adfLINE_DEN_COEFF);
    m_sample_num_coeff = CoeffVec(gdal_rpc.adfSAMP_NUM_COEFF);
    m_sample_den_coeff = CoeffVec(gdal_rpc.adfSAMP_DEN_COEFF);
  }

  RPCModel::RPCModel(std::string const& filename) {
    std::string ext = get_extension(filename);
    if (ext == ".rpb") {
      load_rpb_file(filename);
      return;
    }

    // Must have this check, otherwise GDAL prints an error.
    if (has_image_extension(filename)) {
      boost::scoped_ptr<DiskImageResourceGDAL> s_ptr(new DiskImageResourceGDAL(filename));
      initialize(s_ptr.get());
    }else{
      // Throw an error. It will be caught, but it will get printed
      // only if no other approaches turn out to work later on.
      vw_throw(ArgumentErr() << "Not an image " << filename);
    }

  }

  RPCModel::RPCModel(DiskImageResourceGDAL* resource ) {
    initialize(resource);
  }

  // The constructor just copies all of the input data
  RPCModel::RPCModel(cartography::Datum const& datum,
                     Vector<double,20> const& line_num_coeff,
                     Vector<double,20> const& line_den_coeff,
                     Vector<double,20> const& samp_num_coeff,
                     Vector<double,20> const& samp_den_coeff,
                     Vector2           const& xy_offset,
                     Vector2           const& xy_scale,
                     Vector3           const& lonlatheight_offset,
                     Vector3           const& lonlatheight_scale) :
    m_datum(datum),
    m_line_num_coeff(line_num_coeff),
    m_line_den_coeff(line_den_coeff),
    m_sample_num_coeff(samp_num_coeff),
    m_sample_den_coeff(samp_den_coeff),
    m_xy_offset(xy_offset),
    m_xy_scale(xy_scale),
    m_lonlatheight_offset(lonlatheight_offset),
    m_lonlatheight_scale(lonlatheight_scale) {}

  void RPCModel::load_rpb_file(std::string const& filename) {
    //vw_out() << "Reading RPC model from RPB file, defaulting to WGS84 datum.\n";
    m_datum.set_well_known_datum("WGS84");
    std::ifstream f(filename.c_str());

    std::string line;
    std::vector<std::string> tokens;
    bool lineNumCoeffs = false,
      lineDenCoeffs = false,
      sampNumCoeffs = false,
      sampDenCoeffs = false;
    int coeff_index = 0, max_coeff_index = 0;

    // Read through each line in the file
    while (std::getline(f, line)) {

      try {
        // Break up the line
        boost::split(tokens, line, boost::is_any_of("=,;"));

        // Parse keywords
        if (line.find("lineOffset") != std::string::npos)
          m_xy_offset[1] = atof(tokens[1].c_str());
        if (line.find("sampOffset") != std::string::npos)
          m_xy_offset[0] = atof(tokens[1].c_str());
        if (line.find("latOffset") != std::string::npos)
          m_lonlatheight_offset[1] = atof(tokens[1].c_str());
        if (line.find("longOffset") != std::string::npos)
          m_lonlatheight_offset[0] = atof(tokens[1].c_str());
        if (line.find("heightOffset") != std::string::npos)
          m_lonlatheight_offset[2] = atof(tokens[1].c_str());
        if (line.find("lineScale") != std::string::npos)
          m_xy_scale[1] = atof(tokens[1].c_str());
        if (line.find("sampScale") != std::string::npos)
          m_xy_scale[0] = atof(tokens[1].c_str());
        if (line.find("latScale") != std::string::npos)
          m_lonlatheight_scale[1] = atof(tokens[1].c_str());
        if (line.find("longScale") != std::string::npos)
          m_lonlatheight_scale[0] = atof(tokens[1].c_str());
        if (line.find("heightScale") != std::string::npos)
          m_lonlatheight_scale[2] = atof(tokens[1].c_str());

        // Handle the RPC coefficients.
        if (lineNumCoeffs) {
          m_line_num_coeff[coeff_index] = atof(tokens[0].c_str());
          ++coeff_index;
        }
        if (lineDenCoeffs) {
          m_line_den_coeff[coeff_index] = atof(tokens[0].c_str());
          ++coeff_index;
        }
        if (sampNumCoeffs) {
          m_sample_num_coeff[coeff_index] = atof(tokens[0].c_str());
          ++coeff_index;
        }
        if (sampDenCoeffs) {
          m_sample_den_coeff[coeff_index] = atof(tokens[0].c_str());
          ++coeff_index;
        }

        // Start of a coefficient sequence
        if (line.find("lineNumCoef") != std::string::npos)
          lineNumCoeffs = true;
        if (line.find("lineDenCoef") != std::string::npos)
          lineDenCoeffs = true;
        if (line.find("sampNumCoef") != std::string::npos)
          sampNumCoeffs = true;
        if (line.find("sampDenCoef") != std::string::npos)
          sampDenCoeffs = true;

        // Done reading a coefficient sequence.
        if (line.find(")") != std::string::npos) {
          lineNumCoeffs = false;
          lineDenCoeffs = false;
          sampNumCoeffs = false;
          sampDenCoeffs = false;
          if (coeff_index > max_coeff_index)
            max_coeff_index = coeff_index;
          coeff_index = 0;
        }
      } catch(...) {
        vw_throw(ArgumentErr() << "Error reading file " << filename
                 << ", line = "  << line);
      }
    } // End loop through lines.
    f.close();
    // Basic error check
    if (max_coeff_index != 20)
      vw_throw(ArgumentErr() << "Error reading file " << filename
               << ", loaded wrong number of coefficients!");
  }

  // All of these implementations are largely inspired by the GDAL
  // code. We don't use the GDAL code unfortunately because they don't
  // make that part of the API available. However I believe this is a
  // safe reinterpretation that is safe to distribute.
  Vector2 RPCModel::point_to_pixel(Vector3 const& point) const {
    return geodetic_to_pixel(m_datum.cartesian_to_geodetic(point));
  }

  Vector2 RPCModel::geodetic_to_pixel(Vector3 const& geodetic) const {

    // Should we verify that the  input geodetic is in the box?

    Vector3 normalized_geodetic =
      elem_quot(geodetic - m_lonlatheight_offset, m_lonlatheight_scale);

    Vector2 normalized_pixel = normalized_geodetic_to_normalized_pixel(normalized_geodetic);

    return elem_prod(normalized_pixel, m_xy_scale) + m_xy_offset;
  }

  Vector2 RPCModel::normalized_geodetic_to_normalized_pixel
  (Vector3 const& normalized_geodetic,
   RPCModel::CoeffVec const& line_num_coeff,
   RPCModel::CoeffVec const& line_den_coeff,
   RPCModel::CoeffVec const& sample_num_coeff,
   RPCModel::CoeffVec const& sample_den_coeff){

    CoeffVec term = calculate_terms(normalized_geodetic);
    Vector2 normalized_pixel(dot_prod(term,sample_num_coeff) /
                             dot_prod(term,sample_den_coeff),
                             dot_prod(term,line_num_coeff) /
                             dot_prod(term,line_den_coeff));

    return normalized_pixel;
  }

  Vector2 RPCModel::normalized_geodetic_to_normalized_pixel
  (Vector3 const& normalized_geodetic) const {

    return normalized_geodetic_to_normalized_pixel(normalized_geodetic,
                                                   m_line_num_coeff,
                                                   m_line_den_coeff,
                                                   m_sample_num_coeff,
                                                   m_sample_den_coeff
                                                   );
  }

  RPCModel::CoeffVec RPCModel::calculate_terms(vw::Vector3 const& normalized_geodetic) {

    double x = normalized_geodetic.x(); // normalized lon
    double y = normalized_geodetic.y(); // normalized lat
    double z = normalized_geodetic.z(); // normalized height
    CoeffVec result;
    result[ 0] = 1.0;
    result[ 1] = x;
    result[ 2] = y;
    result[ 3] = z;
    result[ 4] = x*y;
    result[ 5] = x*z;
    result[ 6] = y*z;
    result[ 7] = x*x;
    result[ 8] = y*y;
    result[ 9] = z*z;
    result[10] = x*y*z;
    result[11] = x*x*x;
    result[12] = x*y*y;
    result[13] = x*z*z;
    result[14] = x*x*y;
    result[15] = y*y*y;
    result[16] = y*z*z;
    result[17] = x*x*z;
    result[18] = y*y*z;
    result[19] = z*z*z;
    return result;
  }

  vw::Vector<int,20> RPCModel::get_coeff_order() {
    vw::Vector<int,20> result;
    for (int i= 0; i< 3; ++i) result[i] = 1;
    for (int i= 3; i<10; ++i) result[i] = 2;
    for (int i=10; i<20; ++i) result[i] = 3;
    return result;
  }

  vw::Matrix<double, 20, 2> RPCModel::terms_Jacobian2(vw::Vector3 const& normalized_geodetic) {
    // Partial derivatives of the terms returned by the
    // calculate_terms() function in respect to the first two
    // variables only (unlike the terms_Jacobian3() function).

    vw::Matrix<double, 20, 2> M;
    double x = normalized_geodetic.x(); // normalized lon
    double y = normalized_geodetic.y(); // normalized lat
    double z = normalized_geodetic.z(); // normalized height

    // df/dx            df/dy               // f
    M[ 0][0] = 0.0;     M[ 0][1] = 0.0;     // 1
    M[ 1][0] = 1.0;     M[ 1][1] = 0.0;     // x
    M[ 2][0] = 0.0;     M[ 2][1] = 1.0;     // y
    M[ 3][0] = 0.0;     M[ 3][1] = 0.0;     // z
    M[ 4][0] = y;       M[ 4][1] = x;       // xy
    M[ 5][0] = z;       M[ 5][1] = 0.0;     // xz
    M[ 6][0] = 0.0;     M[ 6][1] = z;       // yz
    M[ 7][0] = 2.0*x;   M[ 7][1] = 0.0;     // xx
    M[ 8][0] = 0.0;     M[ 8][1] = 2.0*y;   // yy
    M[ 9][0] = 0.0;     M[ 9][1] = 0.0;     // zz
    M[10][0] = y*z;     M[10][1] = x*z;     // xyz
    M[11][0] = 3.0*x*x; M[11][1] = 0.0;     // xxx
    M[12][0] = y*y;     M[12][1] = 2.0*x*y; // xyy
    M[13][0] = z*z;     M[13][1] = 0.0;     // xzz
    M[14][0] = 2.0*x*y; M[14][1] = x*x;     // xxy
    M[15][0] = 0.0;     M[15][1] = 3.0*y*y; // yyy
    M[16][0] = 0.0;     M[16][1] = z*z;     // yzz
    M[17][0] = 2.0*x*z; M[17][1] = 0.0;     // xxz
    M[18][0] = 0.0;     M[18][1] = 2.0*y*z; // yyz
    M[19][0] = 0.0;     M[19][1] = 0.0;     // zzz

    return M;
  }

  vw::Matrix<double, 20, 3> RPCModel::terms_Jacobian3(vw::Vector3 const& normalized_geodetic) {
    // Partial derivatives of the terms returned by the
    // calculate_terms() function in respect to all three
    // variables only (unlike the terms_Jacobian2() function).

    vw::Matrix<double, 20, 3> M;
    double x = normalized_geodetic.x(); // normalized lon
    double y = normalized_geodetic.y(); // normalized lat
    double z = normalized_geodetic.z(); // normalized height

    // df/dx            df/dy               df/dz               // f
    M[ 0][0] = 0.0;     M[ 0][1] = 0.0;     M[ 0][2] = 0.0;     // 1
    M[ 1][0] = 1.0;     M[ 1][1] = 0.0;     M[ 1][2] = 0.0;     // x
    M[ 2][0] = 0.0;     M[ 2][1] = 1.0;     M[ 2][2] = 0.0;     // y
    M[ 3][0] = 0.0;     M[ 3][1] = 0.0;     M[ 3][2] = 1.0;     // z
    M[ 4][0] = y;       M[ 4][1] = x;       M[ 4][2] = 0.0;     // xy
    M[ 5][0] = z;       M[ 5][1] = 0.0;     M[ 5][2] = x;       // xz
    M[ 6][0] = 0.0;     M[ 6][1] = z;       M[ 6][2] = y;       // yz
    M[ 7][0] = 2.0*x;   M[ 7][1] = 0.0;     M[ 7][2] = 0.0;     // xx
    M[ 8][0] = 0.0;     M[ 8][1] = 2.0*y;   M[ 8][2] = 0.0;     // yy
    M[ 9][0] = 0.0;     M[ 9][1] = 0.0;     M[ 9][2] = 2.0*z;   // zz
    M[10][0] = y*z;     M[10][1] = x*z;     M[10][2] = x*y;     // xyz
    M[11][0] = 3.0*x*x; M[11][1] = 0.0;     M[11][2] = 0.0;     // xxx
    M[12][0] = y*y;     M[12][1] = 2.0*x*y; M[12][2] = 0.0;     // xyy
    M[13][0] = z*z;     M[13][1] = 0.0;     M[13][2] = 2.0*x*z; // xzz
    M[14][0] = 2.0*x*y; M[14][1] = x*x;     M[14][2] = 0.0;     // xxy
    M[15][0] = 0.0;     M[15][1] = 3.0*y*y; M[15][2] = 0.0;     // yyy
    M[16][0] = 0.0;     M[16][1] = z*z;     M[16][2] = 2.0*y*z; // yzz
    M[17][0] = 2.0*x*z; M[17][1] = 0.0;     M[17][2] = x*x;     // xxz
    M[18][0] = 0.0;     M[18][1] = 2.0*y*z; M[18][2] = y*y;     // yyz
    M[19][0] = 0.0;     M[19][1] = 0.0;     M[19][2] = 3.0*z*z; // zzz

    return M;
  }

  RPCModel::CoeffVec
  RPCModel::quotient_Jacobian(RPCModel::CoeffVec const& c,
                              RPCModel::CoeffVec const& d,
                              RPCModel::CoeffVec const& u) {

    // Return the Jacobian of dot_prod(c, u) / dot_prod(d, u)
    // as a vector with 20 elements.

    double cu  = dot_prod(c, u);
    double du  = dot_prod(d, u);
    double den = du*du;

    return elem_quot(du * c - cu * d, den);
  }

  vw::Matrix3x3 RPCModel::normalization_Jacobian(Vector3 const& q) {

    // Return the Jacobian of the function
    // f(x1, x2, x3) = ((x1 - c1)/q1, (x2 - c2)/q2, (x3 - c3)/q3)

    vw::Matrix3x3 M;
    M[0][0] = 1.0/q[0]; M[0][1] = 0.0;      M[0][2] = 0.0;
    M[1][0] = 0.0;      M[1][1] = 1.0/q[1]; M[1][2] = 0.0;
    M[2][0] = 0.0;      M[2][1] = 0.0;      M[2][2] = 1.0/q[2];
    return M;
  }

  Matrix<double, 2, 3> RPCModel::geodetic_to_pixel_Jacobian(Vector3 const& geodetic) const {

    Vector3 normalized_geodetic = elem_quot(geodetic - m_lonlatheight_offset, m_lonlatheight_scale);

    CoeffVec term = calculate_terms(normalized_geodetic);

    CoeffVec Qs = quotient_Jacobian(sample_num_coeff(), sample_den_coeff(), term);
    CoeffVec Ql = quotient_Jacobian(line_num_coeff(),   line_den_coeff(),   term);
    Matrix<double, 20, 3> MN = terms_Jacobian3(normalized_geodetic) *
      normalization_Jacobian(m_lonlatheight_scale);

    Matrix<double, 2, 3> J;
    select_row(J, 0) = m_xy_scale[0] * transpose(Qs) * MN;
    select_row(J, 1) = m_xy_scale[1] * transpose(Ql) * MN;

    return J;
  }

  Matrix<double, 2, 2> RPCModel::normalized_geodetic_to_pixel_Jacobian(Vector3 const& normalized_geodetic) const {

    // This function is different from geodetic_to_pixel_Jacobian() in several respects:

    // 1. The input is the normalized geodetic, and the derivatives
    //    are in respect to the normalized geodetic as well.

    // 2. The derivatives are taken only in respect to the first two
    //    variables (normalized lon and lat, no height).

    // 3. The output is in normalized pixels (see m_xy_scale and m_xy_offset).

    CoeffVec term = calculate_terms(normalized_geodetic);

    CoeffVec Qs = quotient_Jacobian(sample_num_coeff(), sample_den_coeff(), term);
    CoeffVec Ql = quotient_Jacobian(line_num_coeff(),   line_den_coeff(),   term);

    Matrix<double, 20, 2> Jt = terms_Jacobian2(normalized_geodetic);

    Matrix<double, 2, 2> J;
    select_row(J, 0) = transpose(Qs) * Jt;
    select_row(J, 1) = transpose(Ql) * Jt;

    return J;
  }

  Matrix<double, 2, 3> RPCModel::geodetic_to_pixel_numerical_Jacobian(Vector3 const& geodetic, double tol) const {

    // Find the Jacobian of geodetic_to_pixel using numerical
    // differentiation. This is used for testing purposes.

    Matrix<double, 2, 3> J;

    Vector2 B  = geodetic_to_pixel(geodetic);

    Vector2 B0 = (geodetic_to_pixel(geodetic + Vector3(tol, 0,   0 )) - B)/tol;
    Vector2 B1 = (geodetic_to_pixel(geodetic + Vector3(0,   tol, 0 )) - B)/tol;
    Vector2 B2 = (geodetic_to_pixel(geodetic + Vector3(0,   0,   tol)) - B)/tol;

    select_col(J, 0) = B0;
    select_col(J, 1) = B1;
    select_col(J, 2) = B2;

    return J;
  }

  Vector2 RPCModel::image_to_ground(Vector2 const& pixel, double height, Vector2 lonlat_guess) const {

    // The absolute tolerance is experimental, needs more investigation
    double abs_tolerance = 1e-6;

    Vector2 normalized_pixel = elem_quot(pixel - m_xy_offset, m_xy_scale);

    // Initial guess for the normalized lon and lat
    if (lonlat_guess == Vector2(0.0, 0.0)){
      lonlat_guess = subvector(m_lonlatheight_offset, 0, 2);
    }
    Vector2 normalized_lonlat = elem_quot(lonlat_guess - subvector(m_lonlatheight_offset, 0, 2),
                                          subvector(m_lonlatheight_scale, 0, 2)
                                          );
    double len = norm_2(normalized_lonlat);
    if (len != len || len > 1.5){
      // If the input guess is NaN or unreasonable, use 0 as initial guess
      normalized_lonlat = Vector2(0.0, 0.0);
    }

    // 10 iterations should be enough for Newton's method to converge
    for (int iter = 0; iter < 10; iter++){

      Vector3 normalized_geodetic;
      normalized_geodetic[0] = normalized_lonlat[0];
      normalized_geodetic[1] = normalized_lonlat[1];
      normalized_geodetic[2] = (height - m_lonlatheight_offset[2])/m_lonlatheight_scale[2];

      Vector2              p = normalized_geodetic_to_normalized_pixel(normalized_geodetic);
      Matrix<double, 2, 2> J = normalized_geodetic_to_pixel_Jacobian(normalized_geodetic);

      // The inverse matrix computed analytically
      double det = J[0][0]*J[1][1] - J[0][1]*J[1][0];
      Matrix<double, 2, 2> invJ;
      invJ[0][0] =  J[1][1];
      invJ[0][1] = -J[0][1];
      invJ[1][0] = -J[1][0];
      invJ[1][1] =  J[0][0];
      invJ /= det;

      // Newton's method for F(x) = y is
      // x = x - J^{-1}(F(x) - y)
      Vector2 error_try = p - normalized_pixel;
      normalized_lonlat -= invJ*error_try;

      // Absolute error convergence criterion
      double  norm_try = norm_2(error_try);
      if (norm_try < abs_tolerance) {
        break;
      }

    }

    Vector2 lonlat = elem_prod(normalized_lonlat, subvector(m_lonlatheight_scale, 0, 2))
      + subvector(m_lonlatheight_offset, 0, 2);

    return lonlat;

  }

  void RPCModel::point_and_dir(Vector2 const& pix, Vector3 & P, Vector3 & dir) const {

    // For an RPC model there is no defined origin so it and the ray need to be computed.

    // Center of valid region to bottom of valid region (normalized)
    const double VERT_SCALE_FACTOR = 0.9; // - The virtual center should be above the terrain
    double  height_up = m_lonlatheight_offset[2] + m_lonlatheight_scale[2]*VERT_SCALE_FACTOR;
    double  height_dn = m_lonlatheight_offset[2] - m_lonlatheight_scale[2]*VERT_SCALE_FACTOR;

    //vw_out() << "m_lonlatheight_offset = " << m_lonlatheight_offset << std::endl;
    //vw_out() << "m_lonlatheight_scale = " << m_lonlatheight_scale << std::endl;

    //vw_out() << "Height up = " << height_up << std::endl;
    //vw_out() << "Height dn = " << height_dn << std::endl;

    // Given the pixel and elevation, estimate lon-lat.
    // Use m_lonlatheight_offset as initial guess for lonlat_up,
    // and then use lonlat_up as initial guess for lonlat_dn.
    Vector2 lonlat_up = image_to_ground(pix, height_up, subvector(m_lonlatheight_offset, 0, 2));
    Vector2 lonlat_dn = image_to_ground(pix, height_dn, lonlat_up);

    //vw_out() << "lonlat_up = " << lonlat_up << std::endl;
    //vw_out() << "lonlat_dn = " << lonlat_dn << std::endl;

    Vector3 geo_up = Vector3(lonlat_up[0], lonlat_up[1], height_up);
    Vector3 geo_dn = Vector3(lonlat_dn[0], lonlat_dn[1], height_dn);

    //vw_out() << "geo_up = " << geo_up << std::endl;
    //vw_out() << "geo_dn = " << geo_dn << std::endl;

    Vector3 P_up = m_datum.geodetic_to_cartesian(geo_up);
    Vector3 P_dn = m_datum.geodetic_to_cartesian(geo_dn);

    dir = normalize(P_dn - P_up);

    // Set the origin location very far in the opposite direction of the pointing vector,
    //  to put it high above the terrain.
    const double LONG_SCALE_UP = 10000; // This is a distance in meters approx from the top of the llh valid cube
    P = P_up - dir*LONG_SCALE_UP;
  }

  Vector3 RPCModel::camera_center(Vector2 const& pix) const{
    // Return an arbitrarily chosen point on the ray back-projected
    // through the camera from the current pixel.
    Vector3 P;
    Vector3 dir;
    point_and_dir(pix, P, dir);
    return P;
  }

  Vector3 RPCModel::pixel_to_vector(Vector2 const& pix) const {
    // Find the normalized direction of the ray back-projected through
    // the camera from the current pixel.
    Vector3 P;
    Vector3 dir;
    point_and_dir(pix, P, dir);
    return dir;
  }

  std::ostream& operator<<(std::ostream& os, const RPCModel& rpc) {
    os << "RPC Model:"         << std::endl
       << "Line Numerator: "   << rpc.line_num_coeff()      << std::endl
       << "Line Denominator: " << rpc.line_den_coeff()      << std::endl
       << "Samp Numerator: "   << rpc.sample_num_coeff()    << std::endl
       << "Samp Denominator: " << rpc.sample_den_coeff()    << std::endl
       << "XY Offset: "        << rpc.xy_offset()           << std::endl
       << "XY Scale: "         << rpc.xy_scale()            << std::endl
       << "Geodetic Offset: "  << rpc.lonlatheight_offset() << std::endl
       << "Geodetic Scale: "   << rpc.lonlatheight_scale();
    return os;
  }
}