// Copyright 2013 Daniel Parker
// Distributed under Boost license

#ifdef __linux__
#define BOOST_TEST_DYN_LINK
#endif

#include <boost/test/unit_test.hpp>
#include <sstream>
#include <vector>
#include <utility>
#include <ctime>
#include <new>
#include "jsoncons/json.hpp"
#include "jsoncons_ext/jsonpath/json_query.hpp"

using namespace jsoncons;
using namespace jsoncons::jsonpath;

BOOST_AUTO_TEST_SUITE(jsonpath_filter_test_suite)

struct jsonpath_filter_fixture
{
    static const char* store_text()
    {
        static const char* text = "{ \"store\": {\"book\": [ { \"category\": \"reference\",\"author\": \"Nigel Rees\",\"title\": \"Sayings of the Century\",\"price\": 8.95},{ \"category\": \"fiction\",\"author\": \"Evelyn Waugh\",\"title\": \"Sword of Honour\",\"price\": 12.99},{ \"category\": \"fiction\",\"author\": \"Herman Melville\",\"title\": \"Moby Dick\",\"isbn\": \"0-553-21311-3\",\"price\": 8.99},{ \"category\": \"fiction\",\"author\": \"J. R. R. Tolkien\",\"title\": \"The Lord of the Rings\",\"isbn\": \"0-395-19395-8\",\"price\": 22.99}],\"bicycle\": {\"color\": \"red\",\"price\": 19.95}}}";
        return text;
    }
    static const char* book_text()
    {
        static const char* text = "{ \"category\": \"reference\",\"author\": \"Nigel Rees\",\"title\": \"Sayings of the Century\",\"price\": 8.95}";
        return text;
    }

    json book()
    {
        json root = json::parse(jsonpath_filter_fixture::store_text());
        json book = root["store"]["book"];
        return book;
    }

    json bicycle()
    {
        json root = json::parse(jsonpath_filter_fixture::store_text());
        json bicycle = root["store"]["bicycle"];
        return bicycle;
    }
};

BOOST_AUTO_TEST_CASE(test_jsonpath_filter)
{
    std::string expr1 = "(1 + 1)";
    size_t line = 1;
    size_t column = 1;
    const char* pend;
    jsonpath_filter_parser<json> parser(&pend,&line,&column);
    json parent = json::array();
    parent.add(1);
    parent.add(2);

    parser.parse(expr1.c_str(), expr1.c_str()+ expr1.length());
    auto result1 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(2),result1);

    std::string expr2 = "(1 - 1)";
    parser.parse(expr2.c_str(), expr2.c_str()+ expr2.length());
    auto result2 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(0), result2);

    std::string expr3 = "(@.length - 1)";
    parser.parse(expr3.c_str(), expr3.c_str()+ expr3.length());
    auto result3 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(1), result3);

}

BOOST_AUTO_TEST_CASE(test_jsonpath_filter_exclaim)
{
    size_t line = 1;
    size_t column = 1;
    const char* pend;
    jsonpath_filter_parser<json> parser(&pend,&line,&column);
    json parent = json::array();
    parent.add(1);
    parent.add(2);

    std::string expr1 = "(!(1 + 1))";
    parser.parse(expr1.c_str(), expr1.c_str()+ expr1.length());
    auto result1 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(false),result1);

    std::string expr2 = "(!0)";
    parser.parse(expr2.c_str(), expr2.c_str()+ expr2.length());
    auto result2= parser.eval(parent);
    BOOST_CHECK_EQUAL(json(true),result2);
}

BOOST_AUTO_TEST_CASE(test_jsonpath_index_expression)
{
    json root = json::parse(jsonpath_filter_fixture::store_text());

    json result = json_query(root,"$..book[(@.length-1)]");

    BOOST_CHECK_EQUAL(1,result.size());
    BOOST_CHECK_EQUAL(root["store"]["book"][3],result[0]);

    //    std::cout << pretty_print(result) << std::endl;
}

BOOST_AUTO_TEST_CASE(test_jsonpath_filter_negative_numbers)
{
    size_t line = 1;
    size_t column = 1;
    const char* pend;
    jsonpath_filter_parser<json> parser(&pend,&line,&column);
    json parent = json::array();
    parent.add(1);
    parent.add(2);

    std::string expr1 = "(-1 + 1)";
    parser.parse(expr1.c_str(), expr1.c_str()+ expr1.length());
    auto result1 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(0),result1);

    std::string expr2 = "(1 + -1)";
    parser.parse(expr2.c_str(), expr2.c_str()+ expr2.length());
    auto result2 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(0), result2);

    std::string expr3 = "(-1 - -1)";
    parser.parse(expr3.c_str(), expr3.c_str()+ expr3.length());
    auto result3 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(0), result3);

    std::string expr4 = "(-1 - -3)";
    parser.parse(expr4.c_str(), expr4.c_str()+ expr4.length());
    auto result4 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(2), result4);

    std::string expr5 = "((-2 < -1) && (-3 > -4))";
    parser.parse(expr5.c_str(), expr5.c_str()+ expr5.length());
    auto result5 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(true), result5);

    std::string expr6 = "((-2 < -1) || (-4 > -3))";
    parser.parse(expr6.c_str(), expr6.c_str()+ expr6.length());
    auto result6 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(true), result6);
}

BOOST_AUTO_TEST_CASE(test_jsonpath_filter_uni)
{
    size_t line = 1;
    size_t column = 1;
    const char* pend;
    jsonpath_filter_parser<json> parser(&pend,&line,&column);
    json parent = json::array();
    parent.add(1);
    parent.add(2);

    std::string expr1 = "(0)";
    parser.parse(expr1.c_str(), expr1.c_str()+ expr1.length());
    auto result1 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(0),result1);
}

#if defined(__GNUC__) && (__GNUC__ == 4 && __GNUC_MINOR__ < 9)
// GCC 4.8 has broken regex support: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=53631
BOOST_AUTO_TEST_CASE_EXPECTED_FAILURES(test_jsonpath_filter_regex, 2)
#endif
BOOST_AUTO_TEST_CASE(test_jsonpath_filter_regex)
{
    size_t line = 1;
    size_t column = 1;
    const char* pend;
    jsonpath_filter_parser<json> parser(&pend,&line,&column);
    json parent = json::array();
    parent.add(1);
    parent.add(2);

    std::string expr1 = "('today I go' =~ /today.*?/)";
    parser.parse(expr1.c_str(), expr1.c_str()+ expr1.length());
    auto result1 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(true),result1);

    std::string expr2 = "('today I go' =~ /Today.*?/)";
    parser.parse(expr2.c_str(), expr2.c_str()+ expr2.length());
    auto result2 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(false),result2);

    std::string expr3 = "('today I go' =~ /Today.*?/i)";
    parser.parse(expr3.c_str(), expr3.c_str()+ expr3.length());
    auto result3 = parser.eval(parent);
    BOOST_CHECK_EQUAL(json(true),result3);
}

BOOST_AUTO_TEST_CASE(test_precedence)
{
    size_t line = 1;
    size_t column = 1;
    const char* pend;
    jsonpath_filter_parser<json> parser(&pend,&line,&column);
    json parent = json::array();
    parent.add(1);
    parent.add(2);

    //BOOST_CHECK(false || true && false);

    //std::string expr5 = "false || true && true";
    //parser.parse(expr5.c_str(), expr5.c_str()+ expr5.length());
    //auto result5 = parser.eval(parent);
    //BOOST_CHECK(result5);
}

BOOST_AUTO_TEST_SUITE_END()


/// @brief Eigen wrappers for the exact CCD methods.

#pragma once

#include <Eigen/Core>

namespace IPC {
namespace ExactCCD {

/// @brief Methods of geometrically exact continous collision detection.
enum Method {
    NONE, ///< @brief Do not use exact CCD.
    ROOT_PARITY, ///< @brief Root parity method of Brochu et al. [2012].
    BSC, ///< @brief Bernstein sign classification method of Tang et al. [2014].
    RATIONAL_ROOT_PARITY ///< @brief Teseo's reimplementation of Brochu et al. [2012] using rationals
};

/**
 * @brief Detect collisions between two edges as they move.
 *
 * Looks for collisions between edges (q0start, p0start) and (q1start, p1start)
 * as they move towards (q0end, p0end) and (q1end, p1end). Returns true if the
 * edges collide.
 *
 * @param[in]  q0start  Start position of the first edge's first vertex.
 * @param[in]  p0start  Start position of the first edge's second vertex.
 * @param[in]  q1start  Start position of the second edge's first vertex.
 * @param[in]  p1start  Start position of the second edge's second vertex.
 * @param[in]  q0end    End position of the first edge's first vertex.
 * @param[in]  p0end    End position of the first edge's second vertex.
 * @param[in]  q1end    End position of the second edge's first vertex.
 * @param[in]  p1end    End position of the second edge's second vertex.
 * @param[in]  method   Method of exact CCD.
 *
 * @returns True if the edges collide.
 */
bool edgeEdgeCCD(
    const Eigen::Vector3d& q0start,
    const Eigen::Vector3d& p0start,
    const Eigen::Vector3d& q1start,
    const Eigen::Vector3d& p1start,
    const Eigen::Vector3d& q0end,
    const Eigen::Vector3d& p0end,
    const Eigen::Vector3d& q1end,
    const Eigen::Vector3d& p1end,
    const Method method);

/**
 * @brief Detect collisions between a vertex and a triangular face.
 *
 * Looks for collisions between the vertex q0start and the face
 * (q1start, q2start, q3start) as they move towards q0end and
 * (q1end, q2end, q3end). Returns true if the vertex and face collide.
 *
 * @param[in]  q0start  Start position of the vertex.
 * @param[in]  q1start  Start position of the first vertex of the face.
 * @param[in]  q2start  Start position of the second vertex of the face.
 * @param[in]  q3start  Start position of the third vertex of the face.
 * @param[in]  q0end    End position of the vertex.
 * @param[in]  q1end    End position of the first vertex of the face.
 * @param[in]  q2end    End position of the second vertex of the face.
 * @param[in]  q3end    End position of the third vertex of the face.
 * @param[in]  method   Method of exact CCD.
 *
 * @returns  True if the vertex and face collide.
 */
bool vertexFaceCCD(
    const Eigen::Vector3d& q0start,
    const Eigen::Vector3d& q1start,
    const Eigen::Vector3d& q2start,
    const Eigen::Vector3d& q3start,
    const Eigen::Vector3d& q0end,
    const Eigen::Vector3d& q1end,
    const Eigen::Vector3d& q2end,
    const Eigen::Vector3d& q3end,
    const Method method);

}
} // namespace IPC::ExactCCD


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  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 Willow Garage, Inc. 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.
 *
 */

#include "precomp.hpp"

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/core/eigen.hpp>
#include <opencv2/sfm/projection.hpp>
#include <opencv2/sfm/triangulation.hpp>
#include <opencv2/sfm/fundamental.hpp>
#include <opencv2/sfm/numeric.hpp>
#include <opencv2/sfm/conditioning.hpp>

// libmv headers
#include "libmv/multiview/fundamental.h"

#include <iostream>
using namespace std;

