// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2011-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_PARTITION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_PARTITION_HPP

#include <cstddef>
#include <vector>
#include <boost/range.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/algorithms/assign.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace partition
{

template <int Dimension, typename Box>
inline void divide_box(Box const& box, Box& lower_box, Box& upper_box)
{
    typedef typename coordinate_type<Box>::type ctype;

    // Divide input box into two parts, e.g. left/right
    ctype two = 2;
    ctype mid = (geometry::get<min_corner, Dimension>(box)
            + geometry::get<max_corner, Dimension>(box)) / two;

    lower_box = box;
    upper_box = box;
    geometry::set<max_corner, Dimension>(lower_box, mid);
    geometry::set<min_corner, Dimension>(upper_box, mid);
}

// Divide forward_range into three subsets: lower, upper and oversized
// (not-fitting)
// (lower == left or bottom, upper == right or top)
template <typename OverlapsPolicy, typename Box, typename IteratorVector>
inline void divide_into_subsets(Box const& lower_box,
        Box const& upper_box,
        IteratorVector const& input,
        IteratorVector& lower,
        IteratorVector& upper,
        IteratorVector& exceeding)
{
    typedef typename boost::range_iterator
        <
            IteratorVector const
        >::type it_type;

    for(it_type it = boost::begin(input); it != boost::end(input); ++it)
    {
        bool const lower_overlapping = OverlapsPolicy::apply(lower_box, **it);
        bool const upper_overlapping = OverlapsPolicy::apply(upper_box, **it);

        if (lower_overlapping && upper_overlapping)
        {
            exceeding.push_back(*it);
        }
        else if (lower_overlapping)
        {
            lower.push_back(*it);
        }
        else if (upper_overlapping)
        {
            upper.push_back(*it);
        }
        else
        {
            // Is nowhere. That is (since 1.58) possible, it might be
            // skipped by the OverlapsPolicy to enhance performance
        }
    }
}

template
<
    typename ExpandPolicy,
    typename Box,
    typename IteratorVector
>
inline void expand_with_elements(Box& total, IteratorVector const& input)
{
    typedef typename boost::range_iterator<IteratorVector const>::type it_type;
    for(it_type it = boost::begin(input); it != boost::end(input); ++it)
    {
        ExpandPolicy::apply(total, **it);
    }
}

// Match forward_range with itself
template <typename Policy, typename IteratorVector>
inline void handle_one(IteratorVector const& input, Policy& policy)
{
    if (boost::size(input) == 0)
    {
        return;
    }

    typedef typename boost::range_iterator<IteratorVector const>::type it_type;

    // Quadratic behaviour at lowest level (lowest quad, or all exceeding)
    for (it_type it1 = boost::begin(input); it1 != boost::end(input); ++it1)
    {
        it_type it2 = it1;
        for (++it2; it2 != boost::end(input); ++it2)
        {
            policy.apply(**it1, **it2);
        }
    }
}

// Match forward range 1 with forward range 2
template
<
    typename Policy,
    typename IteratorVector1,
    typename IteratorVector2
>
inline void handle_two(IteratorVector1 const& input1,
        IteratorVector2 const& input2,
        Policy& policy)
{
    typedef typename boost::range_iterator
        <
            IteratorVector1 const
        >::type iterator_type1;

    typedef typename boost::range_iterator
        <
            IteratorVector2 const
        >::type iterator_type2;

    if (boost::size(input1) == 0 || boost::size(input2) == 0)
    {
        return;
    }

    for(iterator_type1 it1 = boost::begin(input1);
        it1 != boost::end(input1);
        ++it1)
    {
        for(iterator_type2 it2 = boost::begin(input2);
            it2 != boost::end(input2);
            ++it2)
        {
            policy.apply(**it1, **it2);
        }
    }
}

template <typename IteratorVector>
inline bool recurse_ok(IteratorVector const& input,
                std::size_t min_elements, std::size_t level)
{
    return boost::size(input) >= min_elements
        && level < 100;
}

template <typename IteratorVector1, typename IteratorVector2>
inline bool recurse_ok(IteratorVector1 const& input1,
                IteratorVector2 const& input2,
                std::size_t min_elements, std::size_t level)
{
    return boost::size(input1) >= min_elements
        && recurse_ok(input2, min_elements, level);
}

template
<
    typename IteratorVector1,
    typename IteratorVector2,
    typename IteratorVector3
>
inline bool recurse_ok(IteratorVector1 const& input1,
                IteratorVector2 const& input2,
                IteratorVector3 const& input3,
                std::size_t min_elements, std::size_t level)
{
    return boost::size(input1) >= min_elements
        && recurse_ok(input2, input3, min_elements, level);
}

template
<
    int Dimension,
    typename Box,
    typename OverlapsPolicy1,
    typename OverlapsPolicy2,
    typename ExpandPolicy1,
    typename ExpandPolicy2,
    typename VisitBoxPolicy
>
class partition_two_ranges;

template
<
    int Dimension,
    typename Box,
    typename OverlapsPolicy,
    typename ExpandPolicy,
    typename VisitBoxPolicy
>
class partition_one_range
{
    template <typename IteratorVector>
    static inline Box get_new_box(IteratorVector const& input)
    {
        Box box;
        geometry::assign_inverse(box);
        expand_with_elements<ExpandPolicy>(box, input);
        return box;
    }

    template <typename Policy, typename IteratorVector>
    static inline void next_level(Box const& box,
            IteratorVector const& input,
            std::size_t level, std::size_t min_elements,
            Policy& policy, VisitBoxPolicy& box_policy)
    {
        if (recurse_ok(input, min_elements, level))
        {
            partition_one_range
            <
                1 - Dimension,
                Box,
                OverlapsPolicy,
                ExpandPolicy,
                VisitBoxPolicy
            >::apply(box, input, level + 1, min_elements, policy, box_policy);
        }
        else
        {
            handle_one(input, policy);
        }
    }

    // Function to switch to two forward ranges if there are
    // geometries exceeding the separation line
    template <typename Policy, typename IteratorVector>
    static inline void next_level2(Box const& box,
            IteratorVector const& input1,
            IteratorVector const& input2,
            std::size_t level, std::size_t min_elements,
            Policy& policy, VisitBoxPolicy& box_policy)
    {
        if (recurse_ok(input1, input2, min_elements, level))
        {
            partition_two_ranges
            <
                1 - Dimension,
                Box,
                OverlapsPolicy, OverlapsPolicy,
                ExpandPolicy, ExpandPolicy,
                VisitBoxPolicy
            >::apply(box, input1, input2, level + 1, min_elements,
                policy, box_policy);
        }
        else
        {
            handle_two(input1, input2, policy);
        }
    }

public :
    template <typename Policy, typename IteratorVector>
    static inline void apply(Box const& box,
            IteratorVector const& input,
            std::size_t level,
            std::size_t min_elements,
            Policy& policy, VisitBoxPolicy& box_policy)
    {
        box_policy.apply(box, level);

        Box lower_box, upper_box;
        divide_box<Dimension>(box, lower_box, upper_box);

        IteratorVector lower, upper, exceeding;
        divide_into_subsets<OverlapsPolicy>(lower_box, upper_box,
                    input, lower, upper, exceeding);

        if (boost::size(exceeding) > 0)
        {
            // Get the box of exceeding-only
            Box exceeding_box = get_new_box(exceeding);

            // Recursively do exceeding elements only, in next dimension they
            // will probably be less exceeding within the new box
            next_level(exceeding_box, exceeding, level, min_elements,
                policy, box_policy);

            // Switch to two forward ranges, combine exceeding with
            // lower resp upper, but not lower/lower, upper/upper
            next_level2(exceeding_box, exceeding, lower, level, min_elements,
                policy, box_policy);
            next_level2(exceeding_box, exceeding, upper, level, min_elements,
                policy, box_policy);
        }

        // Recursively call operation both parts
        next_level(lower_box, lower, level, min_elements, policy, box_policy);
        next_level(upper_box, upper, level, min_elements, policy, box_policy);
    }
};

template
<
    int Dimension,
    typename Box,
    typename OverlapsPolicy1,
    typename OverlapsPolicy2,
    typename ExpandPolicy1,
    typename ExpandPolicy2,
    typename VisitBoxPolicy
>
class partition_two_ranges
{
    template
    <
        typename Policy,
        typename IteratorVector1,
        typename IteratorVector2
    >
    static inline void next_level(Box const& box,
            IteratorVector1 const& input1,
            IteratorVector2 const& input2,
            std::size_t level, std::size_t min_elements,
            Policy& policy, VisitBoxPolicy& box_policy)
    {
        partition_two_ranges
        <
            1 - Dimension,
            Box,
            OverlapsPolicy1,
            OverlapsPolicy2,
            ExpandPolicy1,
            ExpandPolicy2,
            VisitBoxPolicy
        >::apply(box, input1, input2, level + 1, min_elements,
                 policy, box_policy);
    }

    template <typename ExpandPolicy, typename IteratorVector>
    static inline Box get_new_box(IteratorVector const& input)
    {
        Box box;
        geometry::assign_inverse(box);
        expand_with_elements<ExpandPolicy>(box, input);
        return box;
    }

    template <typename IteratorVector1, typename IteratorVector2>
    static inline Box get_new_box(IteratorVector1 const& input1,
                    IteratorVector2 const& input2)
    {
        Box box = get_new_box<ExpandPolicy1>(input1);
        expand_with_elements<ExpandPolicy2>(box, input2);
        return box;
    }

public :
    template
    <
        typename Policy,
        typename IteratorVector1,
        typename IteratorVector2
    >
    static inline void apply(Box const& box,
            IteratorVector1 const& input1,
            IteratorVector2 const& input2,
            std::size_t level,
            std::size_t min_elements,
            Policy& policy, VisitBoxPolicy& box_policy)
    {
        box_policy.apply(box, level);

        Box lower_box, upper_box;
        divide_box<Dimension>(box, lower_box, upper_box);

        IteratorVector1 lower1, upper1, exceeding1;
        IteratorVector2 lower2, upper2, exceeding2;
        divide_into_subsets<OverlapsPolicy1>(lower_box, upper_box,
                    input1, lower1, upper1, exceeding1);
        divide_into_subsets<OverlapsPolicy2>(lower_box, upper_box,
                    input2, lower2, upper2, exceeding2);

        if (boost::size(exceeding1) > 0)
        {
            // All exceeding from 1 with 2:

            if (recurse_ok(exceeding1, exceeding2, min_elements, level))
            {
                Box exceeding_box = get_new_box(exceeding1, exceeding2);
                next_level(exceeding_box, exceeding1, exceeding2, level,
                           min_elements, policy, box_policy);
            }
            else
            {
                handle_two(exceeding1, exceeding2, policy);
            }

            // All exceeding from 1 with lower and upper of 2:

            // (Check sizes of all three forward ranges to avoid recurse into
            // the same combinations again and again)
            if (recurse_ok(lower2, upper2, exceeding1, min_elements, level))
            {
                Box exceeding_box = get_new_box<ExpandPolicy1>(exceeding1);
                next_level(exceeding_box, exceeding1, lower2, level,
                           min_elements, policy, box_policy);
                next_level(exceeding_box, exceeding1, upper2, level,
                           min_elements, policy, box_policy);
            }
            else
            {
                handle_two(exceeding1, lower2, policy);
                handle_two(exceeding1, upper2, policy);
            }
        }

        if (boost::size(exceeding2) > 0)
        {
            // All exceeding from 2 with lower and upper of 1:
            if (recurse_ok(lower1, upper1, exceeding2, min_elements, level))
            {
                Box exceeding_box = get_new_box<ExpandPolicy2>(exceeding2);
                next_level(exceeding_box, lower1, exceeding2, level,
                    min_elements, policy, box_policy);
                next_level(exceeding_box, upper1, exceeding2, level,
                    min_elements, policy, box_policy);
            }
            else
            {
                handle_two(lower1, exceeding2, policy);
                handle_two(upper1, exceeding2, policy);
            }
        }

        if (recurse_ok(lower1, lower2, min_elements, level))
        {
            next_level(lower_box, lower1, lower2, level,
                       min_elements, policy, box_policy);
        }
        else
        {
            handle_two(lower1, lower2, policy);
        }
        if (recurse_ok(upper1, upper2, min_elements, level))
        {
            next_level(upper_box, upper1, upper2, level,
                       min_elements, policy, box_policy);
        }
        else
        {
            handle_two(upper1, upper2, policy);
        }
    }
};

struct visit_no_policy
{
    template <typename Box>
    static inline void apply(Box const&, std::size_t )
    {}
};

struct include_all_policy
{
    template <typename Item>
    static inline bool apply(Item const&)
    {
        return true;
    }
};

}} // namespace detail::partition