namespace cv
{
namespace sfm
{
  template<typename T>
  void
  projectionsFromFundamental( const Mat_<T> &F,
                              Mat_<T> P1,
                              Mat_<T> P2 )
  {
    P1 << 1, 0, 0, 0,
          0, 1, 0, 0,
          0, 0, 1, 0;

    Vec<T,3> e2;
    cv::SVD::solveZ(F.t(), e2);

    Mat_<T> P2cols = skew(e2) * F;
    for(char j=0;j<3;++j) {
      for(char i=0;i<3;++i)
        P2(j,i) = P2cols(j,i);
      P2(j,3) = e2(j);
    }

  }

  void
  projectionsFromFundamental( InputArray _F,
                              OutputArray _P1,
                              OutputArray _P2 )
  {
    const Mat F = _F.getMat();
    const int depth = F.depth();
    CV_Assert(F.cols == 3 && F.rows == 3 && (depth == CV_32F || depth == CV_64F));

    _P1.create(3, 4, depth);
    _P2.create(3, 4, depth);

    Mat P1 = _P1.getMat(),  P2 = _P2.getMat();

    // type
    if( depth == CV_32F )
    {
      projectionsFromFundamental<float>(F, P1, P2);
    }
    else
    {
      projectionsFromFundamental<double>(F, P1, P2);
    }

  }

  template<typename T>
  void
  fundamentalFromProjections( const Mat_<T> &P1,
                              const Mat_<T> &P2,
                              Mat_<T> F )
  {
    Mat_<T> X[3];
    vconcat( P1.row(1), P1.row(2), X[0] );
    vconcat( P1.row(2), P1.row(0), X[1] );
    vconcat( P1.row(0), P1.row(1), X[2] );

    Mat_<T> Y[3];
    vconcat( P2.row(1), P2.row(2), Y[0] );
    vconcat( P2.row(2), P2.row(0), Y[1] );
    vconcat( P2.row(0), P2.row(1), Y[2] );

    Mat_<T> XY;
    for (int i = 0; i < 3; ++i)
      for (int j = 0; j < 3; ++j)
      {
        vconcat(X[j], Y[i], XY);
        F(i, j) = determinant(XY);
      }
  }

  void
  fundamentalFromProjections( InputArray _P1,
                              InputArray _P2,
                              OutputArray _F )
  {
    const Mat P1 = _P1.getMat(), P2 = _P2.getMat();
    const int depth = P1.depth();
    CV_Assert((P1.cols == 4 && P1.rows == 3) && P1.rows == P2.rows && P1.cols == P2.cols);
    CV_Assert((depth == CV_32F || depth == CV_64F) && depth == P2.depth());

    _F.create(3, 3, depth);

    Mat F = _F.getMat();

    // type
    if( depth == CV_32F )
    {
      fundamentalFromProjections<float>(P1, P2, F);
    }
    else
    {
      fundamentalFromProjections<double>(P1, P2, F);
    }

  }

  template<typename T>
  void
  normalizedEightPointSolver( const Mat_<T> &_x1,
                              const Mat_<T> &_x2,
                              Mat_<T> _F )
  {
    libmv::Mat x1, x2;
    libmv::Mat3 F;

    cv2eigen(_x1, x1);
    cv2eigen(_x2, x2);

    libmv::NormalizedEightPointSolver(x1, x2, &F);

    eigen2cv(F, _F);
  }

  void
  normalizedEightPointSolver( InputArray _x1, InputArray _x2, OutputArray _F )
  {
    const Mat x1 = _x1.getMat(), x2 = _x2.getMat();
    const int depth = x1.depth();
    CV_Assert(x1.dims == 2 && x1.dims == x2.dims && (depth == CV_32F || depth == CV_64F));

    _F.create(3, 3, depth);

    Mat F = _F.getMat();

    // type
    if( depth == CV_32F )
    {
      normalizedEightPointSolver<float>(x1, x2, F);
    }
    else
    {
      normalizedEightPointSolver<double>(x1, x2, F);
    }

  }

  template<typename T>
  void
  relativeCameraMotion( const Mat_<T> &R1,
                        const Mat_<T> &t1,
                        const Mat_<T> &R2,
                        const Mat_<T> &t2,
                        Mat_<T> R,
                        Mat_<T> t )
  {
    R = R2 * R1.t();
    t = t2 - R * t1;
  }

  void
  relativeCameraMotion( InputArray _R1, InputArray _t1, InputArray _R2,
                        InputArray _t2, OutputArray _R, OutputArray _t )
  {
    const Mat R1 = _R1.getMat(), t1 = _t1.getMat(), R2 = _R2.getMat(), t2 = _t2.getMat();
    const int depth = R1.depth();
    CV_Assert((R1.cols == 3 && R1.rows == 3) && (R1.size() == R2.size()));
    CV_Assert((t1.cols == 1 && t1.rows == 3) && (t1.size() == t2.size()));
    CV_Assert((depth == CV_32F || depth == CV_64F) && depth == R2.depth() && depth == t1.depth() && depth == t2.depth());

    _R.create(3, 3, depth);
    _t.create(3, 1, depth);

    Mat R = _R.getMat(), t = _t.getMat();

    // type
    if( depth == CV_32F )
    {
      relativeCameraMotion<float>(R1, t1, R2, t2, R, t);
    }
    else
    {
      relativeCameraMotion<double>(R1, t1, R2, t2, R, t);
    }

  }

  template<typename T>
  void
  motionFromEssential( const Mat_<T> &_E,
                       std::vector<Mat> &_Rs,
                       std::vector<Mat> &_ts )
  {
    libmv::Mat3 E;
    std::vector < libmv::Mat3 > Rs;
    std::vector < libmv::Vec3 > ts;

    cv2eigen(_E, E);

    libmv::MotionFromEssential(E, &Rs, &ts);

    _Rs.clear();
    _ts.clear();

    int n = Rs.size();
    CV_Assert(ts.size() == n);

    for ( int i = 0; i < n; ++i )
    {
      Mat_<T> R_temp, t_temp;

      eigen2cv(Rs[i], R_temp);
      _Rs.push_back(R_temp);

      eigen2cv(ts[i], t_temp);
      _ts.push_back(t_temp);
    }

  }

  void
  motionFromEssential( InputArray _E, OutputArrayOfArrays _Rs,
                       OutputArrayOfArrays _ts )
  {
    const Mat E = _E.getMat();
    const int depth = E.depth(), cn = 4;
    CV_Assert(E.cols == 3 && E.rows == 3 && (depth == CV_32F || depth == CV_64F));

    _Rs.create(cn, 1, depth);
    _ts.create(cn, 1, depth);
    for (int i = 0; i < cn; ++i)
    {
      _Rs.create(Size(3,3), depth, i);
      _ts.create(Size(3,1), depth, i);
    }

    std::vector<Mat> Rs, ts;
    _Rs.getMatVector(Rs);
    _ts.getMatVector(ts);

    // type
    if( depth == CV_32F )
    {
      motionFromEssential<float>(E, Rs, ts);
    }
    else
    {
      motionFromEssential<double>(E, Rs, ts);
    }

    for (int i = 0; i < cn; ++i)
    {
      Rs[i].copyTo(_Rs.getMatRef(i));
      ts[i].copyTo(_ts.getMatRef(i));
    }

  }

  template<typename T>
  int motionFromEssentialChooseSolution( const std::vector<Mat> &Rs,
                                         const std::vector<Mat> &ts,
                                         const Mat_<T> &K1,
                                         const Mat_<T> &x1,
                                         const Mat_<T> &K2,
                                         const Mat_<T> &x2 )
  {
    Mat_<T> P1, P2, R1 = Mat_<T>::eye(3,3);

    T val = static_cast<T>(0.0);
    Vec<T,3> t1(val, val, val);

    projectionFromKRt(K1, R1, t1, P1);

    std::vector<Mat_<T> > points2d;
    points2d.push_back(x1);
    points2d.push_back(x2);

    for ( int i = 0; i < 4; ++i )
    {
      const Mat_<T> R2 = Rs[i];
      const Vec<T,3> t2 = ts[i];
      projectionFromKRt(K2, R2, t2, P2);

      std::vector<Mat_<T> > Ps;
      Ps.push_back(P1);
      Ps.push_back(P2);

      Vec<T,3> X;
      triangulatePoints(points2d, Ps, X);

      T d1 = depth(R1, t1, X);
      T d2 = depth(R2, t2, X);

      // Test if point is front to the two cameras.
      if ( d1 > 0 && d2 > 0 )
      {
        return i;
      }
    }

    return -1;
  }

  int motionFromEssentialChooseSolution( InputArrayOfArrays _Rs,
                                         InputArrayOfArrays _ts,
                                         InputArray _K1,
                                         InputArray _x1,
                                         InputArray _K2,
                                         InputArray _x2 )
  {
    std::vector<Mat> Rs, ts;
    _Rs.getMatVector(Rs);
    _ts.getMatVector(ts);
    const Mat K1 = _K1.getMat(), x1 = _x1.getMat(), K2 = _K2.getMat(), x2 = _x2.getMat();
    const int depth = K1.depth();
    CV_Assert( Rs.size() == 4 && ts.size() == 4 );
    CV_Assert((K1.cols == 3 && K1.rows == 3) && (K1.size() == K2.size()));
    CV_Assert((x1.cols == 1 && x1.rows == 2) && (x1.size() == x2.size()));
    CV_Assert((depth == CV_32F || depth == CV_64F) && depth == K2.depth() && depth == x1.depth() && depth == x2.depth());

    int solution = 0;

    // type
    if( depth == CV_32F )
    {
      solution = motionFromEssentialChooseSolution<float>(Rs, ts, K1, x1, K2, x2);
    }
    else
    {
      solution = motionFromEssentialChooseSolution<double>(Rs, ts, K1, x1, K2, x2);
    }

    return solution;
  }

  template<typename T>
  void
  fundamentalFromEssential( const Mat_<T> &E,
                            const Mat_<T> &K1,
                            const Mat_<T> &K2,
                            Mat_<T> F )
  {
    F = K2.inv().t() * E * K1.inv();
  }

  void
  fundamentalFromEssential( InputArray _E,
                            InputArray _K1,
                            InputArray _K2,
                            OutputArray _F )
  {
    const Mat E = _E.getMat(), K1 = _K1.getMat(), K2 = _K2.getMat();
    const int depth =  E.depth();
    CV_Assert(E.cols == 3 && E.rows == 3 && E.size() == _K1.size() && E.size() == _K2.size() && (depth == CV_32F || depth == CV_64F));

    _F.create(3, 3, depth);

    Mat F = _F.getMat();

    // type
    if( depth == CV_32F )
    {
      fundamentalFromEssential<float>(E, K1, K2, F);
    }
    else
    {
      fundamentalFromEssential<double>(E, K1, K2, F);
    }

  }

  template<typename T>
  void
  essentialFromFundamental( const Mat_<T> &F,
                            const Mat_<T> &K1,
                            const Mat_<T> &K2,
                            Mat_<T> E )
  {
    E = K2.t() * F * K1;
  }