template
<
    typename Box,
    typename ExpandPolicy1,
    typename OverlapsPolicy1,
    typename ExpandPolicy2 = ExpandPolicy1,
    typename OverlapsPolicy2 = OverlapsPolicy1,
    typename IncludePolicy1 = detail::partition::include_all_policy,
    typename IncludePolicy2 = detail::partition::include_all_policy,
    typename VisitBoxPolicy = detail::partition::visit_no_policy
>
class partition
{
    template
    <
        typename ExpandPolicy,
        typename IncludePolicy,
        typename ForwardRange,
        typename IteratorVector
    >
    static inline void expand_to_range(ForwardRange const& forward_range,
                Box& total, IteratorVector& iterator_vector)
    {
        for(typename boost::range_iterator<ForwardRange const>::type it
            = boost::begin(forward_range);
            it != boost::end(forward_range);
            ++it)
        {
            if (IncludePolicy::apply(*it))
            {
                ExpandPolicy::apply(total, *it);
                iterator_vector.push_back(it);
            }
        }
    }

public :
    template <typename ForwardRange, typename VisitPolicy>
    static inline void apply(ForwardRange const& forward_range,
            VisitPolicy& visitor,
            std::size_t min_elements = 16,
            VisitBoxPolicy box_visitor = detail::partition::visit_no_policy()
            )
    {
        typedef typename boost::range_iterator
            <
                ForwardRange const
            >::type iterator_type;

        if (std::size_t(boost::size(forward_range)) > min_elements)
        {
            std::vector<iterator_type> iterator_vector;
            Box total;
            assign_inverse(total);
            expand_to_range<ExpandPolicy1, IncludePolicy1>(forward_range,
                    total, iterator_vector);

            detail::partition::partition_one_range
                <
                    0, Box,
                    OverlapsPolicy1,
                    ExpandPolicy1,
                    VisitBoxPolicy
                >::apply(total, iterator_vector, 0, min_elements,
                         visitor, box_visitor);
        }
        else
        {
            for(iterator_type it1 = boost::begin(forward_range);
                it1 != boost::end(forward_range);
                ++it1)
            {
                iterator_type it2 = it1;
                for(++it2; it2 != boost::end(forward_range); ++it2)
                {
                    visitor.apply(*it1, *it2);
                }
            }
        }
    }

    template
    <
        typename ForwardRange1,
        typename ForwardRange2,
        typename VisitPolicy
    >
    static inline void apply(ForwardRange1 const& forward_range1,
                ForwardRange2 const& forward_range2,
                VisitPolicy& visitor,
                std::size_t min_elements = 16,
                VisitBoxPolicy box_visitor
                    = detail::partition::visit_no_policy()
                )
    {
        typedef typename boost::range_iterator
            <
                ForwardRange1 const
            >::type iterator_type1;

        typedef typename boost::range_iterator
            <
                ForwardRange2 const
            >::type iterator_type2;

        if (std::size_t(boost::size(forward_range1)) > min_elements
            && std::size_t(boost::size(forward_range2)) > min_elements)
        {
            std::vector<iterator_type1> iterator_vector1;
            std::vector<iterator_type2> iterator_vector2;
            Box total;
            assign_inverse(total);
            expand_to_range<ExpandPolicy1, IncludePolicy1>(forward_range1,
                    total, iterator_vector1);
            expand_to_range<ExpandPolicy2, IncludePolicy2>(forward_range2,
                    total, iterator_vector2);

            detail::partition::partition_two_ranges
                <
                    0, Box, OverlapsPolicy1, OverlapsPolicy2,
                    ExpandPolicy1, ExpandPolicy2, VisitBoxPolicy
                >::apply(total, iterator_vector1, iterator_vector2,
                         0, min_elements, visitor, box_visitor);
        }
        else
        {
            for(iterator_type1 it1 = boost::begin(forward_range1);
                it1 != boost::end(forward_range1);
                ++it1)
            {
                for(iterator_type2 it2 = boost::begin(forward_range2);
                    it2 != boost::end(forward_range2);
                    ++it2)
                {
                    visitor.apply(*it1, *it2);
                }
            }
        }
    }
};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_PARTITION_HPP