  void
  essentialFromFundamental( InputArray _F,
                            InputArray _K1,
                            InputArray _K2,
                            OutputArray _E )
  {
    const Mat F = _F.getMat(), K1 = _K1.getMat(), K2 = _K2.getMat();
    const int depth =  F.depth();
    CV_Assert(F.cols == 3 && F.rows == 3 && F.size() == _K1.size() && F.size() == _K2.size() && (depth == CV_32F || depth == CV_64F));

    _E.create(3, 3, depth);

    Mat E = _E.getMat();

    // type
    if( depth == CV_32F )
    {
      essentialFromFundamental<float>(F, K1, K2, E);
    }
    else
    {
      essentialFromFundamental<double>(F, K1, K2, E);
    }
  }

  template<typename T>
  void
  essentialFromRt( const Mat_<T> &_R1,
                   const Mat_<T> &_t1,
                   const Mat_<T> &_R2,
                   const Mat_<T> &_t2,
                   Mat_<T> _E )
  {
    libmv::Mat3 E;
    libmv::Mat3 R1, R2;
    libmv::Vec3 t1, t2;

    cv2eigen( _R1, R1 );
    cv2eigen( _t1, t1 );
    cv2eigen( _R2, R2 );
    cv2eigen( _t2, t2 );

    libmv::EssentialFromRt( R1, t1, R2, t2, &E );

    eigen2cv( E, _E );
  }

  void
  essentialFromRt( InputArray _R1,
                   InputArray _t1,
                   InputArray _R2,
                   InputArray _t2,
                   OutputArray _E )
  {
    const Mat R1 = _R1.getMat(), t1 = _t1.getMat(), R2 = _R2.getMat(), t2 = _t2.getMat();
    const int depth = R1.depth();
    CV_Assert((R1.cols == 3 && R1.rows == 3) && (R1.size() == R2.size()));
    CV_Assert((t1.cols == 1 && t1.rows == 3) && (t1.size() == t2.size()));
    CV_Assert((depth == CV_32F || depth == CV_64F) && depth == R2.depth() && depth == t1.depth() && depth == t2.depth());

    _E.create(3, 3, depth);

    Mat E = _E.getMat();

    // type
    if( depth == CV_32F )
    {
      essentialFromRt<float>(R1, t1, R2, t2, E);
    }
    else
    {
      essentialFromRt<double>(R1, t1, R2, t2, E);
    }

  }

  template<typename T>
  void
  normalizeFundamental( const Mat_<T> &F, Mat_<T> F_normalized )
  {
    F_normalized = F * (1.0/norm(F,NORM_L2));  // Frobenius Norm

    if ( F_normalized(2,2) < 0 )
    {
        F_normalized *= -1;
    }
  }

  void
  normalizeFundamental( InputArray _F,
                        OutputArray _F_normalized )
  {
    const Mat F = _F.getMat();
    const int depth =  F.depth();
    CV_Assert(F.cols == 3 && F.rows == 3 && (depth == CV_32F || depth == CV_64F));

    _F_normalized.create(3, 3, depth);

    Mat F_normalized = _F_normalized.getMat();

    // type
    if( depth == CV_32F )
    {
      normalizeFundamental<float>(F, F_normalized);
    }
    else
    {
      normalizeFundamental<double>(F, F_normalized);
    }
  }

  template<typename T>
  void
  computeOrientation( const Mat_<T> &x1,
                      const Mat_<T> &x2,
                      Mat_<T> R,
                      Mat_<T> t,
                      T s )
  {
    Mat_<T> rr, rl, rt, lt;
    normalizePoints(x1, rr, rt);
    normalizePoints(x2, rl, lt);

    Mat_<T> rrBar, rlBar, rVar, lVar;
    meanAndVarianceAlongRows(rr, rrBar, rVar);
    meanAndVarianceAlongRows(rl, rlBar, lVar);

    Mat_<T> rrp, rlp;
    rrp = rr - repeat(rrBar, x1.rows, x1.cols);
    rlp = rl - repeat(rlBar, x2.rows, x2.cols);

    // TODO: finish implementation
    // https://github.com/vrabaud/sfm_toolbox/blob/master/sfm/computeOrientation.m#L44
  }

  void
  computeOrientation( InputArrayOfArrays _x1,
                      InputArrayOfArrays _x2,
                      OutputArray _R,
                      OutputArray _t,
                      double s )
  {
    const Mat x1 = _x1.getMat(), x2 = _x2.getMat();
    const int depth =  x1.depth();
    CV_Assert(x1.size() == x2.size() && (depth == CV_32F || depth == CV_64F));

    _R.create(3, 3, depth);
    _t.create(3, 1, depth);

    Mat R = _R.getMat(), t = _t.getMat();

    // type
    if( depth == CV_32F )
    {
      computeOrientation<float>(x1, x2, R, t, s);
    }
    else
    {
      computeOrientation<double>(x1, x2, R, t, s);
    }
  }

} /* namespace sfm */
} /* namespace cv */


//
// Created by alex on 5/28/19.
//

#include <types/Image.h>
#include <types/Color.h>
#include <boost/python.hpp>
#include <cmath>
#include <algorithm>
#include <random>
#include <numeric>

#include <iostream>
#include <fstream>

template<typename T>
T clamp(T x, T a, T b)
{
    if(x < a) return a;
    if(x > b) return b;
    return x;
}

struct Coord
{
    int x, y;
};

Color bilinear_interpolation(std::array<Color, 4> corners, std::array<Coord, 4> coords, double x, double y)
{
    double r = 0.0, g = 0.0, b = 0.0;
    double sum = 0.0;

    for(int i = 0; i < 4; i++)
    {
        double d = sqrt(
                (coords[i].x - x) * (coords[i].x - x) +
                (coords[i].y - y) * (coords[i].y - y));

        r += corners[i].r * d;
        g += corners[i].g * d;
        b += corners[i].b * d;

        sum += d;
    }

    r /= sum;
    g /= sum;
    b /= sum;

    return Color(
            static_cast<uint8_t>(r),
            static_cast<uint8_t>(g),
            static_cast<uint8_t>(b));
}

Image separate_blue_channel(const Image& image)
{
    Image new_image(image.width(), image.height());

    for(int row = 0; row < static_cast<int>(image.height()); row++)
        for(int column = 0; column < static_cast<int>(image.width()); column++)
        {
            auto color = image.pixelAt(row, column);
            new_image.setPixel(row, column, Color(0, 0, color.b));
        }

    return new_image;
}

Image separate_red_channel(const Image& image)
{
    Image new_image(image.width(), image.height());

    for(int row = 0; row < static_cast<int>(image.height()); row++)
        for(int column = 0; column < static_cast<int>(image.width()); column++)
        {
            auto color = image.pixelAt(row, column);
            new_image.setPixel(row, column, Color(color.r, 0, 0));
        }

    return new_image;
}

Image separate_green_channel(const Image& image)
{
    Image new_image(image.width(), image.height());

    for(int row = 0; row < static_cast<int>(image.height()); row++)
        for(int column = 0; column < static_cast<int>(image.width()); column++)
        {
            auto color = image.pixelAt(row, column);
            new_image.setPixel(row, column, Color(0, color.g, 0));
        }

    return new_image;
}

Image luminance_map(const Image& image)
{
    Image new_image(image.width(), image.height());

    for(int row = 0; row < static_cast<int>(image.height()); row++)
        for(int column = 0; column < static_cast<int>(image.width()); column++)
        {
            auto color = image.pixelAt(row, column);
            auto luminance = static_cast<long long int>(color.r * 0.2126 + color.g * 0.7152 + color.b * 0.0722);
            new_image.setPixel(row, column, Color(luminance, luminance, luminance));
        }

    return new_image;
}

Image pixel_sort(const Image& source, const Image& mask)
{
    struct Interval {int column, row_start, row_end; };

    std::vector<Interval> intervals;
    for(int column = 0; column < static_cast<int>(source.width()); column++)
    {
        int row = 0;
        while(row < static_cast<int>(source.height()))
        {
            int row_begin = row;
            while(row < static_cast<int>(source.height()) && mask.pixelAt(row, column).r == 255u)
                row ++;
            intervals.push_back({column, row_begin, row - 1});

            while(row < static_cast<int>(source.height()) && mask.pixelAt(row, column).r != 255u)
                row ++;
        }
    }

    Image result = source;

    std::vector<Color> pixels;
    for(const auto& [column, row_start, row_end]: intervals)
    {
        for(int row = row_start; row <= row_end; row++)
            pixels.push_back(result.pixelAt(row, column));

        std::sort(pixels.begin(), pixels.end(), [](const auto& lhs, const auto& rhs)
        {
            auto a = lhs.r * 0.2126 + lhs.g * 0.7152 + lhs.b * 0.0722;
            auto b = rhs.r * 0.2126 + rhs.g * 0.7152 + rhs.b * 0.0722;

            return a < b;
        });

        for(int row = row_start, i = 0; row <= row_end; row++, i++)
            result.setPixel(row, column, pixels[i]);

        pixels.clear();
    }

    return result;
}