/*
 * Copyright (c) 2020, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// Core open-loop LQ game solver based on Basar and Olsder, Chapter 6. All
// notation matches the text, though we shall assume that `c` (additive drift in
// dynamics) is always `0`, which holds because these dynamics are for delta x,
// delta us. Also, we have modified terms slightly to account for linear terms
// in the stage cost for control, i.e.
//       control penalty i = 0.5 \sum_j du_j^T R_ij (du_j + 2 r_ij)
//
// Solve a time-varying, finite horizon LQ game (finds open-loop Nash
// feedback strategies for both players).
//
// Assumes that dynamics are given by
//           ``` dx_{k+1} = A_k dx_k + \sum_i Bs[i]_k du[i]_k ```
//
// Returns strategies Ps, alphas. Here, all the Ps are zero (by default), and
// only the alphas are nonzero.
//
// Notation is based on derivation which may be found in the PDF included in
// this repository named "open_loop_lq_derivation.pdf".
//
///////////////////////////////////////////////////////////////////////////////

#include <ilqgames/dynamics/multi_player_integrable_system.h>
#include <ilqgames/solver/lq_feedback_solver.h>
#include <ilqgames/solver/lq_open_loop_solver.h>
#include <ilqgames/utils/linear_dynamics_approximation.h>
#include <ilqgames/utils/quadratic_cost_approximation.h>
#include <ilqgames/utils/strategy.h>

#include <glog/logging.h>
#include <Eigen/Core>
#include <vector>

namespace ilqgames {

std::vector<Strategy> LQOpenLoopSolver::Solve(
    const std::vector<LinearDynamicsApproximation>& linearization,
    const std::vector<std::vector<QuadraticCostApproximation>>&
        quadraticization,
    const VectorXf& x0, std::vector<VectorXf>* delta_xs,
    std::vector<std::vector<VectorXf>>* costates) {
  CHECK_EQ(linearization.size(), num_time_steps_);
  CHECK_EQ(quadraticization.size(), num_time_steps_);

  // Make sure delta_xs and costates are the right size.
  if (delta_xs) CHECK_NOTNULL(costates);
  if (costates) CHECK_NOTNULL(delta_xs);
  if (delta_xs) {
    delta_xs->resize(num_time_steps_);
    costates->resize(num_time_steps_);
    for (size_t kk = 0; kk < num_time_steps_; kk++) {
      (*delta_xs)[kk].resize(dynamics_->XDim());
      (*costates)[kk].resize(dynamics_->NumPlayers());
      for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++)
        (*costates)[kk][ii].resize(dynamics_->XDim());
    }
  }

  // List of player-indexed strategies (each of which is a time-indexed
  // affine state error-feedback controller). Since this is an open-loop
  // strategy, we will not change the default zero value of the P matrix.
  std::vector<Strategy> strategies;
  for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++)
    strategies.emplace_back(num_time_steps_, dynamics_->XDim(),
                            dynamics_->UDim(ii));

  // Initialize m^i and M^i and index first by time and then by player.
  for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++) {
    ms_.back()[ii] = quadraticization.back()[ii].state.grad;
    Ms_.back()[ii] = quadraticization.back()[ii].state.hess;
  }

  // (1) Work backward in time and cache "special" terms.
  // NOTE: time starts from the second-to-last entry since we'll treat the
  // final entry as a terminal cost as in Basar and Olsder, ch. 6.
  for (int kk = num_time_steps_ - 2; kk >= 0; kk--) {
    // Unpack linearization and quadraticization at this time step.
    const auto& lin = linearization[kk];
    const auto& quad = quadraticization[kk];

    // Campute capital lambdas.
    capital_lambdas_[kk].setIdentity();
    for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++) {
      const auto control_iter = quad[ii].control.find(ii);
      CHECK(control_iter != quad[ii].control.end());

      chol_Rs_[kk][ii].compute(control_iter->second.hess);
      warped_Bs_[kk][ii] = chol_Rs_[kk][ii].solve(lin.Bs[ii].transpose());
      warped_rs_[kk][ii] = chol_Rs_[kk][ii].solve(control_iter->second.grad);
      capital_lambdas_[kk] += lin.Bs[ii] * warped_Bs_[kk][ii] * Ms_[kk + 1][ii];
    }

    // Compute inv(capital lambda).
    qr_capital_lambdas_[kk].compute(capital_lambdas_[kk]);

    // Compute Ms and ms.
    intermediate_terms_[kk].setZero();
    for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++) {
      intermediate_terms_[kk] -=
          lin.Bs[ii] *
          (warped_Bs_[kk][ii] * ms_[kk + 1][ii] + warped_rs_[kk][ii]);
    }

    for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++) {
      Ms_[kk][ii] =
          quad[ii].state.hess + lin.A.transpose() * Ms_[kk + 1][ii] *
                                    qr_capital_lambdas_[kk].solve(lin.A);
      ms_[kk][ii] =
          quad[ii].state.grad +
          lin.A.transpose() * (ms_[kk + 1][ii] +
                               Ms_[kk + 1][ii] * qr_capital_lambdas_[kk].solve(
                                                     intermediate_terms_[kk]));
    }
  }

  // (2) Now compute optimal state and control trajectory forward in time.
  VectorXf x_star = x0;
  VectorXf last_x_star;
  for (size_t kk = 0; kk < num_time_steps_ - 1; kk++) {
    // Maybe set delta_x.
    if (delta_xs) (*delta_xs)[kk] = x_star;

    // Unpack linearization at this time step.
    const auto& lin = linearization[kk];

    // Compute optimal x.
    last_x_star = x_star;
    x_star = qr_capital_lambdas_[kk].solve(lin.A * last_x_star +
                                           intermediate_terms_[kk]);

    // Compute optimal u and store (sign flipped) in alpha.
    // Also maybe compute costates.
    for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++) {
      const VectorXf intermediate_term =
          Ms_[kk + 1][ii] * x_star + ms_[kk + 1][ii];
      strategies[ii].alphas[kk] =
          warped_Bs_[kk][ii] * intermediate_term + warped_rs_[kk][ii];

      if (costates) (*costates)[kk][ii] = lin.A.transpose() * intermediate_term;
    }

    // Check dynamic feasibility.
    // VectorXf check_x = lin.A * last_x_star;
    // for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++)
    //   check_x -= lin.Bs[ii] * strategies[ii].alphas[kk];

    // CHECK_LE((x_star - check_x).cwiseAbs().maxCoeff(), 1e-1);
  }

  // Set delta_x and costate for last time step.
  if (delta_xs) {
    delta_xs->back() = x_star;
    for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++)
      costates->back()[ii].setZero();
  }

  return strategies;
}

}  // namespace ilqgames


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Value0SubgraphTest

// tests of value ordering of the matches

#include "blip.hpp"
#include <algorithm>
#include <boost/test/unit_test.hpp>
#include <vector>
#include <popart/logging.hpp>

using namespace fwtools::subgraph;
using namespace blip;

BOOST_AUTO_TEST_CASE(Value0_Subgraph) {

  auto test = [](const std::vector<blip::Type> &types,
                 const std::map<blip::Type, blip::Value> &value_map,
                 std::vector<Match> expected_matches) {
    // construct a schedule from the input values to this test
    std::vector<const Blip *> sched;
    std::vector<std::unique_ptr<Blip>> blips;
    for (auto &t : types) {
      blips.emplace_back(
          std::unique_ptr<Blip>(new Blip(t, value_map.at(t), {})));
    }
    for (int i = 0; i < blips.size(); ++i) {
      sched.push_back(blips[i].get());
    }

    algo0::RinseMatcherAlgo0<const Blip> rinseMatcher(sched);
    auto matches = rinseMatcher.getRepeatedSequences();

    setValues(matches, sched);
    std::sort(matches.rbegin(), matches.rend());

    std::stringstream ss;
    ss << "\nExpected matches:";
    for (auto &x : expected_matches) {
      ss << "\n" << x;
    }
    ss << "\nComputed matches:";
    for (auto &x : matches) {
      ss << "\n" << x;
    }
    popart::logging::debug(ss.str());
    BOOST_CHECK(matches == expected_matches);
  };

  // -------------------------------------------
  popart::logging::info("priorities, simple case");
  //                            0  1  2  3  4  5
  std::vector<blip::Type> types{0, 1, 2, 0, 1, 2};

  std::map<blip::Type, blip::Value> value_map{
      {0, 5.0f}, {1, 11.0f}, {2, 13.0f}};

  std::vector<Match> expected_matches = {
      {{0, 3}, 3}, // 012 : 29
      {{1, 4}, 2}, // 12  : 24
      {{0, 3}, 2}, // 01  : 16
      {{2, 5}, 1}, // 2   : 13
      {{1, 4}, 1}, // 1   : 11
      {{0, 3}, 1}  // 0   : 5
  };
  test(types, value_map, expected_matches);

  // -------------------------------------------
  popart::logging::info("priorities, length rules");
  //       0  1  2  3  4  5  6  7  8  9
  types = {0, 1, 2, 3, 0, 1, 0, 1, 2, 3};

  value_map = {{0, 9.0f}, {1, 1.0f}, {2, 4.0f}, {3, 6.0f}};

  expected_matches = {
      {{0, 6}, 4},    // 0123 : 20.0
      {{0, 6}, 3},    // 012  : 14.0
      {{1, 7}, 3},    // 123  : 11.0
      {{0, 4, 6}, 2}, // 01   : 10.0  as there are 3 sequences here.
      {{2, 8}, 2},    // 23   : 10.0  and only 2 here, 3 takes precedence.
      {{0, 4, 6}, 1}, // 0    : 9.0
      {{3, 9}, 1},    // 3    : 6.0
      {{1, 7}, 2},    // 12   : 5.0
      {{2, 8}, 1},    // 2    : 4.0
      {{1, 5, 7}, 1}  // 1    : 1.0
  };
  test(types, value_map, expected_matches);
}


/**
 * @file      SIMesh.cpp
 *
 * @brief     This file contains the implementation of the SIMesh file writer.
 *
 * @author    Stefan Reinhold
 * @copyright Copyright (C) 2018 Stefan Reinhold  -- All Rights Reserved.
 *            You may use, distribute and modify this code under the terms of
 *            the AFL 3.0 license; see LICENSE for full license details.
 */

#include "SIMesh.h"
#include "Mesh.h"
#include "MeshHelpers.h"
#include "Optional.h"

#include <Eigen/Core>
#include <igl/adjacency_list.h>
#include <igl/edges.h>

#include <fstream>
#include <sstream>

namespace CortidQCT {
namespace Internal {

namespace {

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wpadded"
template <class T> struct Vertex {
  Eigen::Matrix<T, 3, 1> position;
  std::vector<typename Mesh<T>::Index> adjacentVertexIndices;
  std::vector<typename Mesh<T>::Index> incidentEdgeIndices;
  std::optional<typename Mesh<T>::Label> label;
};
#pragma clang diagnostic pop

template <class T> struct Edge {
  Eigen::Matrix<T, 2, 1> vertexIndices;
  Eigen::Matrix<T, 2, 1> vertexNeighboursIndices;
};

template <class T> struct Facet {
  Eigen::Matrix<T, 3, 1> vertexIndices;
  Eigen::Matrix<T, 3, 1> edgeIndices;
};

template <class T>
std::ostream &operator<<(std::ostream &out, Vertex<T> const &v) {
  out << v.position.transpose() << " " << v.adjacentVertexIndices.size() << " ";
  for (auto n : v.adjacentVertexIndices) out << n << " ";
  for (auto e : v.incidentEdgeIndices) out << e << " ";
  if (v.label) {
    out << *v.label;
  } else {
    out.seekp(-1, out.cur);
  }
  return out;
}

template <class T>
std::ostream &operator<<(std::ostream &out, Edge<T> const &e) {
  return out << e.vertexIndices.transpose() << " "
             << e.vertexNeighboursIndices.transpose();
}

template <class T>
std::ostream &operator<<(std::ostream &out, Facet<T> const &f) {
  return out << f.vertexIndices.transpose() << " " << f.edgeIndices.transpose();
}

} // namespace

template <class T>
void writeToSIMesh(Mesh<T> const &mesh, std::string const &filename,
                   bool includeLabels) {
  using namespace Eigen;
  using namespace gsl;
  using std::size_t;

  VertexMatrix<T> const V = vertexMatrix(mesh);
  FacetMatrix const F = facetMatrix(mesh);
  LabelVector const L = labelVector(mesh);

  std::vector<std::vector<typename Mesh<T>::Index>> vertexAdjacencyList;
  igl::adjacency_list(F, vertexAdjacencyList, true);

  Matrix<Index, Dynamic, 2> E;
  igl::edges(F, E);

  using Idx = typename Mesh<T>::Index;
  std::vector<Vertex<T>> vertices(mesh.vertexCount());
  std::vector<Edge<Idx>> edges(narrow_cast<size_t>(E.rows()));
  std::vector<Facet<Idx>> facets(mesh.triangleCount());

  typename Mesh<T>::Index vIdx{0};
  for (auto &&adjacentVertices : vertexAdjacencyList) {
    auto &vi = vertices[narrow_cast<size_t>(vIdx)];
    vi.position = V.row(narrow_cast<Index>(vIdx));
    vi.adjacentVertexIndices = adjacentVertices;

    if (includeLabels) { vi.label = L(vIdx); }

    for (auto j = 0; j < E.rows(); ++j) {
      if (E(j, 0) == vIdx || E(j, 1) == vIdx) {
        vi.incidentEdgeIndices.push_back(narrow_cast<Idx>(j));
      }
    }
    ++vIdx;
  }

  // Populate edge array
  for (auto i = 0; i < E.rows(); ++i) {
    auto &ei = edges[narrow_cast<size_t>(i)];
    ei.vertexIndices[0] = E(i, 0);
    ei.vertexIndices[1] = E(i, 1);

    auto j = 0;
    for (auto u :
         vertices[narrow_cast<size_t>(E(i, 0))].adjacentVertexIndices) {
      if (u == E(i, 1)) break;
      ++j;
    }
    ei.vertexNeighboursIndices[0] = j;

    j = 0;
    for (auto u :
         vertices[narrow_cast<size_t>(E(i, 1))].adjacentVertexIndices) {
      if (u == E(i, 0)) break;
      ++j;
    }
    ei.vertexNeighboursIndices[1] = j;
  }

  // Populate facet array
  for (auto i = 0; i < F.rows(); ++i) {
    auto &fi = facets[narrow_cast<size_t>(i)];

    fi.vertexIndices = F.row(i).transpose();

    for (auto k = 0; k < 3; ++k) {
      auto const vi = F(i, k);
      auto const vj = F(i, (k + 1) % 3);

      for (auto j = 0; j < E.rows(); ++j) {
        if ((E(j, 0) == vi && E(j, 1) == vj) ||
            (E(j, 0) == vj && E(j, 1) == vi)) {
          fi.edgeIndices[k] = j;
          break;
        }
      }
    }
  }

  // Output
  std::ofstream out(filename);
  out << mesh.vertexCount() << " " << mesh.triangleCount() << " "
      << edges.size() << std::endl;
  out << "Vertices" << std::endl;
  for (auto const &v : vertices) out << v << std::endl;
  out << "Edges" << std::endl;
  for (auto const &e : edges) out << e << std::endl;
  out << "Facets" << std::endl;
  for (auto const &f : facets) out << f << std::endl;
}

template <class T>
Mesh<T> readFromSIMesh(std::string const &filename, bool withLabels) {
  using namespace Eigen;
  using namespace gsl;

  std::ifstream input{filename};
  std::string line;

  // Skip to first line
  while (std::getline(input, line)) {
    if (line != "") break;
  }

  std::size_t nVertices, nTriangles, nEdges;
  std::istringstream{line} >> nVertices >> nTriangles >> nEdges;

  // Skip to "Vertices block
  while (std::getline(input, line)) {
    if (line == "Vertices") break;
  }

  VertexMatrix<T> vertices(narrow_cast<Index>(nVertices), 3);
  FacetMatrix facets(narrow_cast<Index>(nTriangles), 3);
  LabelVector labels(narrow_cast<Index>(nVertices));
  // Read vertices
  for (auto i = 0; i < vertices.rows(); ++i) {
    // skip empty lines
    while (std::getline(input, line)) {
      if (line != "") break;
    }
    std::istringstream is{line};
    is >> vertices(i, 0) >> vertices(i, 1) >> vertices(i, 2);
    int degree;
    is >> degree;
    int dummy;
    // skip 2 * degree entries
    for (auto j = 0; j < 2 * degree; ++j) is >> dummy;
    if (!is.eof()) { is >> labels(i); }
  }

  // Skip to "Facets block
  while (std::getline(input, line)) {
    if (line == "Facets") break;
  }

  for (auto i = 0; i < facets.rows(); ++i) {
    // skip empty lines
    while (std::getline(input, line)) {
      if (line != "") break;
    }
    std::istringstream is{line};
    is >> facets(i, 0) >> facets(i, 1) >> facets(i, 2);
  }

  auto mesh = Mesh<T>(nVertices, nTriangles);

  mesh.withUnsafeVertexPointer([&vertices](auto *ptr) {
    Map<Matrix<T, 3, Dynamic>>{ptr, 3, vertices.rows()} = vertices.transpose();
  });

  mesh.withUnsafeIndexPointer([&facets](auto *ptr) {
    Map<Matrix<typename Mesh<T>::Index, 3, Dynamic>>{ptr, 3, facets.rows()} =
        facets.transpose();
  });

  if (withLabels) {
    mesh.withUnsafeLabelPointer([&labels](auto *ptr) {
      Map<Matrix<typename Mesh<T>::Label, Dynamic, 1>>{ptr, labels.rows()} =
          labels;
    });
  }

  return mesh;
}

template void writeToSIMesh<float>(Mesh<float> const &, std::string const &,
                                   bool);
template void writeToSIMesh<double>(Mesh<double> const &, std::string const &,
                                    bool);

template Mesh<float> readFromSIMesh<float>(std::string const &, bool);
template Mesh<double> readFromSIMesh<double>(std::string const &, bool);

} // namespace Internal
} // namespace CortidQCT