Image pixel_distort_displace(const Image& source, const Image& map, double row_factor, double column_factor)
{
    Image new_image(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
        for(int column = 0; column < static_cast<int>(source.width()); column++)
        {
            double displacement = row_factor * (map.pixelAt(row, column).r / 255. * 2. - 1.);
            long long int column_displaced = static_cast<long long int>(displacement * column_factor) + column;
            long long int row_displaced= static_cast<long long int>(displacement * row_factor) + row;

            if((row_displaced >= 0 && row_displaced < static_cast<int>(source.height())) &&
               (column_displaced >= 0 && column_displaced < static_cast<int>(source.width()))
                    )
            {
                new_image.setPixel(row_displaced, column_displaced, source.pixelAt(row, column));
            }
        }

    for(int row = 0; row < static_cast<int>(source.height()); row++)
        for(int column = 0; column < static_cast<int>(source.width()); column++)
        {
            if(new_image.pixelAt(row, column).r == 0 &&
               new_image.pixelAt(row, column).g == 0 &&
               new_image.pixelAt(row, column).b == 0)
                new_image.setPixel(row, column, source.pixelAt(row, column));
        }

    return new_image;
}

Image displacement(const Image& source, const Image& map, double row_factor, double column_factor)
{
    Image new_image(source.width(), source.height());
    Image known_pixels(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
    {
        for(int column = 0; column < static_cast<int>(source.width()); column++)
        {
            double displacement = (map.pixelAt(row, column).r / 255. * 2. - 1.);
            long long int column_displaced = static_cast<long long int>(displacement * column_factor) + column;
            long long int row_displaced= static_cast<long long int>(displacement * row_factor) + row;

            row_displaced = (row_displaced + static_cast<int>(source.height())) % static_cast<int>(source.height());
            column_displaced = (column_displaced + static_cast<int>(source.width())) % static_cast<int>(source.width());

            new_image.setPixel(row_displaced, column_displaced, source.pixelAt(row, column));
            known_pixels.setPixel(row_displaced, column_displaced, Color(1, 0, 0));
        }
    }

    int row_radius = std::max(1, static_cast<int>(row_factor)) * 3;
    int column_radius = std::max(1, static_cast<int>(column_factor)) * 3;

    for(int row = 0; row < static_cast<int>(source.height()); row++)
    {
        for(int column = 0; column < static_cast<int>(source.width()); column++)
        {
            if(!known_pixels.pixelAt(row, column).r)
            {
                std::array<Color, 4> samples{};
                std::array<Coord, 4> coords{};
                char rowDir[] = {1, -1, 0, 0};
                char colDir[] = {0, 0, -1, 1};

                for(int i = 0; i < 4; i++)
                {
                    int row_ = row, column_ = column;
                    bool finished = false;

                    for(int l = 0; l < row_radius && !finished; l++)
                    {
                        row_ += rowDir[i];
                        column_ = column;
                        for(int k = 0; k < column_radius && !finished; k++)
                        {
                            column_ += colDir[i];

                            if(known_pixels.pixelAt(
                                    (row_ + static_cast<int>(source.height())) % static_cast<int>(source.height()),
                                    (column_ + static_cast<int>(source.width())) % static_cast<int>(source.width())).r)
                            {
                                coords[i] = {
                                        (column_ + static_cast<int>(source.width())) % static_cast<int>(source.width()),
                                        (row_ + static_cast<int>(source.height())) % static_cast<int>(source.height())};
                                samples[i] = new_image.pixelAt(
                                        (row_ + static_cast<int>(source.height())) % static_cast<int>(source.height()),
                                        (column_ + static_cast<int>(source.width())) % static_cast<int>(source.width()));
                                finished = true;
                            }
                        }
                    }
                }

                new_image.setPixel(row, column,
                        bilinear_interpolation(samples, coords, column, row));
                known_pixels.setPixel(row, column, Color(1, 0, 0));
            }
        }
    }

    return new_image;
}

Image lift_gain(const Image& source, double lift, double gain)
{
    Image new_image(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
        for(int column = 0; column < static_cast<int>(source.width()); column++)
        {
            auto color = source.pixelAt(static_cast<int>(row), static_cast<int>(column));
            auto r = color.r * (gain - lift) + lift;
            auto g = color.g * (gain - lift) + lift;
            auto b = color.b * (gain - lift) + lift;

            r = clamp(r, 0., 255.);
            g = clamp(g, 0., 255.);
            b = clamp(b, 0., 255.);

            new_image.setPixel(static_cast<int>(row), static_cast<int>(column), Color(static_cast<uint8_t>(r), static_cast<uint8_t>(g),
                                                                                static_cast<uint8_t>(b)));
        }

    return new_image;
}

Image gamma_(const Image& source, double gamma)
{
    Image new_image(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
        for(int column = 0; column < static_cast<int>(source.width()); column++)
        {
            auto color = source.pixelAt(static_cast<int>(row), static_cast<int>(column));
            auto r = pow(color.r, 1. / gamma);
            auto g = pow(color.g, 1. / gamma);
            auto b = pow(color.b, 1. / gamma);

            r = clamp(r, 0., 255.);
            g = clamp(g, 0., 255.);
            b = clamp(b, 0., 255.);

            new_image.setPixel(static_cast<int>(row), static_cast<int>(column), Color(static_cast<uint8_t>(r), static_cast<uint8_t>(g),
                                                                                static_cast<uint8_t>(b)));
        }

    return new_image;
}

Image contrast(const Image& source, double contrast)
{
    Image new_image(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
        for(int column = 0; column < static_cast<int>(source.width()); column++)
        {
            auto color = source.pixelAt(static_cast<int>(row), static_cast<int>(column));
            auto r = color.r * (1. + contrast) * (-1.) - contrast / 2.;
            auto g = color.g * (1. + contrast) * (-1.) - contrast / 2.;
            auto b = color.b * (1. + contrast) * (-1.) - contrast / 2.;

            r = clamp(r, 0., 255.);
            g = clamp(g, 0., 255.);
            b = clamp(b, 0., 255.);

            new_image.setPixel(static_cast<int>(row), static_cast<int>(column), Color(static_cast<uint8_t>(r), static_cast<uint8_t>(g),
                                                                                static_cast<uint8_t>(b)));
        }

    return new_image;
}

Image brightness(const Image& source, double brightness)
{
    Image new_image(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
        for(int column = 0; column < static_cast<int>(source.width()); column++)
        {
            auto color = source.pixelAt(static_cast<int>(row), static_cast<int>(column));
            auto r = color.r + brightness * 255;
            auto g = color.g + brightness * 255;
            auto b = color.b + brightness * 255;

            r = clamp(r, 0., 255.);
            g = clamp(g, 0., 255.);
            b = clamp(b, 0., 255.);

            new_image.setPixel(static_cast<int>(row), static_cast<int>(column), Color(static_cast<uint8_t>(r), static_cast<uint8_t>(g),
                                                                                static_cast<uint8_t>(b)));
        }

    return new_image;
}

Image box_blur(const Image& source, double row_factor, double column_factor)
{
    static auto get_pixel_or_black = [](const Image& image, int row, int column)
    {
        if(row < static_cast<int>(image.height()) && column < static_cast<int>(image.width()) && row >= 0 && column >= 0)
            return image.pixelAt(row, column);
        return Color();
    };

    Image tmp(source.width(), source.height());
    {
        double weight = 1.0f / column_factor;
        int half = static_cast<int>(column_factor / 2);
        for(int row = 0; row < static_cast<int>(source.height()); row++)
            for(int column = 0; column < static_cast<int>(source.width()); column++)
            {
                double r = 0., g = 0., b = 0.;
                for(int i = -half; i <= half; i++)
                {
                    r += get_pixel_or_black(source, row, column + i).r * weight;
                    g += get_pixel_or_black(source, row, column + i).g * weight;
                    b += get_pixel_or_black(source, row, column + i).b * weight;
                }

                r = clamp(r, 0., 255.);
                g = clamp(g, 0., 255.);
                b = clamp(b, 0., 255.);

                tmp.setPixel(static_cast<int>(row), static_cast<int>(column), Color(static_cast<uint8_t>(r), static_cast<uint8_t>(g),
                                                                                static_cast<uint8_t>(b)));
            }
    }

    Image new_image(source.width(), source.height());

    {
        double weight = 1.0f / row_factor;
        int half = static_cast<int>(column_factor / 2);
        for(int row = 0; row < static_cast<int>(source.height()); row++)
            for(int column = 0; column < static_cast<int>(source.width()); column++)
            {
                double r = 0., g = 0., b = 0.;
                for(int i = -half; i <= half; i++)
                {
                    r += get_pixel_or_black(tmp, row + i, column).r * weight;
                    g += get_pixel_or_black(tmp, row + i, column).g * weight;
                    b += get_pixel_or_black(tmp, row + i, column).b * weight;
                }

                r = clamp(r, 0., 255.);
                g = clamp(g, 0., 255.);
                b = clamp(b, 0., 255.);

                new_image.setPixel(static_cast<int>(row), static_cast<int>(column), Color(static_cast<uint8_t>(r), static_cast<uint8_t>(g),
                                                                                static_cast<uint8_t>(b)));
            }
    }

    return new_image;
}

Image mask(const Image& source, const Image& mask)
{
    Image new_image(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
    {
        for(int column = 0; column < static_cast<int>(source.width()); column++)
            if(mask.pixelAt(row, column).r == 255u)
                new_image.setPixel(row, column, source.pixelAt(row, column));
            else new_image.setPixel(row, column, Color(0, 0, 0));
    }

    return new_image;
}

Image add(const Image& a, const Image& b)
{
    Image new_image(a.width(), a.height());

    for(int row = 0; row < static_cast<int>(a.height()); row++)
    {
        for(int column = 0; column < static_cast<int>(a.width()); column++)
        {
            uint8_t r_ = a.pixelAt(row, column).r + b.pixelAt(row, column).r;
            uint8_t g_ = a.pixelAt(row, column).g + b.pixelAt(row, column).g;
            uint8_t b_ = a.pixelAt(row, column).b + b.pixelAt(row, column).b;

            new_image.setPixel(row, column, Color(r_, g_, b_));
        }
    }

    return new_image;
}

Image strict_add(const Image& a, const Image& b)
{
    Image new_image(a.width(), a.height());

    for(int row = 0; row < static_cast<int>(a.height()); row++)
    {
        for(int column = 0; column < static_cast<int>(a.width()); column++)
        {
            int r_ = static_cast<int>(a.pixelAt(row, column).r) + static_cast<int>(b.pixelAt(row, column).r);
            int g_ = static_cast<int>(a.pixelAt(row, column).g) + static_cast<int>(b.pixelAt(row, column).g);
            int b_ = static_cast<int>(a.pixelAt(row, column).b) + static_cast<int>(b.pixelAt(row, column).b);

            r_ = clamp(r_, 0, 255);
            g_ = clamp(g_, 0, 255);
            b_ = clamp(b_, 0, 255);

            new_image.setPixel(row, column, Color(r_, g_, b_));
        }
    }

    return new_image;
}

Image multiply(const Image& a, const Image& b)
{
    Image new_image(a.width(), a.height());

    for(int row = 0; row < static_cast<int>(a.height()); row++)
    {
        for (int column = 0; column < static_cast<int>(a.width()); column++)
        {
            int r_ = static_cast<int>(a.pixelAt(row, column).r) * static_cast<int>(b.pixelAt(row, column).r);
            int g_ = static_cast<int>(a.pixelAt(row, column).g) * static_cast<int>(b.pixelAt(row, column).g);
            int b_ = static_cast<int>(a.pixelAt(row, column).b) * static_cast<int>(b.pixelAt(row, column).b);

            r_ /= 255;
            g_ /= 255;
            b_ /= 255;

            new_image.setPixel(row, column, Color(r_, g_, b_));
        }
    }

    return new_image;
}

namespace PerlinNoise
{
    double lerp(double a, double b, double x)
    {
        return a + x * (b - a);
    }

    double fade(double t)
    {
        return t * t * t * (t * (t * 6 - 15) + 10);
    }

    int inc(int num)
    {
        return num + 1;
    }

    double grad(int hash, double x, double y, double z)
    {
        switch(hash & 0xF)
        {
            case 0x0: return  x + y;
            case 0x1: return -x + y;
            case 0x2: return  x - y;
            case 0x3: return -x - y;
            case 0x4: return  x + z;
            case 0x5: return -x + z;
            case 0x6: return  x - z;
            case 0x7: return -x - z;
            case 0x8: return  y + z;
            case 0x9: return -y + z;
            case 0xA: return  y - z;
            case 0xB: return -y - z;
            case 0xC: return  y + x;
            case 0xD: return -y + z;
            case 0xE: return  y - x;
            case 0xF: return -y - z;
            default: return 0;
        }
    }

    double perlin(const std::vector<int>& p, double x, double y, double z)
    {
        int xi = static_cast<int>(x) & 255;
        int yi = static_cast<int>(y) & 255;
        int zi = static_cast<int>(z) & 255;

        double xf = x - static_cast<int>(x);
        double yf = y - static_cast<int>(y);
        double zf = z - static_cast<int>(z);

        double u = fade(xf);
        double v = fade(yf);
        double w = fade(zf);

        int aaa, aba, aab, abb, baa, bba, bab, bbb;
        aaa = p[p[p[    xi ]+    yi ]+    zi ];
        aba = p[p[p[    xi ]+inc(yi)]+    zi ];
        aab = p[p[p[    xi ]+    yi ]+inc(zi)];
        abb = p[p[p[    xi ]+inc(yi)]+inc(zi)];
        baa = p[p[p[inc(xi)]+    yi ]+    zi ];
        bba = p[p[p[inc(xi)]+inc(yi)]+    zi ];
        bab = p[p[p[inc(xi)]+    yi ]+inc(zi)];
        bbb = p[p[p[inc(xi)]+inc(yi)]+inc(zi)];

        double x1, x2, y1, y2;
        x1 = lerp(grad (aaa, xf, yf, zf), grad(baa, xf - 1, yf, zf), u);
        x2 = lerp(grad (aba, xf, yf - 1, zf), grad(bba, xf - 1, yf - 1, zf), u);
        y1 = lerp(x1, x2, v);
        x1 = lerp(grad (aab, xf, yf, zf - 1), grad (bab, xf - 1, yf , zf - 1), u);
        x2 = lerp(grad (abb, xf, yf - 1, zf - 1), grad (bbb, xf - 1, yf - 1, zf - 1), u);
        y2 = lerp(x1, x2, v);

        return (lerp(y1, y2, w) + 1) / 2;
    }

    Image perlin_noise(const Image& source, long long int seed, double zoom, double z, long long int octaves, double persistence)
    {
        Image new_image(source.width(), source.height());

        std::mt19937 engine(seed);

        std::vector<int> permutation;
        permutation.resize(256);
        std::iota(std::begin(permutation), std::end(permutation), 0);
        std::shuffle(std::begin(permutation), std::end(permutation), engine);

        permutation.insert(permutation.end(), permutation.begin(), permutation.end());

        std::vector<double> noise_array;

        noise_array.reserve(source.width() * source.height());
        double min = 1000;
        double max = -1000;
        for(int row = 0; row < static_cast<int>(source.height()); row++)
        {
            for(int column = 0; column < static_cast<int>(source.width()); column++)
            {
                double frequency = 1;
                double amplitude = 1;
                double max_value = 0;
                double total = 0;

                const double x = static_cast<double>(row) / static_cast<int>(source.height()) * zoom;
                const double y = static_cast<double>(column) / static_cast<int>(source.width()) * zoom;

                for(int i = 0; i < octaves; i++)
                {
                    total += amplitude * perlin(permutation, x * frequency, y * frequency, z * frequency);

                    max_value += amplitude;
                    amplitude *= persistence;
                    frequency *= 2;
                }

                noise_array.push_back(total);
                min = std::min(min, total);
                max = std::max(max, total);
            }
        }

        int i = 0;
        for(int row = 0; row < static_cast<int>(source.height()); row++)
        {
            for (int column = 0; column < static_cast<int>(source.width()); column++)
            {
                double noise = noise_array[i++];
                noise = -1. + 2. * (noise - min) / (max - min);
                noise += 1.;
                noise /= 2.;

                uint8_t sample = static_cast<double>(noise * 255);

                new_image.setPixel(row, column, Color(sample, sample, sample));
            }
        }

        return new_image;
    }
}

Image noise(const Image& source, long long int seed)
{
    Image new_image(source.width(), source.height());

    std::mt19937 engine(seed);
    std::uniform_int_distribution<uint8_t> distribution;

    for(int row = 0; row < static_cast<int>(source.height()); row++)
    {
        for (int column = 0; column < static_cast<int>(source.width()); column++)
        {
            uint8_t sample = distribution(engine);
            new_image.setPixel(row, column, Color(sample, sample, sample));
        }
    }

    return new_image;
}

Image discriminator_greater_than(const Image& source, long long int threshold)
{
    Image new_image(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
    {
        for(int column = 0; column < static_cast<int>(source.width()); column++)
            if(source.pixelAt(row, column).r >= threshold)
                new_image.setPixel(row, column, Color(255, 255, 255));
            else new_image.setPixel(row, column, Color(0, 0, 0));
    }

    return new_image;
}

Image discriminator_lesser_than(const Image& source, long long int threshold)
{
    Image new_image(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
    {
        for(int column = 0; column < static_cast<int>(source.width()); column++)
            if(source.pixelAt(row, column).r <= threshold)
                new_image.setPixel(row, column, Color(255, 255, 255));
            else new_image.setPixel(row, column, Color(0, 0, 0));
    }

    return new_image;
}

Image discriminator_range(const Image& source, long long int a, long long int b)
{
    Image new_image(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
    {
        for(int column = 0; column < static_cast<int>(source.width()); column++)
            if(source.pixelAt(row, column).r >= a && source.pixelAt(row, column).r <= b)
                new_image.setPixel(row, column, Color(255, 255, 255));
            else new_image.setPixel(row, column, Color(0, 0, 0));
    }

    return new_image;
}

Image solid_color(const Image& source, Color color)
{
    Image new_image(source.width(), source.height());

    for(int row = 0; row < static_cast<int>(source.height()); row++)
    {
        for (int column = 0; column < static_cast<int>(source.width()); column++)
            new_image.setPixel(row, column, color);
    }

    return new_image;
}

Image lens_distortion(const Image& source, double strength, double zoom)
{
    Image new_image(source.width(), source.height());

    int half_width = source.width() / 2;
    int half_height = source.height() / 2;

    strength = strength == 0 ? 0.0001 : strength;
    double correction_radius = sqrt(
            source.width() * source.width() +
            source.height() * source.height()) / strength;

    for(int row = 0; row < static_cast<int>(source.height()); row++)
    {
        for (int column = 0; column < static_cast<int>(source.width()); column++) {
            int new_row = row - half_height;
            int new_column = column - half_width;

            double distance = sqrt(new_row * new_row + new_column * new_column);
            double r = distance / correction_radius;
            double theta = r == 0 ? 1 : atan(r) / r;

            double source_row = half_height + theta * new_row * zoom;
            double source_column = half_width + theta * new_column * zoom;

            if (0 < source_row && source_row < source.height() &&
                0 < source_column && source_column < source.width())
            {
                double x = source_column - floor(source_column);
                double y = source_row - floor(source_row);

                new_image.setPixel(
                        row,
                        column,
                        bilinear_interpolation({
                            source.pixelAt(static_cast<unsigned int>(floor(source_row)), static_cast<unsigned int>(floor(source_column))), //0, 0
                            source.pixelAt(static_cast<unsigned int>(floor(source_row)), static_cast<unsigned int>(ceil(source_column))), //1, 0
                            source.pixelAt(static_cast<unsigned int>(ceil(source_row)), static_cast<unsigned int>(ceil(source_column))), //1, 1
                            source.pixelAt(static_cast<unsigned int>(ceil(source_row)), static_cast<unsigned int>(floor(source_column)))  //0, 1
                        },{
                            Coord{0, 0},
                            Coord{1, 0},
                            Coord{1, 1},
                            Coord{0, 1}
                        }, x, y));
            }
            else new_image.setPixel(row, column, Color(0, 0, 0));
        }
    }

    return new_image;
}

int hue(Color color)
{
    int max = std::max({color.r, color.g, color.b});
    int min = std::min({color.r, color.g, color.b});

    min = max == min ? min - 1 : min;

    int hue;
    if(max == color.r)
        hue = (static_cast<int>(color.g ) - color.b) / (max - min);
    else if(max == color.g)
        hue = 2 + (static_cast<int>(color.b) - color.r) / (max - min);
    else hue = 4 + (static_cast<int>(color.r) - color.g) / (max - min);

    hue *= 60;
    hue = hue < 0 ? hue + 360 : hue;

    return hue;
}

Image jpeg_artifacts(const Image& source, int radius)
{
    Image new_image = source;

    for(int row = 0; row < static_cast<int>(new_image.height()); row++)
    {
        for (int column = 0; column < static_cast<int>(new_image.width()); column++)
        {
            int row_swap = -1;
            int column_swap = -1;
            int max = hue(new_image.pixelAt(row, column));

            int row_ = row - radius;
            for(int i = -radius; i < radius; i++)
            {
                if(row_ < 0 || row_ >= static_cast<int>(new_image.height()))
                    continue;

                int column_ = column - radius;
                for(int j = -radius; j < radius; j++)
                {
                    if(column_ < 0 || column_ >= static_cast<int>(new_image.width()))
                        continue;

                    if(max < hue(new_image.pixelAt(row_, column_)))
                    {
                        max = hue(new_image.pixelAt(row_, column_));
                        row_swap = row_;
                        column_swap = column_;
                    }

                    column_++;
                }
            }

            if(row_swap != -1)
            {
                new_image.setPixel(row, column, new_image.pixelAt(row_swap, column_swap));
                new_image.setPixel(row_swap, column_swap, source.pixelAt(row, column));
            }
        }
    }

    return new_image;
}

BOOST_PYTHON_MODULE(images_internal)
{
    using namespace boost::python;

    def("luminance_map", luminance_map);
    def("separate_blue_channel", separate_blue_channel);
    def("separate_red_channel", separate_red_channel);
    def("separate_green_channel", separate_green_channel);
    def("pixel_sort", pixel_sort);
    def("pixel_distort_displace", pixel_distort_displace);
    def("displacement", displacement);
    def("lift_gain", lift_gain);
    def("gamma", gamma_);
    def("contrast", contrast);
    def("brightness", brightness);
    def("box_blur", box_blur);
    def("mask", mask);
    def("strict_add", strict_add);
    def("add", add);
    def("multiply", multiply);
    def("perlin_noise", PerlinNoise::perlin_noise);
    def("noise", noise);
    def("discriminator_greater_than", discriminator_greater_than);
    def("discriminator_lesser_than", discriminator_lesser_than);
    def("discriminator_range", discriminator_range);
    def("solid_color", solid_color);
    def("lens_distortion", lens_distortion);
    def("jpeg_artifacts", jpeg_artifacts);
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <complex>
#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <utility> // std::move

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"

#include "dune/common/stdstreams.hh"
#include "dune/grid/sgrid.hh"
#include "dune/grid/uggrid.hh"
#include "dune/grid/common/gridinfo.hh"

#include "fem/assemble.hh"
#include "fem/gridmanager.hh"
#include "fem/lagrangespace.hh"
#include "fem/functional_aux.hh"
#include "fem/coarsening.hh"
#include "fem/norms.hh"

#include "linalg/umfpack_solve.hh"

#include "timestepping/limexWithoutJens.hh"
#include "timestepping/semieuler.hh"
#include "timestepping/extrapolation.hh"

#include "io/vtk.hh"

#include "integrate.hh"
#include "movingsource.hh"

//#include "io/lossystorageDUNE.hh"
#include "io/lossystorageWithoutTemporalPred.hh"
// #include "io/lossystorage.hh"

using namespace Kaskade;

template <class Grid>
std::unique_ptr<Grid> RefineGrid(int refinements, int heapSize=500)
{
  Grid::setDefaultHeapSize(heapSize);
  Dune::GridFactory<Grid> factory;
  Dune::FieldVector<double,2> v;
  // vertices
  v[0]=0; v[1]=0;
  factory.insertVertex(v);
  v[0]=1; v[1]=0;
  factory.insertVertex(v);
  v[0]=1; v[1]=1;
  factory.insertVertex(v);
  v[0]=0; v[1]=1;
  factory.insertVertex(v);
  // elements
  std::vector<unsigned int> vid(3);
  vid[0]=0; vid[1]=1; vid[2]=2;
  factory.insertElement(Dune::GeometryType(Dune::GeometryType::simplex,2),vid);
  vid[0]=0; vid[1]=2; vid[2]=3;
  factory.insertElement(Dune::GeometryType(Dune::GeometryType::simplex,2),vid);
  std::unique_ptr<Grid> grid( factory.createGrid() ) ;
  grid->globalRefine(refinements);
  return grid;
}

struct InitialValue
{
  typedef double Scalar;
  static int const components = 1;
  typedef Dune::FieldVector<Scalar,components> ValueType;

  InitialValue(int c): component(c) {}

  template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }
  template <class Cell>
  ValueType value(Cell const& cell,
                  Dune::FieldVector<typename Cell::ctype,Cell::dimension> const& localCoordinate) const
  {
    Dune::FieldVector<typename Cell::ctype,Cell::dimensionworld> x = cell.geometry().global(localCoordinate);

	double vx = x[0]-0.5;
	double vy = x[1]-0.75;

    return 0.8*exp(-80.0*(vx*vx + vy*vy));
  }

private:
  int component;
};

int main(int argc, char *argv[])
  {
    int const dim = 2;
    int refinements = 7, order = 1, extrapolOrder = 1, maxSteps = 10, heapSize = 1000 ;
    double dt = 0.1, maxDT = 1.0, T = 10.0, rTolT = 1.0e-2, aTolT = 1.0e-2, rTolX = 1.0e-4, aTolX = 1.0e-4, writeInterval = 1.0;

    typedef Dune::UGGrid<dim> Grid;
    std::unique_ptr<Grid> grid( RefineGrid<Grid>(refinements,heapSize) );

    std::cout << "Grid: " << grid->size(0) << " " << grid->size(1) << " " << grid->size(2) << std::endl << std::endl;

    GridManager<Grid> gridManager(std::move(grid));

    // construct involved spaces and define equation
    typedef Grid::LeafGridView LeafView;
    typedef FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> > H1Space;

    H1Space temperatureSpace(gridManager,gridManager.grid().leafView(),order);

    typedef boost::fusion::vector<H1Space const*> Spaces;
    Spaces spaces(&temperatureSpace);

    typedef boost::fusion::vector<VariableDescription<0,1,0> > VariableDescriptions;
    std::string varNames[1] = { "u" };

    typedef VariableSetDescription<Spaces,VariableDescriptions> VariableSet;
    VariableSet variableSet(spaces,varNames);

    typedef MovingSourceEquation<double,VariableSet> Equation;
    Equation Eq;

    std::vector<VariableSet::VariableSet> solutions;

    // prepare lossy storage
    std::vector<double> times ;

    int coarseLevel = 0 ;
    double qTol = 1e-5 ;   // quantization error tolerance

    typedef FEFunctionSpace<ContinuousLagrangeMapper<double,Grid::LevelGridView> > HierarchicH1Space;
    typedef UniformQuantizationPolicy<VariableSet,Grid> QuantizationPolicy ;
    typedef LossyStorage<Grid,VariableSet,HierarchicH1Space,QuantizationPolicy> Storage ;

    QuantizationPolicy quantizationPolicy ;
    Storage lossyStorage( gridManager, variableSet, coarseLevel, qTol, true, quantizationPolicy ) ;

    // integrate equation
    Eq.time(0);
    VariableSet::VariableSet x(variableSet);
    Eq.scaleInitialValue<0>(InitialValue(0),x);

    x = integrate(gridManager,Eq,variableSet,spaces,gridManager.grid(),
		  dt,maxDT,T,maxSteps,rTolT,aTolT,rTolX,aTolX,extrapolOrder,
		  std::back_inserter(solutions),writeInterval,x,DirectType::UMFPACK,times,lossyStorage);

    // decode files & check error
    // decode backwards in time (necessary if temporal prediction/differential
    // encoding is used)
    std::cout << "\n\nckecking error:\n" ;

    VariableSet::VariableSet state_data(variableSet) ;
    L2Norm l2 ;

    std::ostringstream fn ;
    fn.width(3);  fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    for( int t = times.size()-1 ; t >= 0 ; t-- )
    {
      fn << "graph/quant" ;
      fn << t ;
      fn.flush();
      lossyStorage.decode( gridManager, state_data, fn.str() ) ;

      // write reconstructed states as vtu-files
//       writeVTKFile(gridManager.grid().leafView(), variableSet, state_data, fn.str());

      fn.clear() ; fn.str("");

      // compute and print L^\infty error
      state_data -= solutions[t] ;
      std::vector<double> foo( gridManager.grid().size(dim) ) ;
      state_data.write( foo.begin() ) ;
      double absQuantErr = fabs(*std::max_element(foo.begin(),foo.end(),abscompare)) ;
      std::cout << "t = " << times[t] << "\tL^\\infty = " << absQuantErr ;
      absQuantErr = sqrt(l2.square( boost::fusion::at_c<0>(state_data.data) ));
      std::cout << "\tL^2 = " << absQuantErr << "\n" ;
    }

    return 0;

//     std::cout << times.size() << " timesteps.\n" ;
//     for( int i = 0 ; i < times.size() ; i++ ) std::cout << times[i] << "   " ;
//     std::cout << "\n\n" ;
//     std::cout << "Solution vector contains " << solutions.size() << " entries.\n" ;
//
//     int windowSize = 2 ;
//
//     for( int i = 0 ; i < times.size()-windowSize ; i++ )
//     {
//
//       VariableSet::VariableSet a = solutions[i+1] , b = solutions[i+0] ;
//       a -= solutions[i+0] ; a *= 1.0/(times[i+1]-times[i+0]) ;
//
//       VariableSet::VariableSet pred = b ;
//       pred.axpy(times[i+2]-times[i+0],a);
//
//       typedef Grid::LeafGridView LeafGridView;
//       LeafGridView leafView = gridManager.grid().leafView();
//       std::ostringstream fn ;
//       fn.width(3);  fn.fill('0');
//       fn.setf(std::ios_base::right,std::ios_base::adjustfield);
//       fn << "graph/pred" ;
//       fn << i+2 ;
//       fn.flush();
//       writeVTKFile(leafView,variableSet,pred,fn.str() );
//
//       lossyStorage.encode( pred, fn.str() ) ;
//
//       fn.str("") ;
//       fn << "graph/diff" ;
//       fn << i+2 ;
//       fn.flush();
//       pred -= solutions[i+2] ;
//       writeVTKFile(leafView,variableSet,pred,fn.str());
//
//       lossyStorage.encode( pred, fn.str() ) ;
//     }
  }


/** @file */

#include "Transform.h"

#include <boost/regex.hpp>

#include <YukariCommon/LoggingService.h>
#include <YukariCommon/StringParsers.h>
#include <YukariMaths/Units.h>

using namespace Yukari::Common;
using namespace Yukari::Maths;

namespace Yukari
{
namespace Maths
{
  Eigen::IOFormat Transform::EIGEN_FORMAT(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",",
                                          "", "", "[", "]");

  Transform::Transform(const Eigen::Matrix4f &mat)
      : m_orientation(Eigen::Quaternionf::Identity())
      , m_position(mat.block(0, 3, 3, 1))
  {
    Eigen::Matrix3f rm = mat.block(0, 0, 3, 3).matrix();
    m_orientation = rm;
  }

  Transform::Transform(const Eigen::Quaternionf &orientation, const Eigen::Vector3f &position)
      : m_orientation(orientation)
      , m_position(position)
  {
  }

  Transform::Transform(const boost::program_options::variables_map &args,
                       const std::string &orientationName, const std::string &positionName)
      : m_orientation(Eigen::Quaternionf::Identity())
      , m_position(Eigen::Vector3f::Zero())
  {
    auto logger = LoggingService::Instance().getLogger("Transform");

    /* Parse orientation */
    if (args.count(orientationName))
    {
      std::string input(args[orientationName].as<std::string>());
      logger->debug("Parsing orientation string \"{}\"", input);

      if (!input.empty())
      {
        Eigen::Vector3f axis;
        float angle;

        boost::regex expression("(\\[.+\\])\\s*([\\-\\.\\w]+)");
        boost::cmatch what;
        if (boost::regex_match(input.c_str(), what, expression))
        {
          axis = StringParsers::ParseVector(what[1]);
          angle = std::stof(what[2]);
        }
        else
        {
          logger->error("Could not parse regex");
        }

        logger->debug("Axis: {}, Angle: {}", axis, angle);
        m_orientation = Eigen::AngleAxisf(angle * DEG_TO_RAD, axis);
      }
      else
      {
        logger->debug("Orientation parameter empty, ignoring");
      }
    }

    /* Parse position */
    if (args.count(positionName))
    {
      std::string input(args[positionName].as<std::string>());
      logger->debug("Parsing position string \"{}\"", input);

      if (!input.empty())
      {
        m_position = StringParsers::ParseVector(input);
      }
      else
      {
        logger->debug("Position parameter empty, ignoring");
      }
    }
  }

  Transform::Transform(const std::string &str)
      : m_orientation(Eigen::Quaternionf::Identity())
      , m_position(Eigen::Vector3f::Zero())
  {
    auto logger = LoggingService::Instance().getLogger("Transform");

    boost::regex expression("\\(\\s*o\\s*\\=\\s*(\\[[\\w\\.\\-\\,\\s]+\\])\\,\\s*p\\s*\\=\\s*(\\[["
                            "\\w\\.\\-\\,\\s]+\\])\\)");

    boost::cmatch what;
    if (boost::regex_match(str.c_str(), what, expression))
    {
      m_orientation = StringParsers::ParseQuaternion(what[1]);
      m_position = StringParsers::ParseVector(what[2]);
    }
    else
    {
      logger->error("Could not parse regex");
    }
  }

  Eigen::Matrix4f Transform::toEigen() const
  {
    Eigen::Matrix4f out = Eigen::Matrix4f::Identity();
    out.block(0, 0, 3, 3) = m_orientation.toRotationMatrix();
    out.block(0, 3, 3, 1) = m_position;
    return out;
  }

  std::ostream &operator<<(std::ostream &s, const Transform &t)
  {
    s << "(o=" << t.m_orientation.coeffs().format(Transform::EIGEN_FORMAT)
      << ", p=" << t.m_position.format(Transform::EIGEN_FORMAT) << ")";

    return s;
  }

  std::istream &operator>>(std::istream &s, Transform &t)
  {
    std::string str;
    std::getline(s, str, ')');
    str += ')';
    t = Transform(str);
    return s;
  }
}
}


//    boost atanh.hpp header file

//  (C) Copyright Hubert Holin 2001.
//  (C) Copyright John Maddock 2008.
//  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://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_ATANH_HPP
#define BOOST_ATANH_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/config/no_tr1/cmath.hpp>
#include <boost/config.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/log1p.hpp>

// This is the inverse of the hyperbolic tangent function.

namespace boost
{
    namespace math
    {
       namespace detail
       {
        // This is the main fare

        template<typename T, typename Policy>
        inline T    atanh_imp(const T x, const Policy& pol)
        {
            BOOST_MATH_STD_USING
            static const char* function = "boost::math::atanh<%1%>(%1%)";

            if(x < -1)
            {
               return policies::raise_domain_error<T>(
                  function,
                  "atanh requires x >= -1, but got x = %1%.", x, pol);
            }
            else if(x > 1)
            {
               return policies::raise_domain_error<T>(
                  function,
                  "atanh requires x <= 1, but got x = %1%.", x, pol);
            }
            else if((boost::math::isnan)(x))
            {
               return policies::raise_domain_error<T>(
                  function,
                  "atanh requires -1 <= x <= 1, but got x = %1%.", x, pol);
            }
            else if(x < -1 + tools::epsilon<T>())
            {
               // -Infinity:
               return -policies::raise_overflow_error<T>(function, 0, pol);
            }
            else if(x > 1 - tools::epsilon<T>())
            {
               // Infinity:
               return policies::raise_overflow_error<T>(function, 0, pol);
            }
            else if(abs(x) >= tools::forth_root_epsilon<T>())
            {
                // http://functions.wolfram.com/ElementaryFunctions/ArcTanh/02/
                if(abs(x) < 0.5f)
                   return (boost::math::log1p(x, pol) - boost::math::log1p(-x, pol)) / 2;
                return(log( (1 + x) / (1 - x) ) / 2);
            }
            else
            {
                // http://functions.wolfram.com/ElementaryFunctions/ArcTanh/06/01/03/01/
                // approximation by taylor series in x at 0 up to order 2
                T    result = x;

                if    (abs(x) >= tools::root_epsilon<T>())
                {
                    T    x3 = x*x*x;

                    // approximation by taylor series in x at 0 up to order 4
                    result += x3/static_cast<T>(3);
                }

                return(result);
            }
        }
       }

        template<typename T, typename Policy>
        inline typename tools::promote_args<T>::type atanh(T x, const Policy&)
        {
            typedef typename tools::promote_args<T>::type result_type;
            typedef typename policies::evaluation<result_type, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;
           return policies::checked_narrowing_cast<result_type, forwarding_policy>(
              detail::atanh_imp(static_cast<value_type>(x), forwarding_policy()),
              "boost::math::atanh<%1%>(%1%)");
        }
        template<typename T>
        inline typename tools::promote_args<T>::type atanh(T x)
        {
           return boost::math::atanh(x, policies::policy<>());
        }

    }
}

#endif /* BOOST_ATANH_HPP */


/*    Copyright (c) 2010-2016, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      160406    M. Van den Broeck Creation
 *      <YYMMDD>  <author name>     <comment>
 *
 *    References
 *      Vittaldev, V. (2010). The Unified State Model: Derivation and application in astrodynamics
 *          and navigation. Master's thesis, Delft University of Technology.
 *      <Second reference>
 *
 *    Notes
 *
 */

#include <cmath>

#include <boost/exception/all.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/missionGeometry.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unifiedStateModelElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"

namespace tudat
{

namespace orbital_element_conversions
{

//! Convert Keplerian elements to Unified State Model elements.
Eigen::Matrix< double, 7, 1 > convertKeplerianToUnifiedStateModelElements(
        const basic_mathematics::Vector6d& keplerianElements,
        const double centralBodyGravitationalParameter )
{
    using mathematical_constants::PI;

    // Declaring eventual output vector.
    Eigen::Matrix< double, 7, 1 > convertedUnifiedStateModelElements = Eigen::Matrix< double, 7, 1 >::Zero( );

    // Define the tolerance of a singularity
    double singularityTolerance = 1.0e-15; // Based on tolerance chosen in
                                           // orbitalElementConversions.cpp in Tudat Core.

    // If eccentricity is outside range [0,inf)
    if ( keplerianElements( eccentricityIndex ) < 0.0 )
    {
        //Define the error message
        std::stringstream errorMessage;
        errorMessage << "Eccentricity is expected in range [0,inf)\n"
                     << "Specified eccentricity: " << keplerianElements( eccentricityIndex ) << std::endl;

        // Throw exception
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If inclination is outside range [0,PI]
    if ( ( keplerianElements( inclinationIndex ) < 0.0 ) || ( keplerianElements( inclinationIndex ) > PI ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "Inclination is expected in range [0," << PI << "]\n"
                     << "Specified inclination: " << keplerianElements( inclinationIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If argument of pericenter is outside range [0,2.0 * PI]
    if ( ( keplerianElements( argumentOfPeriapsisIndex ) < 0.0 ) || ( keplerianElements( argumentOfPeriapsisIndex ) >
                                                                      2.0 * PI ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "RAAN is expected in range [0," << 2.0 * PI << "]\n"
                     << "Specified inclination: " << keplerianElements( argumentOfPeriapsisIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If right ascension of ascending node is outside range [0,2.0 * PI]
    if ( ( keplerianElements( longitudeOfAscendingNodeIndex ) < 0.0 ) ||
         ( keplerianElements( longitudeOfAscendingNodeIndex ) > 2.0 * PI ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "RAAN is expected in range [0," << 2.0 * PI << "]\n"
                     << "Specified inclination: " << keplerianElements( longitudeOfAscendingNodeIndex ) << " rad."
                     << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If true anomaly is outside range [0,2.0 * PI]
    if ( ( keplerianElements( trueAnomalyIndex ) < 0.0 ) || ( keplerianElements( trueAnomalyIndex ) > 2.0 * PI ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "RAAN is expected in range [0," << 2.0 * PI << "]\n"
                     << "Specified inclination: " << keplerianElements( trueAnomalyIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If inclination is zero and the right ascension of ascending node is non-zero
    if ( ( std::fabs( keplerianElements( inclinationIndex ) ) < singularityTolerance ) &&
         ( std::fabs( keplerianElements( longitudeOfAscendingNodeIndex ) ) > singularityTolerance ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "When the inclination is zero, the right ascending node should be zero by definition\n"
                     << "Specified right ascension of ascending node: " <<
                        keplerianElements( longitudeOfAscendingNodeIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If eccentricity is zero and the argument of pericenter is non-zero
    if ( ( std::fabs( keplerianElements( eccentricityIndex ) ) < singularityTolerance ) &&
         ( std::fabs( keplerianElements( argumentOfPeriapsisIndex ) ) > singularityTolerance ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "When the eccentricity is zero, the argument of pericenter should be zero by definition\n"
                     << "Specified argument of pericenter: " <<
                        keplerianElements( argumentOfPeriapsisIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If semi-major axis is negative and the eccentricity is smaller or equal to one
    if ( ( keplerianElements( semiMajorAxisIndex ) < 0.0 ) && ( keplerianElements( eccentricityIndex ) <= 1.0 ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "When the semi-major axis is negative, the eccentricity should be larger than one\n"
                     << "Specified semi-major axis: " << keplerianElements( semiMajorAxisIndex ) << " m.\n"
                     << "Specified eccentricity: " << keplerianElements( eccentricityIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }

    // If semi-major axis is positive and the eccentricity is larger than one
    if ( ( keplerianElements( semiMajorAxisIndex ) > 0.0 ) && ( keplerianElements( eccentricityIndex ) > 1.0 ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "When the semi-major axis is positive, the eccentricity should be smaller than or equal to one\n"
                     << "Specified semi-major axis: " << keplerianElements( semiMajorAxisIndex ) << " m.\n"
                     << "Specified eccentricity: " << keplerianElements( eccentricityIndex ) << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }
    //Else, nothing wrong and continue

    // Compute the C hodograph element of the Unified State Model
    if ( std::fabs( keplerianElements( eccentricityIndex ) - 1.0) < singularityTolerance )
            // parabolic orbit -> semi-major axis is not defined
    {
        convertedUnifiedStateModelElements( CHodographIndex ) =
                std::sqrt( centralBodyGravitationalParameter / keplerianElements( semiLatusRectumIndex ) );
    }
    else
    {
        convertedUnifiedStateModelElements( CHodographIndex ) =
                std::sqrt( centralBodyGravitationalParameter / ( keplerianElements( semiMajorAxisIndex )
                                                  * ( 1 - keplerianElements( eccentricityIndex ) *
                                                      keplerianElements( eccentricityIndex ) ) ) );
    }

    // Calculate the additional R hodograph parameter
    double RHodographElement = keplerianElements( eccentricityIndex ) *
            convertedUnifiedStateModelElements( CHodographIndex );

    // Compute the Rf1 hodograph element of the Unified State Model
    convertedUnifiedStateModelElements( Rf1HodographIndex ) =
            - RHodographElement * std::sin( keplerianElements( longitudeOfAscendingNodeIndex )
                                            + keplerianElements( argumentOfPeriapsisIndex ) );

    // Compute the Rf2 hodograph element of the Unified State Model
    convertedUnifiedStateModelElements( Rf2HodographIndex ) =
              RHodographElement * std::cos( keplerianElements( longitudeOfAscendingNodeIndex )
                                            + keplerianElements( argumentOfPeriapsisIndex ) );

    // Calculate the additional argument of longitude u
    double argumentOfLongitude = keplerianElements( argumentOfPeriapsisIndex ) +
            keplerianElements( trueAnomalyIndex );

    // Compute the epsilon1 quaternion of the Unified State Model
    convertedUnifiedStateModelElements( epsilon1QuaternionIndex ) =
            std::sin( 0.5 * keplerianElements( inclinationIndex ) ) *
            std::cos( 0.5 * ( keplerianElements( longitudeOfAscendingNodeIndex ) - argumentOfLongitude ) );

    // Compute the epsilon2 quaternion of the Unified State Model
    convertedUnifiedStateModelElements( epsilon2QuaternionIndex ) =
            std::sin( 0.5 * keplerianElements( inclinationIndex ) ) *
            std::sin( 0.5 * ( keplerianElements( longitudeOfAscendingNodeIndex ) - argumentOfLongitude ) );

    // Compute the epsilon3 quaternion of the Unified State Model
    convertedUnifiedStateModelElements( epsilon3QuaternionIndex ) =
            std::cos( 0.5 * keplerianElements( inclinationIndex ) ) *
            std::sin( 0.5 * ( keplerianElements( longitudeOfAscendingNodeIndex ) + argumentOfLongitude ) );

    // Compute the eta quaternion of the Unified State Model
    convertedUnifiedStateModelElements( etaQuaternionIndex ) =
            std::cos( 0.5 * keplerianElements( inclinationIndex ) ) *
            std::cos( 0.5 * ( keplerianElements( longitudeOfAscendingNodeIndex ) + argumentOfLongitude ) );

    // Give back result
    return convertedUnifiedStateModelElements;

}

//! Convert Unified State Model elements to Keplerian elements.
basic_mathematics::Vector6d convertUnifiedStateModelToKeplerianElements(
        const Eigen::Matrix< double, 7, 1 >& unifiedStateModelElements,
        const double centralBodyGravitationalParameter )
{
    using mathematical_constants::PI;

    // Declaring eventual output vector.
    basic_mathematics::Vector6d convertedKeplerianElements = basic_mathematics::
            Vector6d::Zero( 6 );

    // Define the tolerance of a singularity
    double singularityTolerance = 1.0e-15; // Based on tolerance chosen in
                                           // orbitalElementConversions.cpp in Tudat Core.

    // Declare auxiliary parameters before using them in the if loop
    double cosineLambda = 0.0;
    double sineLambda = 0.0;
    double lambdaFromSineAndCosine = 0.0;

    // Check whether the Unified State Model elements are within expected limits
    // If inclination is zero and the right ascension of ascending node is non-zero
    const double normOfQuaternionElements = std::sqrt( std::pow( unifiedStateModelElements( epsilon1QuaternionIndex ), 2 ) +
                                                       std::pow( unifiedStateModelElements( epsilon2QuaternionIndex ), 2 ) +
                                                       std::pow( unifiedStateModelElements( epsilon3QuaternionIndex ), 2 ) +
                                                       std::pow( unifiedStateModelElements( etaQuaternionIndex ), 2 ) );

    if ( std::fabs( normOfQuaternionElements - 1.0 ) > singularityTolerance )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "The norm of the quaternion should be equal to one.\n"
                     << "Norm of the specified quaternion is: " << normOfQuaternionElements << " ." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }
    //Else, nothing wrong and continue

    // Compute auxiliary parameters cosineLambda, sineLambda and Lambda
    if ( ( std::fabs( unifiedStateModelElements( epsilon3QuaternionIndex ) ) < singularityTolerance )
        && ( std::fabs( unifiedStateModelElements( etaQuaternionIndex ) ) < singularityTolerance ) )
            // pure-retrograde orbit -> inclination  = pi
    {
        //Define the error message
        std::stringstream errorMessage;
        errorMessage << "Pure-retrograde orbit (inclination = pi).\n"
                     << "Unified State Model elements cannot be transformed to Kepler elements." << std::endl;

        // Throw exception
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }
    else
    {
        cosineLambda = ( unifiedStateModelElements( etaQuaternionIndex ) *
                                unifiedStateModelElements( etaQuaternionIndex ) -
                                unifiedStateModelElements( epsilon3QuaternionIndex ) *
                                unifiedStateModelElements( epsilon3QuaternionIndex ) )
                / ( unifiedStateModelElements( epsilon3QuaternionIndex ) *
                    unifiedStateModelElements( epsilon3QuaternionIndex ) +
                    unifiedStateModelElements( etaQuaternionIndex ) *
                    unifiedStateModelElements( etaQuaternionIndex ) );
        sineLambda = ( 2.0 *
                              unifiedStateModelElements( epsilon3QuaternionIndex ) *
                              unifiedStateModelElements( etaQuaternionIndex ) )
                / ( unifiedStateModelElements( epsilon3QuaternionIndex ) *
                    unifiedStateModelElements( epsilon3QuaternionIndex ) +
                    unifiedStateModelElements( etaQuaternionIndex ) *
                    unifiedStateModelElements( etaQuaternionIndex ) );
        lambdaFromSineAndCosine = std::atan2( sineLambda, cosineLambda );
    }

    // Compute auxiliary parameters auxiliaryParameter1 and auxiliaryParameter2
    double auxiliaryParameter1 = unifiedStateModelElements( Rf1HodographIndex ) * cosineLambda +
            unifiedStateModelElements( Rf2HodographIndex ) * sineLambda;
    double auxiliaryParameter2 = unifiedStateModelElements( CHodographIndex ) -
            unifiedStateModelElements( Rf1HodographIndex ) * sineLambda +
            unifiedStateModelElements( Rf2HodographIndex ) * cosineLambda;

    // Compute auxiliary R hodograph parameter
    double RHodographElement = std::sqrt( unifiedStateModelElements( Rf1HodographIndex )
                                          * unifiedStateModelElements( Rf1HodographIndex )
                                          + unifiedStateModelElements( Rf2HodographIndex )
                                          * unifiedStateModelElements( Rf2HodographIndex ));

    // Compute eccentricity
    convertedKeplerianElements( eccentricityIndex ) =
            RHodographElement / unifiedStateModelElements( CHodographIndex );

    // Compute semi-major axis or, in case of a parabolic orbit, the semi-latus rectum.
    if ( std::fabs( convertedKeplerianElements( eccentricityIndex ) - 1.0 ) < singularityTolerance )
            // parabolic orbit -> semi-major axis is not defined. Use semi-latus rectum instead.
    {
        convertedKeplerianElements( semiLatusRectumIndex ) = centralBodyGravitationalParameter /
                ( unifiedStateModelElements( CHodographIndex ) * unifiedStateModelElements( CHodographIndex ) );
    }
    else
    {
        convertedKeplerianElements( semiMajorAxisIndex ) =
                centralBodyGravitationalParameter /
                ( 2.0 * unifiedStateModelElements( CHodographIndex ) * auxiliaryParameter2 -
                    ( auxiliaryParameter1 * auxiliaryParameter1 + auxiliaryParameter2 * auxiliaryParameter2 ) );
    }

    // Compute inclination
    convertedKeplerianElements( inclinationIndex ) =
            std::acos( 1.0 - 2.0 * ( unifiedStateModelElements( epsilon1QuaternionIndex ) *
                                     unifiedStateModelElements( epsilon1QuaternionIndex ) +
                                     unifiedStateModelElements( epsilon2QuaternionIndex ) *
                                     unifiedStateModelElements( epsilon2QuaternionIndex ) ) );
        // This acos is always defined correctly because the inclination is always below pi rad.

    // Compute longitude of ascending node
    if ( ( ( std::fabs( unifiedStateModelElements( epsilon1QuaternionIndex ) ) < singularityTolerance )
           && ( std::fabs( unifiedStateModelElements( epsilon2QuaternionIndex ) ) < singularityTolerance ) ) ||
         ( ( std::fabs( unifiedStateModelElements( epsilon3QuaternionIndex ) ) < singularityTolerance )
         && ( std::fabs( unifiedStateModelElements( etaQuaternionIndex ) ) < singularityTolerance ) ) )
            // pure-prograde or pure-retrograde orbit
    {
        convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0; // by definition
    }
    else
    {
        convertedKeplerianElements( longitudeOfAscendingNodeIndex ) =
                std::atan2( ( ( unifiedStateModelElements( epsilon1QuaternionIndex ) *
                                unifiedStateModelElements( epsilon3QuaternionIndex ) +
                                unifiedStateModelElements( epsilon2QuaternionIndex ) *
                                unifiedStateModelElements( etaQuaternionIndex ) )
                           / ( std::sqrt( ( unifiedStateModelElements( epsilon1QuaternionIndex ) *
                                               unifiedStateModelElements( epsilon1QuaternionIndex ) +
                                               unifiedStateModelElements( epsilon2QuaternionIndex ) *
                                               unifiedStateModelElements( epsilon2QuaternionIndex ) ) *
                                          ( unifiedStateModelElements( etaQuaternionIndex ) *
                                               unifiedStateModelElements( etaQuaternionIndex ) +
                                               unifiedStateModelElements( epsilon3QuaternionIndex ) *
                                               unifiedStateModelElements( epsilon3QuaternionIndex ) ) ) ) ),
                            ( ( unifiedStateModelElements( epsilon1QuaternionIndex ) *
                             unifiedStateModelElements( etaQuaternionIndex ) -
                             unifiedStateModelElements( epsilon2QuaternionIndex ) *
                             unifiedStateModelElements( epsilon3QuaternionIndex ) )
                        / ( std::sqrt( ( unifiedStateModelElements( epsilon1QuaternionIndex ) *
                                            unifiedStateModelElements( epsilon1QuaternionIndex ) +
                                            unifiedStateModelElements( epsilon2QuaternionIndex ) *
                                            unifiedStateModelElements( epsilon2QuaternionIndex ) ) *
                                       ( unifiedStateModelElements( etaQuaternionIndex ) *
                                            unifiedStateModelElements( etaQuaternionIndex ) +
                                            unifiedStateModelElements( epsilon3QuaternionIndex ) *
                                            unifiedStateModelElements( epsilon3QuaternionIndex ) ) ) ) ) );

        // Round off small values of the right ascension of ascending node to zero
        if ( std::fabs( convertedKeplerianElements( longitudeOfAscendingNodeIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0;
        }
        // Ensure the longitude of ascending node is positive
        while ( convertedKeplerianElements( longitudeOfAscendingNodeIndex ) < 0.0 )
                // Because of the previous if loop, if the longitude of ascending node is smaller than 0, it will
                // always be smaller than -singularityTolerance
        {
            convertedKeplerianElements( longitudeOfAscendingNodeIndex ) =
                    convertedKeplerianElements( longitudeOfAscendingNodeIndex ) + 2.0 * PI;
        }
    }

    // Compute true anomaly and argument of periapsis
    if ( std::fabs( RHodographElement ) < singularityTolerance ) // circular orbit
    {
        convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0; // by definition
        convertedKeplerianElements( trueAnomalyIndex ) =
                lambdaFromSineAndCosine - convertedKeplerianElements( longitudeOfAscendingNodeIndex );

        // Round off small theta to zero
        if ( std::fabs( convertedKeplerianElements( trueAnomalyIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( trueAnomalyIndex ) = 0.0;
        }

        // Ensure the true anomaly is positive
        while ( convertedKeplerianElements( trueAnomalyIndex ) < 0.0 )
                // Because of the previous if loop, if the true anomaly is smaller than zero, it will always be smaller than
                // -singularityTolerance
        {
            convertedKeplerianElements( trueAnomalyIndex ) =
                    convertedKeplerianElements( trueAnomalyIndex ) + 2.0 * PI;
        }
    }
    else
    {
        convertedKeplerianElements( trueAnomalyIndex ) =
                std::atan2( ( auxiliaryParameter1 / RHodographElement ),
                            ( ( auxiliaryParameter2 - unifiedStateModelElements( CHodographIndex ) )
                           / RHodographElement ) );

        // Round off small theta to zero
        if ( std::fabs( convertedKeplerianElements( trueAnomalyIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( trueAnomalyIndex ) = 0.0;
        }

        // Ensure the true anomaly is positive
        while ( convertedKeplerianElements( trueAnomalyIndex ) < 0.0 )
            // Because of the previous if loop, if the true anomaly is smaller than zero, it will always
            // be smaller than -singularityTolerance
        {
            convertedKeplerianElements( trueAnomalyIndex ) =
                    convertedKeplerianElements( trueAnomalyIndex ) + 2.0 * PI;
        }

        convertedKeplerianElements( argumentOfPeriapsisIndex ) =
                lambdaFromSineAndCosine -
                convertedKeplerianElements( longitudeOfAscendingNodeIndex ) -
                convertedKeplerianElements( trueAnomalyIndex );

        // Round off small omega to zero
        if ( std::fabs( convertedKeplerianElements( argumentOfPeriapsisIndex ) ) < singularityTolerance )
        {
            convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0;
        }

        // Ensure the argument of periapsis is positive
        while ( convertedKeplerianElements( argumentOfPeriapsisIndex ) < 0.0 )
            // Because of the previous if loop, if the argument of pericenter is smaller than zero,
            // it will be smaller than -singularityTolerance
        {
            convertedKeplerianElements( argumentOfPeriapsisIndex ) =
                    convertedKeplerianElements( argumentOfPeriapsisIndex ) + 2.0 * PI;
        }
    }

    // Give back result
    return convertedKeplerianElements;
}

} // close namespace orbital_element_conversions

} // close namespace tudat