#include <boost/algorithm/string.hpp>
#include <sstream>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/sensors/SensorFactory.hh>
#include <gazebo/sensors/SensorManager.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/Node.hh>
#include <gazebo/transport/Publisher.hh>

#include "gazebo_rfid/RFIDTransceiver.h"

using namespace gazebo;
using namespace sensors;

/////////////////////////////////////////////////
// constants definiton

// Frequency Quantization in M6e Hardware Guide
const double RFIDTransceiver::STEP_FREQ = 25e3;   // Hertzs
const double RFIDTransceiver::MIN_FREQ_A = 865e6; // Hertzs
const double RFIDTransceiver::MAX_FREQ_A = 869e6; // Hertzs
const int RFIDTransceiver::N_CHAN_A = 160; //
const double RFIDTransceiver::MIN_FREQ_B = 902e6; // Hertzs
const double RFIDTransceiver::MAX_FREQ_B = 928e6; // Hertzs
const int RFIDTransceiver::N_CHAN_B = 1040; //

// EU region
const double RFIDTransceiver::MIN_FREQ_EU = 865.6e6; // Hertzs
const double RFIDTransceiver::MAX_FREQ_EU = 867.6e6; // Hertzs
const double RFIDTransceiver::STEP_FREQ_EU = 100e3;  // Hertzs
const int RFIDTransceiver::N_CHAN_EU = 20; //

// NA region
const double RFIDTransceiver::MIN_FREQ_NA = 902e6;  // Hertzs
const double RFIDTransceiver::MAX_FREQ_NA = 928e6;  // Hertzs
const double RFIDTransceiver::STEP_FREQ_NA = 250e3; // Hertzs
const int RFIDTransceiver::N_CHAN_NA = 104; //

// This comes from the manufacturer. Azimut axis only ...
// gain list entries start at -180 degrees to 180 in steps of 15.
// const double RFIDTransceiver::ANTENNA_LOSSES_LIST[25] = {
//     -22.6, -25.2, -25,   -20.2, -17.6, -15.6, -14,  -11.2, -7.8,
//     -5.2,  -2.4,  -0.6,  0.0,     -0.6,  -2.4,  -5.2, -8.8,  -12.2,
//     -16.4, -19.2, -20.8, -24.4, -28.2, -24,   -22.6};
const double RFIDTransceiver::ANTENNA_LOSSES_LIST[25] = {
    -22.6, -25.2, -25,   -20.2, -17.6, -15.6, -14,  -11.2, -7.8,
    -5.2,  -2.4,  -0.6,  0.0,     -0.6,  -2.4,  -5.2, -7.8,  -11.2,
    -14, -15.6, -17.6, -20.2, -25, -25.2,   -22.6};
// const double RFIDTransceiver::ANTENNA_LOSSES_LIST[25] = {
//     -66.8, -75.6, -75,   -60.6, -52.8, -46.8, -14,  -11.2, -7.8,
//     -5.2,  -2.4,  -0.6,  0,     -0.6,  -2.4,  -5.2, -8.8,  -12.2,
//     -16.4, -57.6, -62.4, -73.2, -84.6, -72,   -67.8};
const double RFIDTransceiver::ANTENNA_ANGLES_LIST[25] = {
    -180.0, -165.0, -150.0, -135.0, -120.0, -105.0, -90.0, -75.0, -60.0,
    -45.0,  -30.0,  -15.0,  0.0,    15.0,   30.0,   45.0,  60.0,  75.0,
    90.0,   105.0,  120.0,  135.0,  150.0,  165.0,  180.0};

// We mostly use UPM frog 3D tag, which is more or less isotropic.
const double RFIDTransceiver::TAG_LOSSES = -4.8;

// Propagation loss constant = 20 * log10 ( c / (4*pi) )
const double RFIDTransceiver::LOSS_CONSTANT = 147.55;

// Constant to obtain propagation delay = 4*pi/c
const double RFIDTransceiver::PHASE_CONSTANT = 4.192e-8;
const double RFIDTransceiver::STD_DEV_DB_NOISE = 3.92; // tipical office: https://www.gaussianwaves.com/2013/09/log-distance-path-loss-or-log-normal-shadowing-model/
const double RFIDTransceiver::STD_DEV_PH_NOISE = 0.01;

// static const std::string fieldSep_ = (":");
// static const std::string entrySep_ = ("\n");

/////////////////////////////////////////////////
RFIDTransceiver::RFIDTransceiver() : Sensor(sensors::OTHER) {
  this->active = false;
}

/////////////////////////////////////////////////
RFIDTransceiver::~RFIDTransceiver() {
}

//////////////////////////////////////////////////
std::string RFIDTransceiver::Topic() const {
  std::string topicName = "~/";
  topicName += this->ParentName() + "/" + this->Name() + "/transceiver";
  boost::replace_all(topicName, "::", "/");

  return topicName;
}

//////////////////////////////////////////////////
void RFIDTransceiver::Load(const std::string &_worldName){
  Sensor::Load(_worldName);

  this->parentEntity = boost::dynamic_pointer_cast<physics::Link>( this->world->EntityByName(this->ParentName()));

  GZ_ASSERT(this->parentEntity.lock() != nullptr, "parentEntity is null");

  this->referencePose = this->pose + this->parentEntity.lock()->WorldPose();

  if (!this->sdf->HasElement("transceiver"))  {
    gzthrow("Transceiver sensor is missing <transceiver> SDF element");
  }

}

//////////////////////////////////////////////////
void RFIDTransceiver::Init(){
  Sensor::Init();
}

/////////////////////////////////////////////////
void RFIDTransceiver::Fini(){
  this->pub.reset();
  this->parentEntity.lock().reset();
  Sensor::Fini();
}


/* Copyright (c) 2021, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
 * platform" software 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 <ff_common/init.h>
#include <ff_common/thread.h>
#include <ff_common/utils.h>
#include <config_reader/config_reader.h>
#include <camera/camera_params.h>
#include <camera/camera_model.h>
#include <dense_map_utils.h>
#include <texture_processing.h>

#include <boost/filesystem.hpp>

#include <glog/logging.h>
#include <opencv2/imgcodecs.hpp>

#include <cmath>

// This code used to project a camera image onto a plane most aligned
// to the mesh portion seen in the current camera view. It is no
// longer needed, it is broken, but may be of use one day.

#if 0

DEFINE_string(image, "",
              "The image to orthoproject. For each image named image.jpg, "
              "an input file image_world2cam.txt is expected, and the output "
              "orthoimage will be written to image_ortho.png");

DEFINE_string(image_list, "",
              "Instead of a single image, specify the list of images to orthoproject, "
              "with the same convention as above. ");

DEFINE_double(min_ray_length, 0.0,
              "The shortest distance from the camera to the mesh.");

DEFINE_double(max_ray_length, 10.0,
              "The longest distance from the camera to the mesh.");

DEFINE_double(pixel_size, 0.01,
              "The output orthoimage pixel size, in meters.");

DEFINE_bool(snap_plane, false,
            "Adjust the plane to project onto to make "
            "angles multiple of 45 degrees with the coordinate axes.");

DEFINE_string(plane_normal, "",
              "Override the auto-computed the normal of the plane to "
              "project onto with this. Specify as three values in quotes.");

// Shoot a set of rays from the camera onto the mesh and see
// where they intersect
void sample_mesh(cv::Mat const& image,
                 camera::CameraModel const& cam,
                 std::shared_ptr<BVHTree> const& bvh_tree,
                 std::vector<Eigen::Vector3d> & intersections) {
  // Wipe the output
  intersections.clear();

  Eigen::Vector3d cam_ctr = -cam.GetTransform().rotation().transpose() *
    cam.GetTransform().translation();

  // Sample the image uniformly
  int num_samples = 100;
  Eigen::Matrix3d cam_to_world_rot = cam.GetTransform().rotation().transpose();
#pragma omp parallel for
  for (int itx = 0; itx < num_samples; itx++) {
    for (int ity = 0; ity < num_samples; ity++) {
      double x = (image.cols - 1.0) * static_cast<double>(itx) / (num_samples - 1.0);
      double y = (image.rows - 1.0) * static_cast<double>(ity) / (num_samples - 1.0);

      Eigen::Vector2d dist_pix;
      dist_pix << x, y;

      Eigen::Vector2d undist_centered_pix;
      cam.GetParameters().Convert<camera::DISTORTED, camera::UNDISTORTED_C>
        (dist_pix, &undist_centered_pix);

      // Ray from camera going through the pixel
      Eigen::Vector3d cam_ray(undist_centered_pix.x() / cam.GetParameters().GetFocalVector()[0],
                              undist_centered_pix.y() / cam.GetParameters().GetFocalVector()[1],
                              1.0);
      cam_ray.normalize();

      Eigen::Vector3d world_ray = cam_to_world_rot * cam_ray;

      // Set up the ray structure for the mesh
      BVHTree::Ray bvh_ray;
      bvh_ray.origin = dense_map::eigen_to_vec3f(cam_ctr);
      bvh_ray.dir = dense_map::eigen_to_vec3f(world_ray);
      bvh_ray.dir.normalize();
      bvh_ray.tmin = FLAGS_min_ray_length;
      bvh_ray.tmax = FLAGS_max_ray_length;

      // Intersect the ray with the mesh
      BVHTree::Hit hit;
      if (!bvh_tree->intersect(bvh_ray, &hit))
        continue;
      double cam_to_mesh_dist = hit.t;

      Eigen::Vector3d intersection = cam_ctr + cam_to_mesh_dist * world_ray;

#pragma omp critical
      {
        intersections.push_back(intersection);
      }
    }
  }

  if (intersections.empty())
    LOG(FATAL) << "Could not project the image onto the mesh.\n";
}

// Create a new coordinate system having the desired plane as its x-y
// plane, and the normal to the plane as its z axis.
void setup_plane_coord_system(camera::CameraModel const& cam,
                              Eigen::Vector3d & plane_normal,
                              Eigen::Matrix3d & world2planecoord) {
  // The camera view (the ray starting at the camera center and going through
  // the image center).
  Eigen::Matrix3d cam_to_world_rot = cam.GetTransform().rotation().transpose();
  Eigen::Vector3d cam_dir = cam_to_world_rot * Eigen::Vector3d(0, 0, 1);

  // Ensure that the normal to the plane agrees with the camera direction
  double dot_prod = cam_dir.dot(plane_normal);
  if (dot_prod < 0)
    plane_normal *= -1.0;

  // The "up" direction
  Eigen::Vector3d z(0, 0, 1);

  Eigen::Vector3d lz = plane_normal.normalized();
  // The new y points "down", as suggested by the z axis, while still
  // being in the plane
  Eigen::Vector3d ly = -1.0 * (z - z.dot(plane_normal) * plane_normal);
  // Treat the special case when the plane is perfectly horizontal,
  // then the normal can't point down. Normalize.
  if (ly.norm() == 0)
    ly = Eigen::Vector3d(0, 1, 0);
  ly.normalize();
  // The new x is then the remaining direction so that we have a
  // right-handed coordinate system.
  Eigen::Vector3d lx = ly.cross(lz);
  if (lx.norm() == 0) {
    lx = Eigen::Vector3d(1, 0, 0);  // should never happen
    ly = lz.cross(lx);
  }
  lx.normalize();

  // The matrix to transform to the coordinate system having the
  // desired plane to project onto as its x-y plane.
  Eigen::Matrix3d planecoord2world(3, 3);
  planecoord2world.col(0) = lx;
  planecoord2world.col(1) = ly;
  planecoord2world.col(2) = lz;

  world2planecoord = planecoord2world.transpose();
}

void orthoproject(cv::Mat const& image,
                  Eigen::Matrix3d const& world2planecoord,
                  Eigen::Vector3d const& plane_normal,
                  camera::CameraModel const& cam,
                  std::shared_ptr<BVHTree> const& bvh_tree,
                  std::vector<Eigen::Vector3d> const& intersections,
                  double pixel_size,
                  cv::Mat & orthoimage) {
  // Find the bounding box of the mesh points seen by the camera
  // in the coordinate system of the desired plane

  double min_x = std::numeric_limits<double>::max(), max_x = -min_x;
  double min_y = min_x, max_y = max_x;
  double min_z = min_x, max_z = max_x;
  for (size_t it = 0; it < intersections.size(); it++) {
    Eigen::Vector3d local_pt = world2planecoord * intersections[it];
    if (local_pt.x() < min_x) min_x = local_pt.x();
    if (local_pt.x() > max_x) max_x = local_pt.x();
    if (local_pt.y() < min_y) min_y = local_pt.y();
    if (local_pt.y() > max_y) max_y = local_pt.y();
    if (local_pt.z() < min_z) min_z = local_pt.z();
    if (local_pt.z() > max_z) max_z = local_pt.z();
  }

  if (min_x > max_x || min_y > max_y || min_z > max_z)
    LOG(FATAL) << "The rays don't intersect the mesh.";

  // The output image dimensions
  int num_cols = ceil((max_x - min_x)/pixel_size);
  int num_rows = ceil((max_y - min_y)/pixel_size);

  orthoimage = cv::Mat(num_rows, num_cols, CV_8UC1, cv::Scalar(0));

#pragma omp parallel for
  for (int col = 0; col < num_cols; col++) {
    for (int row = 0; row < num_rows; row++) {
      // Create a point in the orthoimage plane corresponding to a future pixel
      double z = min_z;
      double y = min_y + row * pixel_size + pixel_size/2.0;
      double x = min_x + col * pixel_size + pixel_size/2.0;
      Eigen::Vector3d world_pt = world2planecoord.transpose() * Eigen::Vector3d(x, y, z);

      // Shoot a ray from that point to the mesh
      BVHTree::Ray bvh_ray;
      bvh_ray.dir    = dense_map::eigen_to_vec3f(plane_normal);
      bvh_ray.origin = dense_map::eigen_to_vec3f(world_pt);
      bvh_ray.tmin   = 0;
      bvh_ray.tmax   = max_z - min_z;

      // Intersect the ray with the mesh
      BVHTree::Hit hit;
      if (!bvh_tree->intersect(bvh_ray, &hit))
        continue;

      double plane_to_mesh_dist = hit.t;
      Eigen::Vector3d intersection = world_pt + plane_to_mesh_dist * plane_normal;
      Eigen::Vector3d cam_pt = cam.GetTransform() * intersection;

      // Skip points that project behind the camera
      if (cam_pt.z() <= 0)
        continue;

      // Get the distorted pixel value
      Eigen::Vector2d undist_centered_pix =
        cam.GetParameters().GetFocalVector().cwiseProduct(cam_pt.hnormalized());
      Eigen::Vector2d dist_pix;
      cam.GetParameters().Convert<camera::UNDISTORTED_C, camera::DISTORTED>
        (undist_centered_pix, &dist_pix);

      // Skip pixels that don't project in the image
      if (dist_pix.x() < 0 || dist_pix.x() > image.cols - 1 ||
          dist_pix.y() < 0 || dist_pix.y() > image.rows - 1)
        continue;

      // Find the pixel value
      cv::Size s(1, 1);
      cv::Mat interp_pix_val;
      cv::Point2f P;
      P.x = dist_pix[0];
      P.y = dist_pix[1];

      // Do bilinear interpolation into the image
      cv::getRectSubPix(image, s, P, interp_pix_val);
      uchar color = interp_pix_val.at<uchar>(0, 0);

      orthoimage.at<uchar>(row, col) = color;
    }
  }
}

void run_orthoproject(bool snap_plane,
                      std::string const& image_file,
                      double pixel_size,
                      std::string const& plane_normal_str,
                      std::shared_ptr<BVHTree> const& bvh_tree,
                      camera::CameraModel & cam) {
  // Manufacture the world_to_cam_file and output orthoimage_file
  std::string world_to_cam_file, orthoimage_file;

  world_to_cam_file = ff_common::ReplaceInStr(image_file,
                                           ".jpg", "_world2cam.txt");
  orthoimage_file = ff_common::ReplaceInStr(image_file, ".jpg", "_ortho.png");

  if (world_to_cam_file == image_file || orthoimage_file == image_file)
    LOG(FATAL) << "Expecting a .jpg image for file: " << image_file;

  Eigen::Affine3d world_to_cam;
  if (!dense_map::readAffine(world_to_cam, world_to_cam_file))
    LOG(FATAL) << "Could not read a 4x4 matrix from: " << world_to_cam_file << "\n";
  cam.SetTransform(world_to_cam);

  cv::Mat image = cv::imread(image_file, CV_LOAD_IMAGE_GRAYSCALE);
  if (image.rows == 0 || image.cols == 0)
    LOG(FATAL) << "Could not read image from: " << image_file;

  // Shoot a set of rays from the camera onto the mesh and see
  // where they intersect
  std::vector<Eigen::Vector3d> intersections;
  sample_mesh(image, cam, bvh_tree, intersections);

  Eigen::Vector3d plane_normal;
  if (plane_normal_str != "") {
    // The user wants to override the plane normal
    std::istringstream is(plane_normal_str);
    if (!(is >> plane_normal[0] >> plane_normal[1] >> plane_normal[2]))
      LOG(FATAL) << "Could not parse three values (the plane normal) from: " << plane_normal_str;
    std::cout << "Using user-defined normal to the plane: "
              << plane_normal.transpose() << std::endl;
  } else {
    // Find the best fit plane for the projections of the pixels onto the walls
    Eigen::Vector3d centroid;
    dense_map::bestFitPlane(intersections, centroid, plane_normal);

    std::cout << "Normal to the plane: " << plane_normal.transpose() << std::endl;

    // Make the plane have angles of 45 degrees with the coordinate axes
    if (snap_plane) {
      dense_map::snapPlaneNormal(plane_normal);
      std::cout << "Snapped normal to the plane: " << plane_normal.transpose() << std::endl;
    }
  }

  plane_normal.normalize();

  // Create a new coordinate system having the desired plane as its x-y
  // plane, and the normal to the plane as its z axis.
  Eigen::Matrix3d world2planecoord;
  setup_plane_coord_system(cam, plane_normal, world2planecoord);

  cv::Mat orthoimage;
  orthoproject(image, world2planecoord, plane_normal, cam,
               bvh_tree, intersections, pixel_size,
               orthoimage);

  std::cout << "Writing: " << orthoimage_file << std::endl;
  cv::imwrite(orthoimage_file, orthoimage);
}

int main(int argc, char** argv) {
  ff_common::InitFreeFlyerApplication(&argc, &argv);

  if (FLAGS_mesh.empty())
    LOG(FATAL) << "The mesh was not specified.";

  // Load the mesh
  mve::TriangleMesh::Ptr mesh;
  std::shared_ptr<mve::MeshInfo> mesh_info;
  std::shared_ptr<tex::Graph> graph;  // TODO(oalexan1): Is this necessary?
  std::shared_ptr<BVHTree> bvh_tree;
  dense_map::loadMeshBuildTree(FLAGS_mesh, mesh, mesh_info, graph, bvh_tree);

  // Load the camera intrinsics
  config_reader::ConfigReader config;
  config.AddFile("cameras.config");
  if (!config.ReadFiles())
    LOG(FATAL) << "Could not read the configuration file.";
  camera::CameraParameters cam_params(&config, "nav_cam");
  camera::CameraModel cam(Eigen::Vector3d(), Eigen::Matrix3d::Identity(),
                          cam_params);

  // Run either a single image or a list of images
  std::vector<std::string> image_list;
  if (!FLAGS_image.empty()) {
    image_list.push_back(FLAGS_image);
  } else if (!FLAGS_image_list.empty()) {
    std::ifstream ifs(FLAGS_image_list.c_str());
    std::string image_file;
    while (ifs >> image_file)
      image_list.push_back(image_file);
  } else {
    LOG(FATAL) << "Must specify either an image to orthoproject or an image list.";
  }

  for (size_t it = 0; it < image_list.size(); it++)
    run_orthoproject(FLAGS_snap_plane,
                     image_list[it],
                     FLAGS_pixel_size,
                     FLAGS_plane_normal,
                     bvh_tree, cam);

  return 0;
}
#endif

int main(int argc, char** argv) {
  std::cout << "This code is broken and commented out." << std::endl;
}