/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/car/design-variables/OdometryDesignVariables.h"

#include <boost/make_shared.hpp>

#include <sm/kinematics/EulerAnglesYawPitchRoll.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>

#include <sm/PropertyTree.hpp>

#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/RotationQuaternion.hpp>
#include <aslam/backend/Scalar.hpp>
#include <aslam/backend/GenericScalar.hpp>
#include <aslam/backend/FixedPointNumber.hpp>

#include <aslam/calibration/data-structures/VectorDesignVariable.h>
#include <aslam/calibration/core/OptimizationProblem.h>

using namespace sm;
using namespace sm::kinematics;
using namespace aslam::backend;

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    OdometryDesignVariables::OdometryDesignVariables(const PropertyTree&
        config) {
      e_r = boost::make_shared<Scalar>(config.getDouble(
        "intrinsics/halfRearTrack"));
      e_r->setActive(true);
      e_f = boost::make_shared<Scalar>(config.getDouble(
        "intrinsics/halfFrontTrack"));
      e_f->setActive(true);
      L = boost::make_shared<Scalar>(config.getDouble(
        "intrinsics/wheelBase"));
      L->setActive(true);

      a = boost::make_shared<VectorDesignVariable<4> >(
        (VectorDesignVariable<4>::Container() <<
        config.getDouble("intrinsics/steeringCoefficient0"),
        config.getDouble("intrinsics/steeringCoefficient1"),
        config.getDouble("intrinsics/steeringCoefficient2"),
        config.getDouble("intrinsics/steeringCoefficient3")
        ).finished());
      a->setActive(true);

      k_rl = boost::make_shared<Scalar>(config.getDouble(
        "intrinsics/rlwCoefficient"));
      k_rl->setActive(true);
      k_rr = boost::make_shared<Scalar>(config.getDouble(
        "intrinsics/rrwCoefficient"));
      k_rr->setActive(true);
      k_fl = boost::make_shared<Scalar>(config.getDouble(
        "intrinsics/flwCoefficient"));
      k_fl->setActive(true);
      k_fr = boost::make_shared<Scalar>(config.getDouble(
        "intrinsics/frwCoefficient"));
      k_fr->setActive(true);
      k_dmi = boost::make_shared<Scalar>(config.getDouble(
        "intrinsics/dmiCoefficient"));
      k_dmi->setActive(true);

      v_r_vr = boost::make_shared<EuclideanPoint>(Eigen::Vector3d(
        config.getDouble("extrinsics/translation/x"),
        config.getDouble("extrinsics/translation/y"),
        config.getDouble("extrinsics/translation/z")));
      v_r_vr->setActive(false);
      const EulerAnglesYawPitchRoll ypr;
      v_R_r = boost::make_shared<RotationQuaternion>(
        ypr.parametersToRotationMatrix(
        Eigen::Vector3d(config.getDouble("extrinsics/rotation/yaw"),
        config.getDouble("extrinsics/rotation/pitch"),
        config.getDouble("extrinsics/rotation/roll"))));
      v_R_r->setActive(false);

      t_r = boost::make_shared<TimeDesignVariable>(
        config.getInt("timeDelays/rearWheels"));
      t_f = boost::make_shared<TimeDesignVariable>(
        config.getInt("timeDelays/frontWheels"));
      t_s = boost::make_shared<TimeDesignVariable>(
        config.getInt("timeDelays/steering"));
      t_dmi = boost::make_shared<TimeDesignVariable>(
        config.getInt("timeDelays/dmi"));
      if (config.getBool("timeDelays/active")) {
        t_r->setActive(true);
        t_f->setActive(true);
        t_s->setActive(true);
        t_dmi->setActive(true);
      }
      else {
        t_r->setActive(false);
        t_f->setActive(false);
        t_s->setActive(false);
        t_dmi->setActive(false);
      }
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    void OdometryDesignVariables::addToBatch(const BatchSP& batch, size_t
        groupId) {
      batch->addDesignVariable(e_r, groupId);
      batch->addDesignVariable(e_f, groupId);
      batch->addDesignVariable(L, groupId);
      batch->addDesignVariable(a, groupId);
      batch->addDesignVariable(k_rl, groupId);
      batch->addDesignVariable(k_rr, groupId);
      batch->addDesignVariable(k_fl, groupId);
      batch->addDesignVariable(k_fr, groupId);
      batch->addDesignVariable(k_dmi, groupId);
      batch->addDesignVariable(v_r_vr, groupId);
      batch->addDesignVariable(v_R_r, groupId);
      batch->addDesignVariable(t_r, groupId);
      batch->addDesignVariable(t_f, groupId);
      batch->addDesignVariable(t_s, groupId);
      batch->addDesignVariable(t_dmi, groupId);
    }

    Eigen::VectorXd OdometryDesignVariables::getParameters() const {
      Eigen::VectorXd params(22);
      params(0) = e_r->getParameters()(0);
      params(1) = e_f->getParameters()(0);
      params(2) = L->getParameters()(0);
      params(3) = k_rl->getParameters()(0);
      params(4) = k_rr->getParameters()(0);
      params(5) = k_fl->getParameters()(0);
      params(6) = k_fr->getParameters()(0);
      params(7) = k_dmi->getParameters()(0);
      params.segment<4>(8) = a->getValue();
      params.segment<3>(12) = v_r_vr->toEuclidean();
      const EulerAnglesYawPitchRoll ypr;
      params.segment<3>(15) = ypr.rotationMatrixToParameters(
        quat2r(v_R_r->getQuaternion()));
      params(18) = t_r->getParameters()(0);
      params(19) = t_f->getParameters()(0);
      params(20) = t_s->getParameters()(0);
      params(11) = t_dmi->getParameters()(0);
      return params;
    }

/******************************************************************************/
/* Stream methods                                                             */
/******************************************************************************/

    void OdometryDesignVariables::read(std::istream& stream) {
    }

    void OdometryDesignVariables::write(std::ostream& stream) const {
      stream << "e_r: " << e_r->getParameters() << std::endl;
      stream << "e_f: " << e_f->getParameters() << std::endl;
      stream << "L: " << L->getParameters() << std::endl;
      stream << "a: " << *a << std::endl;
      stream << "k_rl: " << k_rl->getParameters() << std::endl;
      stream << "k_rr: " << k_rr->getParameters() << std::endl;
      stream << "k_fl: " << k_fl->getParameters() << std::endl;
      stream << "k_fr: " << k_fr->getParameters() << std::endl;
      stream << "k_dmi: " << k_dmi->getParameters() << std::endl;
      stream << "v_r_vr: " << v_r_vr->toEuclidean().transpose() << std::endl;
      const EulerAnglesYawPitchRoll ypr;
      stream << "v_R_r: " << ypr.rotationMatrixToParameters(
        quat2r(v_R_r->getQuaternion())).transpose() << std::endl;
      stream << "t_r: " << t_r->getParameters() << std::endl;
      stream << "t_f: " << t_f->getParameters() << std::endl;
      stream << "t_s: " << t_s->getParameters() << std::endl;
      stream << "t_dmi: " << t_dmi->getParameters() << std::endl;
    }

    void OdometryDesignVariables::read(std::ifstream& stream) {
    }

    void OdometryDesignVariables::write(std::ofstream& stream) const {
    }

  }
}


//----------------------------------------------------------------------------
/** @file GoGtpExtraCommands.cpp
*/
//----------------------------------------------------------------------------

#include "SgSystem.h"
#include "GoGtpExtraCommands.h"

#include <limits>
#include <boost/format.hpp>
#include "GoBoard.h"
#include "GoBoardUtil.h"
#include "GoGtpCommandUtil.h"
#include "GoLadder.h"
#include "GoStaticLadder.h"

using namespace std;
using boost::format;
using GoGtpCommandUtil::PointArg;
using GoGtpCommandUtil::StoneArg;

//----------------------------------------------------------------------------

GoGtpExtraCommands::GoGtpExtraCommands(GoBoard& bd)
    : m_bd(bd)
{
}

void GoGtpExtraCommands::AddGoGuiAnalyzeCommands(GtpCommand& cmd)
{
    cmd <<
        "sboard/Go CFG Distance/go_cfg_distance %p\n"
        "sboard/Go CFG Distance N/go_cfg_distance %p %s\n"
        "string/Go Ladder/go_ladder %p\n"
        "string/Go Static Ladder/go_static_ladder %p\n";
}

/** Compute the distance from a point as defined in GoBoardUtil::CfgDistance.
    This command is compatible with GoGui's analyze command type
    @c sboard

    Argument: point [max_dist]

    Returns: Board of integer numbers
*/
void GoGtpExtraCommands::CmdCfgDistance(GtpCommand& cmd)
{
    cmd.CheckNuArgLessEqual(2);
    SgPoint p = PointArg(cmd, 0, m_bd);
    int maxDist = numeric_limits<int>::max();
    if (cmd.NuArg() > 1)
        maxDist = cmd.IntArg(1, 0);
    SgPointArray<int> distance = GoBoardUtil::CfgDistance(m_bd, p, maxDist);
    // distance elements are only defined for empty points or block anchors
    SgPointArray<string> stringArray("\"\"");
    for (GoBoard::Iterator it(m_bd); it; ++it)
        if (m_bd.IsEmpty(*it) || m_bd.Anchor(*it) == *it)
            stringArray[*it] = str(format("%i") % distance[*it]);
    cmd << '\n' << SgWritePointArray<string>(stringArray, m_bd.Size());
}

/** Return fast ladder status.
    Arguments: prey point

    Returns: escaped|captured|unsettled

    @see FastLadderStatus
*/
void GoGtpExtraCommands::CmdLadder(GtpCommand& cmd)
{
    cmd.CheckNuArg(1);
    SgPoint prey = StoneArg(cmd, 0, m_bd);
    GoLadderStatus status = GoLadderUtil::LadderStatus(m_bd, prey);
    switch (status)
    {
    case GO_LADDER_ESCAPED:
        cmd << "escaped";
        break;
    case GO_LADDER_CAPTURED:
        cmd << "captured";
        break;
    case GO_LADDER_UNSETTLED:
        cmd << "unsettled";
        break;
    default:
        throw GtpFailure() << "Unexpected ladder status: " << status;
    }
}

/** Return static ladder status.
    Arguments: prey point

    Returns: escaped|captured|unsettled

    @see GoStaticLadder
*/
void GoGtpExtraCommands::CmdStaticLadder(GtpCommand& cmd)
{
    cmd.CheckNuArg(1);
    SgPoint p = StoneArg(cmd, 0, m_bd);
    SgBlackWhite c = m_bd.GetColor(p);
    if (GoStaticLadder::IsLadder(m_bd, p, c))
        cmd << "captured";
    else if (GoStaticLadder::IsLadder(m_bd, p, SgOppBW(c)))
        cmd << "unsettled";
    else
        cmd << "escaped";
}

void GoGtpExtraCommands::Register(GtpEngine& e)
{
    Register(e, "go_cfg_distance", &GoGtpExtraCommands::CmdCfgDistance);
    Register(e, "go_ladder", &GoGtpExtraCommands::CmdLadder);
    Register(e, "go_static_ladder", &GoGtpExtraCommands::CmdStaticLadder);
}

void GoGtpExtraCommands::Register(GtpEngine& engine,
                               const std::string& command,
                               GtpCallback<GoGtpExtraCommands>::Method method)
{
    engine.Register(command,
                    new GtpCallback<GoGtpExtraCommands>(this, method));
}

//----------------------------------------------------------------------------


//=======================================================================
// Copyright (c) 2015
//
// 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)
//=======================================================================
/**
 * @file frequent_directions_long_test.cpp
 * @brief
 * @author Tomasz Strozak
 * @version 1.0
 * @date 2015-04-19
 */

#include "test_utils/get_test_dir.hpp"
#include "test_utils/sketch_accuracy_check.hpp"
#include "test_utils/system.hpp"

#include "paal/sketch/frequent_directions.hpp"
#include "paal/utils/irange.hpp"
#include "paal/utils/parse_file.hpp"
#include "paal/utils/read_rows.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/test/unit_test.hpp>

#include <cassert>
#include <istream>
#include <random>
#include <string>

using coordinate_t = double;
using distribution_t = std::uniform_int_distribution<std::size_t>;
using matrix_t = boost::numeric::ublas::matrix<coordinate_t>;
using row_t = std::vector<coordinate_t>;
using vector_row_t = std::vector<row_t>;

namespace {
    std::default_random_engine generator;
    std::size_t MAX_ROWS = 2000;
    coordinate_t EPS = 1e-7;
}

auto generate_random_matrix(std::size_t rows_count, std::size_t columns_count) {
    matrix_t matrix(rows_count, columns_count);

    std::uniform_real_distribution<coordinate_t> distribution(0,1);

    for (auto i : paal::irange(rows_count)) {
        for (auto j : paal::irange(columns_count)) {
            matrix(i,j) = distribution(generator);
        }
    }

    return matrix;
}

auto generate_matrix_from_row_buffer(vector_row_t const &row_buffer) {
    matrix_t matrix(boost::size(row_buffer), boost::size(row_buffer.front()));

    auto row = row_buffer.begin();
    auto row_matrix = matrix.begin1();
    for (; row != row_buffer.end() && row_matrix != matrix.end1(); ++row, ++row_matrix) {
        std::copy(row->begin(), row->end(), row_matrix.begin());
    }

    return matrix;
}

BOOST_AUTO_TEST_CASE(frequent_directions_random) {
    int test_count = 10;
    std::size_t const min_rows = 10;
    std::size_t const max_rows = 5000;
    std::size_t const min_columns = 10;
    std::size_t const max_columns = 500;
    std::size_t const min_sketch_size = 1;
    distribution_t rows_distribution(min_rows, max_rows);
    distribution_t columns_distribution(min_columns, max_columns);

    while (test_count--) {
        auto rows_count = rows_distribution(generator);
        auto columns_count = columns_distribution(generator);
        distribution_t sketch_size_distribution(min_sketch_size, rows_count/10);
        auto sketch_size = sketch_size_distribution(generator);

        auto fd_sketch = paal::make_frequent_directions<coordinate_t>(sketch_size, columns_count);
        auto data = generate_random_matrix(rows_count, columns_count);
        fd_sketch.update(std::move(data));
        fd_sketch.compress();

        auto sketch = fd_sketch.get_sketch().first;
        check_frequent_directions(std::move(data), std::move(sketch), EPS);
    }
}

BOOST_AUTO_TEST_CASE(frequent_directions_long) {
    std::string test_dir = paal::system::get_test_data_dir("SKETCH");

    using paal::system::build_path;

    paal::parse(build_path(test_dir, "cases.txt"), [&](std::string const & fname,
                std::istream & is_test_cases) {
        std::ifstream ifs(build_path(test_dir, "/cases/" + fname + ".txt"));
        assert(ifs.good());

        std::vector<std::vector<coordinate_t>> row_buffer;
        row_buffer.reserve(MAX_ROWS);
        paal::read_rows_first_row_size<coordinate_t>(ifs, row_buffer, MAX_ROWS);

        auto data = generate_matrix_from_row_buffer(row_buffer);

        std::size_t sketch_size;
        is_test_cases >> sketch_size;
        auto fd_sketch = paal::make_frequent_directions<coordinate_t>(sketch_size, data.size2());
        fd_sketch.update(data);
        fd_sketch.compress();

        auto sketch = fd_sketch.get_sketch().first;
        check_frequent_directions(std::move(data), std::move(sketch), EPS);
    });
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersFixture.hpp"
#include "../TfLiteParser.hpp"
#include <sstream>

BOOST_AUTO_TEST_SUITE(TensorflowLiteParser)

struct SimpleConv2DFixture : public ParserFlatbuffersFixture
{
    explicit SimpleConv2DFixture()
    {
        m_JsonString = R"(
            {
                "version": 3,
                "operator_codes": [ { "builtin_code": "CONV_2D" } ],
                "subgraphs": [ {
                    "tensors": [
                        {
                            "shape": [ 1, 3, 3, 1 ],
                            "type": "UINT8",
                            "buffer": 0,
                            "name": "inputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        },
                        {
                            "shape": [ 1, 1, 1, 1 ],
                            "type": "UINT8",
                            "buffer": 1,
                            "name": "outputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 511.0 ],
                                "scale": [ 2.0 ],
                                "zero_point": [ 0 ],
                            }
                        },
                        {
                            "shape": [ 1, 3, 3, 1 ],
                            "type": "UINT8",
                            "buffer": 2,
                            "name": "filterTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        }
                    ],
                    "inputs": [ 0 ],
                    "outputs": [ 1 ],
                    "operators": [
                        {
                            "opcode_index": 0,
                            "inputs": [ 0, 2 ],
                            "outputs": [ 1 ],
                            "builtin_options_type": "Conv2DOptions",
                            "builtin_options": {
                                "padding": "VALID",
                                "stride_w": 1,
                                "stride_h": 1,
                                "fused_activation_function": "NONE"
                            },
                            "custom_options_format": "FLEXBUFFERS"
                        }
                    ],
                } ],
                "buffers" : [
                    { },
                    { },
                    { "data": [ 2,1,0,  6,2,1, 4,1,2 ], },
                    { },
                ]
            }
        )";
        SetupSingleInputSingleOutput("inputTensor", "outputTensor");
    }
};

BOOST_FIXTURE_TEST_CASE( ParseSimpleConv2D, SimpleConv2DFixture )
{
    RunTest<4, uint8_t>(
        0,
        {
            1, 2, 3,
            4, 5, 6,
            7, 8, 9,
        },
        // because of the output scaling we need to take half of the values
        {
            (1*2 + 2*1 + 3*0 +
             4*6 + 5*2 + 6*1 +
             7*4 + 8*1 + 9*2) /2
        });
}

struct Conv2DWithBiasesFixture : public ParserFlatbuffersFixture
{
    explicit Conv2DWithBiasesFixture(const std::string & inputShape,
                                     const std::string & outputShape,
                                     const std::string & filterShape,
                                     const std::string & filterData,
                                     const std::string & biasShape,
                                     const std::string & biasData,
                                     const std::string & strides,
                                     const std::string & activation="NONE",
                                     const std::string & filterScale="1.0",
                                     const std::string & filterZeroPoint="0",
                                     const std::string & outputScale="2.0",
                                     const std::string & outputZeroPoint="0")
    {
        m_JsonString = R"(
            {
                "version": 3,
                "operator_codes": [ { "builtin_code": "CONV_2D" } ],
                "subgraphs": [ {
                    "tensors": [
                        {
                            "shape": )" + inputShape + R"(,
                            "type": "UINT8",
                            "buffer": 0,
                            "name": "inputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        },
                        {
                            "shape": )" + outputShape + R"(,
                            "type": "UINT8",
                            "buffer": 1,
                            "name": "outputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 511.0 ],
                                "scale": [ )" + outputScale + R"( ],
                                "zero_point": [ )" + outputZeroPoint + R"( ],
                            }
                        },
                        {
                            "shape": )" + filterShape + R"( ,
                            "type": "UINT8",
                            "buffer": 2,
                            "name": "filterTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ )" + filterScale + R"( ],
                                "zero_point": [ )" + filterZeroPoint + R"( ],
                            }
                        },
                        {
                            "shape": )" + biasShape + R"( ,
                            "type": "INT32",
                            "buffer": 3,
                            "name": "biasTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        }
                    ],
                    "inputs": [ 0 ],
                    "outputs": [ 1 ],
                    "operators": [
                        {
                            "opcode_index": 0,
                            "inputs": [ 0, 2, 3 ],
                            "outputs": [ 1 ],
                            "builtin_options_type": "Conv2DOptions",
                            "builtin_options": {
                                "padding": "SAME",
                                "stride_w": )" + strides + R"(,
                                "stride_h": )" + strides + R"(,
                                "fused_activation_function": )" + activation + R"(
                            },
                            "custom_options_format": "FLEXBUFFERS"
                        }
                    ],
                } ],
                "buffers" : [
                    { },
                    { },
                    { "data": )" + filterData + R"(, },
                    { "data": )" + biasData + R"(, },
                ]
            }
        )";
        SetupSingleInputSingleOutput("inputTensor", "outputTensor");
    }
};

struct SimpleConv2DWithBiasesFixture : Conv2DWithBiasesFixture
{
    SimpleConv2DWithBiasesFixture()
    : Conv2DWithBiasesFixture("[ 1, 2, 2, 1 ]",    // inputShape
                              "[ 1, 2, 2, 1 ]",    // outputShape
                              "[ 1, 2, 2, 1 ]",    // filterShape
                              "[ 2,1, 0,6 ]",      // filterData
                              "[ 1 ]",             // biasShape
                              "[ 10, 0, 0, 0 ]",   // biasData
                              "1")                 // stride w and h
    {}
};

BOOST_FIXTURE_TEST_CASE( ParseConv2DWithBias, SimpleConv2DWithBiasesFixture )
{
    RunTest<4, uint8_t>(
        0,
        {
            1, 2,
            3, 4,
        },
        // because of the output scaling we need to take half of the values
        {
            (1*2 + 2*1 + 3*0 + 4*6 + 10)/2,
            (2*2 + 0*1 + 4*0 + 0*6 + 10)/2,
            (3*2 + 4*1 + 0*0 + 0*6 + 10)/2,
            (4*2 + 0*1 + 0*0 + 0*6 + 10)/2
        });
}

struct Conv2DShapeTestFixture : Conv2DWithBiasesFixture
{
    static std::string GenerateInts(unsigned int n)
    {
        std::stringstream ss;
        ss << " [ ";
        for( unsigned int i=0; i<n; ++i ) {
            if (i > 0 )
            {
                ss << " , ";
            }
            ss << " " << (i%256);
        }
        ss << " ] ";
        return ss.str();
    }

    Conv2DShapeTestFixture()
    : Conv2DWithBiasesFixture("[ 1, 224, 224, 3 ]",    // inputShape
                              "[ 1, 112, 112, 32 ]",   // outputShape
                              "[ 32, 3, 3, 3 ]",       // filterShape
                              GenerateInts(32*3*3*3),  // filterData
                              "[ 32 ]",                // biasShape
                              GenerateInts(32*4),      // biasData
                              "2")                     // stride w and h
    {}
};

BOOST_FIXTURE_TEST_CASE( ParseConv2D_112x112_out, Conv2DShapeTestFixture )
{
}

struct ReluConv2DWithBiasesFixture : Conv2DWithBiasesFixture
{
    ReluConv2DWithBiasesFixture()
    : Conv2DWithBiasesFixture("[ 1, 2, 2, 1 ]",    // inputShape
                              "[ 1, 2, 2, 1 ]",    // outputShape
                              "[ 1, 2, 2, 1 ]",    // filterShape
                              "[ 2,1, 0,6 ]",      // filterData
                              "[ 1 ]",             // biasShape
                              "[ 16, 0, 0, 0 ]",   // biasData
                              "1",                 // stride w and h
                              "RELU",              // activation
                              "1.0",               // filter scale
                              "4",                 // filter zero point
                              "2.0",               // output scale
                              "20")                // output zero point
    {}
};

BOOST_FIXTURE_TEST_CASE( ParseConv2DAndReluWithBias, ReluConv2DWithBiasesFixture )
{
    uint8_t bias = 16;
    uint8_t outZero = 20;
    uint8_t fz = 4; // filter zero point

    RunTest<4, uint8_t>(
        0,
        {
            1, 2,
            4, 8,
        },
        // factors to consider:
        // - the filter zero point is non zero, hence the (x-fz)
        // - the output scale is 2 hence the /2
        // - output zero point is non zero, hence the +outZero
        // - RELU cuts negative values and then we add the output zero point
        {
            std::max(outZero, static_cast<uint8_t>((1*(2-fz) + 2*(1-fz) + 4*(0-fz) + 8*(6-fz) + bias)/2 + outZero)),
            std::max(outZero, static_cast<uint8_t>((2*(2-fz) + 0*(1-fz) + 8*(0-fz) + 0*(6-fz) + bias)/2 + outZero)),
            std::max(outZero, static_cast<uint8_t>((4*(2-fz) + 8*(1-fz) + 0*(0-fz) + 0*(6-fz) + bias)/2 + outZero)),
            std::max(outZero, static_cast<uint8_t>((8*(2-fz) + 0*(1-fz) + 0*(0-fz) + 0*(6-fz) + bias)/2 + outZero))
        });
}

struct Relu6Conv2DWithBiasesFixture : Conv2DWithBiasesFixture
{
    Relu6Conv2DWithBiasesFixture()
    : Conv2DWithBiasesFixture("[ 1, 2, 2, 1 ]",    // inputShape
                              "[ 1, 2, 2, 1 ]",    // outputShape
                              "[ 1, 2, 2, 1 ]",    // filterShape
                              "[ 2,1, 0,6 ]",      // filterData
                              "[ 1 ]",             // biasShape
                              "[ 0, 0, 0, 0 ]",    // biasData
                              "1",                 // stride w and h
                              "RELU6",             // activation
                              "1.0",               // filter scale
                              "0",                 // filter zero point
                              "2.0",               // output scale
                              "0")                 // output zero point
    {}
};

BOOST_FIXTURE_TEST_CASE( ParseConv2DAndRelu6WithBias, Relu6Conv2DWithBiasesFixture )
{
    uint8_t relu6Min = 6 / 2; // divide by output scale

    RunTest<4, uint8_t>(
        0,
        {
            1, 2,
            4, 1,
        },
        // factors to consider:
        // - the output scale is 2 hence the /2
        // - RELU6 cuts output values at +6
        {
            std::min(relu6Min, static_cast<uint8_t>((1*2 + 2*1 + 4*0 + 1*6)/2)),
            std::min(relu6Min, static_cast<uint8_t>((2*2 + 0*1 + 1*0 + 0*6)/2)),
            std::min(relu6Min, static_cast<uint8_t>((4*2 + 1*1 + 0*0 + 0*6)/2)),
            std::min(relu6Min, static_cast<uint8_t>((1*2 + 0*1 + 0*0 + 0*6)/2))
        });
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef TVMTL_TVMINIMIZER_PRPT3D_HPP
#define TVMTL_TVMINIMIZER_PRPT3D_HPP

//System includes
#include <iostream>
#include <map>
#include <vector>
#include <chrono>

#ifdef TVMTL_TVMIN_DEBUG
    #include <string>
#endif

//Eigen includes
#include <Eigen/Sparse>
#include <Eigen/Core>
#include <unsupported/Eigen/Splines>

//CGAL includes For linear Interpolation
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Interpolation_traits_2.h>
#include <CGAL/natural_neighbor_coordinates_2.h>
#include <CGAL/interpolation_functions.h>

//vpp includes
#include <vpp/vpp.hh>

namespace tvmtl {

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
    class TV_Minimizer< PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR, 3> {

	public:
	    // Manifold typedefs
	    typedef typename MANIFOLD::scalar_type scalar_type;
	    typedef typename MANIFOLD::value_type value_type;

	    // Functional typedefs
	    typedef typename FUNCTIONAL::weights_mat weights_mat;
	    typedef typename FUNCTIONAL::weights_type weights_type;

	    // Data acccess typedefs
	    typedef typename DATA::storage_type img_type;

	    // Constructor
	    TV_Minimizer(FUNCTIONAL& func, DATA& dat):
		func_(func),
		data_(dat)
	    {
		static_assert(FUNCTIONAL::disc_type == ANISO, "Proximal point is only possible with anisotropic weights!");
		static_assert(DATA::img_dim == 3, "Dimension of Proximal point minimizer(DIM=3) and data class must match! ");

		for(int i = 0; i < 7; i++){
		    proximal_mappings_[i] = img_type(data_.img_.domain());
		}
		use_approximate_mean_ = false;
		max_prpt_steps_ = 50;
		tolerance_ = 1e-4;
		max_runtime_ = 1000.0;
	    }

	    void use_approximate_mean(bool u) { use_approximate_mean_ = u; }

	    void first_guess();

	    void updateFidelity(double muk);
	    void updateTV(double muk, int dim, const weights_mat& W);

	    void geod_mean();
	    void approx_mean();

	    void prpt_step(double muk);
	    void minimize();

	    void setMax_runtime(int t) { max_runtime_ = t; }
	    void setMax_prpt_steps(int n) { max_prpt_steps_ = n; }
	    void setTolerance(double t) {tolerance_ =t; }

	    int max_runtime(int t) const { return max_runtime_; }
	    int max_prpt_steps(int n) const { return max_prpt_steps_; }
	    int tolerance(double t) const { return tolerance_; }

	private:
	    FUNCTIONAL& func_;
	    DATA& data_;

	    img_type proximal_mappings_[7];

	    bool use_approximate_mean_;

	    int prpt_step_;
	    int max_prpt_steps_;
	    double tolerance_;
	    double max_runtime_;

	    std::vector< std::chrono::duration<double> > Ts_;
	    std::vector< typename FUNCTIONAL::result_type > Js_;
    };

/*----- IMPLEMENTATION PRPT------*/
template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR, 3>::updateFidelity(double muk){
    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t Fidelity Update with muk: " << muk << std::endl;
    #endif

    double tau = muk / (muk + 1.0);

    if(data_.doInpaint()){
	auto proximal_mapF = [&] (value_type& p, const value_type& i, const value_type& n, const bool inp ) {
	    value_type temp;
	    MANIFOLD::convex_combination(i, n, tau, temp);
	    p = temp * (1-inp) + i * inp;
	};

	pixel_wise3d(proximal_mapF, proximal_mappings_[0], data_.img_, data_.noise_img_, data_.inp_);
    }
    else{
    	auto proximal_mapF = [&] (value_type& p, const value_type& i, const value_type& n) {
	    MANIFOLD::convex_combination(i, n, tau, p);
	};

	pixel_wise3d(proximal_mapF, proximal_mappings_[0], data_.img_, data_.noise_img_);
    }

}

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR, 3>::updateTV(double muk, int dim, const weights_mat& W){
    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t TV Update with muk: " << muk << std::endl;
	    std::cout << "\t\t... Dimension: " << dim << std::endl;
    #endif
    double lmuk = muk * func_.getlambda();
    weights_mat tau(data_.img_.domain());
    pixel_wise3d([&] (weights_type& t, const weights_type& w) { t = std::min(lmuk * w, 0.5);}, tau, W);

    // 3D
    int ns = data_.img_.nslices();
    int nr = data_.img_.nrows();
    int nc = data_.img_.ncols();

    // Subimage boxes
    vpp::box3d without_last_x(vpp::vint3(0,0,0), vpp::vint3(ns - 1, nr - 1, nc - 2)); // subdomain without last xslice
    vpp::box3d without_last_y(vpp::vint3(0,0,0), vpp::vint3(ns - 1, nr - 2, nc - 1)); // subdomain without last yslice
    vpp::box3d without_last_z(vpp::vint3(0,0,0), vpp::vint3(ns - 2, nr - 1, nc - 1)); // subdomain without last zslice
    vpp::box3d without_first_x(vpp::vint3(0,0,1), vpp::vint3(ns - 1, nr - 1, nc - 1)); // subdomain without first xlice
    vpp::box3d without_first_y(vpp::vint3(0,1,0), vpp::vint3(ns - 1, nr - 1, nc - 1)); // subdomain without first yslice
    vpp::box3d without_first_z(vpp::vint3(1,0,0), vpp::vint3(ns - 1, nr - 1, nc - 1)); // subdomain without first zslice

    // Dimensions of slice
    vpp::vint3 dims(ns, nr, nc); dims(dim) = 2; // z-dim=0, y-dim = 1, x-dim = 2
    vpp::vint3 loopdims(ns, nr, nc); loopdims(dim) = 1;

    // Relative neighbor coordinates
    vpp::vint3 n(0,0,0); n(dim) = 1;

    // Correct Subimage
    vpp::box3d subimage = data_.img_.domain();
    if(dim == 0  && (ns % 2 != 0)) // dim = z
	    subimage = without_last_z;

    if(dim == 1  && (nr % 2 != 0)) // dim = y
	    subimage = without_last_y;

    if(dim == 2  &&  (nc % 2 != 0)) // dim = x
	    subimage = without_last_x;

    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t\t...Odd Pairs..., dims = (" << dims(0) << "," << dims(1) << "," << dims(2) <<  "), n = (" << n(0) << "," << n(1) << "," << n(2) << ")" <<std::endl;
	    int num_blocks = 0;
    #endif

    clone3d(data_.img_, proximal_mappings_[1 + 2 * dim]);
    auto I = data_.img_ | subimage;
    auto T = tau | subimage;
    auto P = proximal_mappings_[1 + 2 * dim] | subimage;

    int Ins = I.nslices();
    int Inr = I.nrows();
    int Inc = I.ncols();

    for(int s = 0; s < Ins; s += dims(0)){
	//#pragma omp parallel for
	for(int r = 0; r < Inr; r += dims(1)){
	    for(int c = 0; c < Inc; c+= dims(2)){
		vpp::vint3 first(s,r,c);
		vpp::vint3 last(s + loopdims(0) - 1, r + loopdims(1) - 1, c + loopdims(2)-1 );
		vpp::box3d block(first, last);
		    #ifdef TVMTL_TVMIN_DEBUG_VERBOSE
			std::cout << "\t\t\tBlock: " << num_blocks << " , Slice: " << s << ", Row: " << r << ", Col: " << c << std::endl;
		    #endif

		for (auto p : block){
		    value_type l;
		    MANIFOLD::log(I(p), I(p + n), l);
		    MANIFOLD::exp(I(p), l * T(p), P(p));
		    MANIFOLD::exp(I(p), l - l * T(p), P(p + n));
		}
	    	#ifdef TVMTL_TVMIN_DEBUG
		    num_blocks++;
		#endif
	    }
	}
    }

    #ifdef TVMTL_TVMIN_DEBUG
	std::cout << "\t\tBlock processed: " << num_blocks << std::endl;
    #endif

   if(dim == 0) // z
	if(ns % 2 == 0)
	    subimage =  vpp::box3d(vpp::vint3(1, 0, 0), vpp::vint3(ns-2, nr-1, nc-1)); // without first and last z slice
	else
	    subimage = without_first_z;

    if(dim == 1) // y
	if(nr % 2 == 0)
	    subimage =  vpp::box3d(vpp::vint3(0, 1, 0), vpp::vint3(ns-1, nr-2, nc-1));
	else
	    subimage = without_first_y;

    if(dim == 2) // x
	if(nc % 2 == 0)
	    subimage = vpp::box3d(vpp::vint3(0, 0, 1), vpp::vint3(ns-1, nr-1, nc-2));
	else
	    subimage = without_first_x;

    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t\t...Even Pairs..." << std::endl;
	    num_blocks = 0;
    #endif
    clone3d(data_.img_, proximal_mappings_[2 + 2 * dim]);
    I = data_.img_ | subimage;
    T = tau | subimage;
    P = proximal_mappings_[2 + 2 * dim] | subimage;

    Ins = I.nslices();
    Inr = I.nrows();
    Inc = I.ncols();

    for(int s = 0; s < Ins; s += dims(0)){
	//#pragma omp parallel for
	for(int r = 0; r < Inr; r += dims(1)){
	    for(int c = 0; c < Inc; c+= dims(2)){
		vpp::vint3 first(s,r,c);
		vpp::vint3 last(s + loopdims(0) - 1, r + loopdims(1) - 1, c + loopdims(2)-1 );
		vpp::box3d block(first, last);
		    #ifdef TVMTL_TVMIN_DEBUG_VERBOSE
			std::cout << "\t\t\tBlock: " << num_blocks << " , Slice: " << s << ", Row: " << r << ", Col: " << c << std::endl;
		    #endif

		for (auto p : block){
		    value_type l;
		    MANIFOLD::log(I(p), I(p + n), l);
		    MANIFOLD::exp(I(p), l * T(p), P(p));
		    MANIFOLD::exp(I(p), l - l * T(p), P(p + n));
		}
	    	#ifdef TVMTL_TVMIN_DEBUG
		    num_blocks++;
		#endif
	    }
	}
    }

    #ifdef TVMTL_TVMIN_DEBUG
	std::cout << "\t\tBlocks processed: " << num_blocks << std::endl;
    #endif

}

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR, 3>::geod_mean(){
    typedef value_type& vtr;
    typedef const vtr cvtr;

    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t Calculate Geodesic mean: "<< std::endl;
	    std::cout << "\t\t...first guess euclidiean mean: "<< std::endl;
    #endif

    // First estimate
    auto euclidian_mean = [&] (vtr i, cvtr p0, cvtr p1, cvtr p2, cvtr p3, cvtr p4, cvtr p5, cvtr p6) {
	i = (p0 + p1 + p2 + p3 + p4 + p5 + p6) / 7.0;
    };
    pixel_wise3d(euclidian_mean, data_.img_, proximal_mappings_[0], proximal_mappings_[1], proximal_mappings_[2], proximal_mappings_[3], proximal_mappings_[4], proximal_mappings_[5], proximal_mappings_[6]);

    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t\t...Karcher mean Iteration: "<< std::endl;
    #endif

    //TODO: Put in Algo traits
    double tol = 1e-10;
    double max_karcher_iterations = 15;

    auto karcher_mean = [&] (vtr i, cvtr p0, cvtr p1, cvtr p2, cvtr p3, cvtr p4, cvtr p5, cvtr p6) {

	/* Slow version, - needs to copy p_i for insertion in value_list
	typename MANIFOLD::value_list v;
	v.push_back(p0);
	v.push_back(p1);
	v.push_back(p2);
	v.push_back(p3);
	v.push_back(p4);
	v.push_back(p5);
	v.push_back(p6);
	MANIFOLD::karcher_mean(i, v, tol, max_karcher_iterations); */

	MANIFOLD::karcher_mean(i, p0, p1, p2, p3, p4, p5, p6);
    };

    pixel_wise3d(karcher_mean, data_.img_, proximal_mappings_[0], proximal_mappings_[1], proximal_mappings_[2], proximal_mappings_[3], proximal_mappings_[4], proximal_mappings_[5], proximal_mappings_[6]);

}

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR, 3>::approx_mean(){
    typedef value_type& vtr;
    typedef const vtr cvtr;

    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t Calculate approximate mean via convex combinations: "<< std::endl;
    #endif
    //TODO: Check weights
    auto convex_comb_mean = [&] (vtr i, cvtr p0, cvtr p1, cvtr p2, cvtr p3, cvtr p4, cvtr p5, cvtr p6) {
	value_type p01, p23, p45;
	MANIFOLD::convex_combination(p0, p1, 0.5, p01);
	MANIFOLD::convex_combination(p2, p3, 0.5, p23);
	MANIFOLD::convex_combination(p4, p5, 0.5, p45);
	MANIFOLD::convex_combination(p01, p23, 0.5, p01);
	MANIFOLD::convex_combination(p45, p6, 1.0/3, p23);
	MANIFOLD::convex_combination(p01, p23, 3.0/7.0, i);
    };

    pixel_wise3d(convex_comb_mean, data_.img_, proximal_mappings_[0], proximal_mappings_[1], proximal_mappings_[2], proximal_mappings_[3], proximal_mappings_[4], proximal_mappings_[5], proximal_mappings_[6]);

}

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR, 3>::prpt_step(double muk){
    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t Proximal step with muk: " << muk << std::endl;
    #endif
    weights_mat X = func_.getweightsX();
    weights_mat Y = func_.getweightsY();
    weights_mat Z = func_.getweightsZ();

    updateFidelity(muk);

    updateTV(muk, 0, Z);
    updateTV(muk, 1, Y);
    updateTV(muk, 2, X);

    if(use_approximate_mean_)
	approx_mean();
    else
	geod_mean();
}

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR, 3>::minimize(){
    std::cout << "Starting Proximal Point Algorithm with..." << std::endl;
    std::cout << "\t Lambda = \t" << func_.getlambda() << std::endl;
    std::cout << "\t Max Steps = \t" << max_prpt_steps_ << std::endl;

    prpt_step_ = 1;

    std::chrono::time_point<std::chrono::system_clock> start, end;
    std::chrono::duration<double> t = std::chrono::duration<double>::zero();
    start = std::chrono::system_clock::now();

    // PRPT Iteration Loop
    while(prpt_step_ <= max_prpt_steps_ && t.count() < max_runtime_){

	std::cout << "PRPT Step #" << prpt_step_ << std::endl;
	typename FUNCTIONAL::result_type J = func_.evaluateJ();
	Js_.push_back(J);
	std::cout << "\t Value of Functional J: " << J << std::endl;

	double muk = 3.0 * std::pow(static_cast<double>(prpt_step_),-0.95);
	#ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t Value of muk: " << muk << std::endl;
	#endif

	prpt_step(muk);

	end = std::chrono::system_clock::now();
	t = end - start;
	std::cout << "\t Elapsed time: " << t.count() << " seconds." << std::endl;
	prpt_step_++;
	Ts_.push_back(t);
    }

    std::cout << "Minimization in " << t.count() << " seconds." << std::endl;
}

} // end namespace tvmtl

#endif


// Testing PPR
#include "pauli_product.cpp"
#include "decode.cpp"
#include <armadillo>
#include <chrono>
#include <iostream>
#include <stdlib.h>

using namespace std;
using namespace arma;

int main()
{
  int n = 256;
  cx_vec psi;
  psi.randn(n);
  psi = normalise(psi, 2);

  decode(psi, 6, 0.01);
}


#ifndef BOOST_GEOMETRY_PROJECTIONS_AEQD_HPP
#define BOOST_GEOMETRY_PROJECTIONS_AEQD_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/impl/aasincos.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_mlfn.hpp>
#include <boost/geometry/extensions/gis/projections/impl/proj_mdist.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace aeqd{
            static const double EPS10 = 1.e-10;
            static const double TOL = 1.e-14;
            static const int N_POLE = 0;
            static const int S_POLE = 1;
            static const int EQUIT = 2;
            static const int OBLIQ = 3;

            struct par_aeqd
            {
                double    sinph0;
                double    cosph0;
                double    en[EN_SIZE];
                double    M1;
                double    N1;
                double    Mp;
                double    He;
                double    G;
                int        mode;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_aeqd_ellipsoid : public base_t_fi<base_aeqd_ellipsoid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_aeqd m_proj_parm;

                inline base_aeqd_ellipsoid(const Parameters& par)
                    : base_t_fi<base_aeqd_ellipsoid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double  coslam, cosphi, sinphi, rho, s, H, H2, c, Az, t, ct, st, cA, sA;

                    coslam = cos(lp_lon);
                    cosphi = cos(lp_lat);
                    sinphi = sin(lp_lat);
                    switch (this->m_proj_parm.mode) {
                    case N_POLE:
                        coslam = - coslam;
                    case S_POLE:
                        xy_x = (rho = fabs(this->m_proj_parm.Mp - pj_mlfn(lp_lat, sinphi, cosphi, this->m_proj_parm.en))) *
                            sin(lp_lon);
                        xy_y = rho * coslam;
                        break;
                    case EQUIT:
                    case OBLIQ:
                        if (fabs(lp_lon) < EPS10 && fabs(lp_lat - this->m_par.phi0) < EPS10) {
                            xy_x = xy_y = 0.;
                            break;
                        }
                        t = atan2(this->m_par.one_es * sinphi + this->m_par.es * this->m_proj_parm.N1 * this->m_proj_parm.sinph0 *
                            sqrt(1. - this->m_par.es * sinphi * sinphi), cosphi);
                        ct = cos(t); st = sin(t);
                        Az = atan2(sin(lp_lon) * ct, this->m_proj_parm.cosph0 * st - this->m_proj_parm.sinph0 * coslam * ct);
                        cA = cos(Az); sA = sin(Az);
                        s = aasin( fabs(sA) < TOL ?
                            (this->m_proj_parm.cosph0 * st - this->m_proj_parm.sinph0 * coslam * ct) / cA :
                            sin(lp_lon) * ct / sA );
                        H = this->m_proj_parm.He * cA;
                        H2 = H * H;
                        c = this->m_proj_parm.N1 * s * (1. + s * s * (- H2 * (1. - H2)/6. +
                            s * ( this->m_proj_parm.G * H * (1. - 2. * H2 * H2) / 8. +
                            s * ((H2 * (4. - 7. * H2) - 3. * this->m_proj_parm.G * this->m_proj_parm.G * (1. - 7. * H2)) /
                            120. - s * this->m_proj_parm.G * H / 48.))));
                        xy_x = c * sA;
                        xy_y = c * cA;
                        break;
                    }
                }

                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double c, Az, cosAz, A, B, D, E, F, psi, t;

                    if ((c = boost::math::hypot(xy_x, xy_y)) < EPS10) {
                        lp_lat = this->m_par.phi0;
                        lp_lon = 0.;
                            return;
                    }
                    if (this->m_proj_parm.mode == OBLIQ || this->m_proj_parm.mode == EQUIT) {
                        cosAz = cos(Az = atan2(xy_x, xy_y));
                        t = this->m_proj_parm.cosph0 * cosAz;
                        B = this->m_par.es * t / this->m_par.one_es;
                        A = - B * t;
                        B *= 3. * (1. - A) * this->m_proj_parm.sinph0;
                        D = c / this->m_proj_parm.N1;
                        E = D * (1. - D * D * (A * (1. + A) / 6. + B * (1. + 3.*A) * D / 24.));
                        F = 1. - E * E * (A / 2. + B * E / 6.);
                        psi = aasin(this->m_proj_parm.sinph0 * cos(E) + t * sin(E));
                        lp_lon = aasin(sin(Az) * sin(E) / cos(psi));
                        if ((t = fabs(psi)) < EPS10)
                            lp_lat = 0.;
                        else if (fabs(t - HALFPI) < 0.)
                            lp_lat = HALFPI;
                        else
                            lp_lat = atan((1. - this->m_par.es * F * this->m_proj_parm.sinph0 / sin(psi)) * tan(psi) /
                                this->m_par.one_es);
                    } else { /* Polar */
                        lp_lat = pj_inv_mlfn(this->m_proj_parm.mode == N_POLE ? this->m_proj_parm.Mp - c : this->m_proj_parm.Mp + c,
                            this->m_par.es, this->m_proj_parm.en);
                        lp_lon = atan2(xy_x, this->m_proj_parm.mode == N_POLE ? -xy_y : xy_y);
                    }
                }
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_aeqd_guam : public base_t_fi<base_aeqd_guam<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_aeqd m_proj_parm;

                inline base_aeqd_guam(const Parameters& par)
                    : base_t_fi<base_aeqd_guam<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double  cosphi, sinphi, t;

                    cosphi = cos(lp_lat);
                    sinphi = sin(lp_lat);
                    t = 1. / sqrt(1. - this->m_par.es * sinphi * sinphi);
                    xy_x = lp_lon * cosphi * t;
                    xy_y = pj_mlfn(lp_lat, sinphi, cosphi, this->m_proj_parm.en) - this->m_proj_parm.M1 +
                        .5 * lp_lon * lp_lon * cosphi * sinphi * t;
                }

                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double x2, t;
                    int i;

                    x2 = 0.5 * xy_x * xy_x;
                    lp_lat = this->m_par.phi0;
                    for (i = 0; i < 3; ++i) {
                        t = this->m_par.e * sin(lp_lat);
                        lp_lat = pj_inv_mlfn(this->m_proj_parm.M1 + xy_y -
                            x2 * tan(lp_lat) * (t = sqrt(1. - t * t)), this->m_par.es, this->m_proj_parm.en);
                    }
                    lp_lon = xy_x * t / cos(lp_lat);
                }
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_aeqd_spheroid : public base_t_fi<base_aeqd_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_aeqd m_proj_parm;

                inline base_aeqd_spheroid(const Parameters& par)
                    : base_t_fi<base_aeqd_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double  coslam, cosphi, sinphi;

                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    coslam = cos(lp_lon);
                    switch (this->m_proj_parm.mode) {
                    case EQUIT:
                        xy_y = cosphi * coslam;
                        goto oblcon;
                    case OBLIQ:
                        xy_y = this->m_proj_parm.sinph0 * sinphi + this->m_proj_parm.cosph0 * cosphi * coslam;
                oblcon:
                        if (fabs(fabs(xy_y) - 1.) < TOL)
                            if (xy_y < 0.)
                                throw proj_exception();
                            else
                                xy_x = xy_y = 0.;
                        else {
                            xy_y = acos(xy_y);
                            xy_y /= sin(xy_y);
                            xy_x = xy_y * cosphi * sin(lp_lon);
                            xy_y *= (this->m_proj_parm.mode == EQUIT) ? sinphi :
                                   this->m_proj_parm.cosph0 * sinphi - this->m_proj_parm.sinph0 * cosphi * coslam;
                        }
                        break;
                    case N_POLE:
                        lp_lat = -lp_lat;
                        coslam = -coslam;
                    case S_POLE:
                        if (fabs(lp_lat - HALFPI) < EPS10) throw proj_exception();;
                        xy_x = (xy_y = (HALFPI + lp_lat)) * sin(lp_lon);
                        xy_y *= coslam;
                        break;
                    }
                }

                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double cosc, c_rh, sinc;

                    if ((c_rh = boost::math::hypot(xy_x, xy_y)) > PI) {
                        if (c_rh - EPS10 > PI) throw proj_exception();;
                        c_rh = PI;
                    } else if (c_rh < EPS10) {
                        lp_lat = this->m_par.phi0;
                        lp_lon = 0.;
                            return;
                    }
                    if (this->m_proj_parm.mode == OBLIQ || this->m_proj_parm.mode == EQUIT) {
                        sinc = sin(c_rh);
                        cosc = cos(c_rh);
                        if (this->m_proj_parm.mode == EQUIT) {
                                        lp_lat = aasin(xy_y * sinc / c_rh);
                            xy_x *= sinc;
                            xy_y = cosc * c_rh;
                        } else {
                            lp_lat = aasin(cosc * this->m_proj_parm.sinph0 + xy_y * sinc * this->m_proj_parm.cosph0 /
                                c_rh);
                            xy_y = (cosc - this->m_proj_parm.sinph0 * sin(lp_lat)) * c_rh;
                            xy_x *= sinc * this->m_proj_parm.cosph0;
                        }
                        lp_lon = xy_y == 0. ? 0. : atan2(xy_x, xy_y);
                    } else if (this->m_proj_parm.mode == N_POLE) {
                        lp_lat = HALFPI - c_rh;
                        lp_lon = atan2(xy_x, -xy_y);
                    } else {
                        lp_lat = c_rh - HALFPI;
                        lp_lon = atan2(xy_x, xy_y);
                    }
                }
            };

            // Azimuthal Equidistant
            template <typename Parameters>
            void setup_aeqd(Parameters& par, par_aeqd& proj_parm)
            {
                par.phi0 = pj_param(par.params, "rlat_0").f;
                if (fabs(fabs(par.phi0) - HALFPI) < EPS10) {
                    proj_parm.mode = par.phi0 < 0. ? S_POLE : N_POLE;
                    proj_parm.sinph0 = par.phi0 < 0. ? -1. : 1.;
                    proj_parm.cosph0 = 0.;
                } else if (fabs(par.phi0) < EPS10) {
                    proj_parm.mode = EQUIT;
                    proj_parm.sinph0 = 0.;
                    proj_parm.cosph0 = 1.;
                } else {
                    proj_parm.mode = OBLIQ;
                    proj_parm.sinph0 = sin(par.phi0);
                    proj_parm.cosph0 = cos(par.phi0);
                }
                if (! par.es) {
                // par.inv = s_inverse;
                // par.fwd = s_forward;
                } else {
                    pj_enfn(par.es, proj_parm.en);
                    if (pj_param(par.params, "bguam").i) {
                        proj_parm.M1 = pj_mlfn(par.phi0, proj_parm.sinph0, proj_parm.cosph0, proj_parm.en);
                // par.inv = e_guam_inv;
                // par.fwd = e_guam_fwd;
                    } else {
                        switch (proj_parm.mode) {
                        case N_POLE:
                            proj_parm.Mp = pj_mlfn(HALFPI, 1., 0., proj_parm.en);
                            break;
                        case S_POLE:
                            proj_parm.Mp = pj_mlfn(-HALFPI, -1., 0., proj_parm.en);
                            break;
                        case EQUIT:
                        case OBLIQ:
                // par.inv = e_inverse;
                // par.fwd = e_forward;
                            proj_parm.N1 = 1. / sqrt(1. - par.es * proj_parm.sinph0 * proj_parm.sinph0);
                            proj_parm.G = proj_parm.sinph0 * (proj_parm.He = par.e / sqrt(par.one_es));
                            proj_parm.He *= proj_parm.cosph0;
                            break;
                        }
                // par.inv = e_inverse;
                // par.fwd = e_forward;
                    }
                }
            }

        }} // namespace detail::aeqd
    #endif // doxygen

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
         - lat_0 guam
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct aeqd_ellipsoid : public detail::aeqd::base_aeqd_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline aeqd_ellipsoid(const Parameters& par) : detail::aeqd::base_aeqd_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
         - lat_0 guam
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct aeqd_guam : public detail::aeqd::base_aeqd_guam<Geographic, Cartesian, Parameters>
    {
        inline aeqd_guam(const Parameters& par) : detail::aeqd::base_aeqd_guam<Geographic, Cartesian, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
         - lat_0 guam
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct aeqd_spheroid : public detail::aeqd::base_aeqd_spheroid<Geographic, Cartesian, Parameters>
    {
        inline aeqd_spheroid(const Parameters& par) : detail::aeqd::base_aeqd_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class aeqd_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    if (! par.es)
                        return new base_v_fi<aeqd_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                    else if (pj_param(par.params, "bguam").i)
                        return new base_v_fi<aeqd_guam<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                    else
                        return new base_v_fi<aeqd_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void aeqd_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("aeqd", new aeqd_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_AEQD_HPP


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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)

#ifndef FCPPT_MPL_DETAIL_FLATTEN_HPP_INCLUDED
#define FCPPT_MPL_DETAIL_FLATTEN_HPP_INCLUDED

#include <fcppt/config/external_begin.hpp>
#include <boost/mpl/back_inserter.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/is_sequence.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/vector/vector10.hpp>
#include <boost/utility/enable_if.hpp>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace mpl
{
namespace detail
{

template<
	typename Type,
	typename Enable = void
>
struct flatten;

template<
	typename Type
>
struct flatten<
	Type,
	typename
	boost::enable_if<
		boost::mpl::is_sequence<
			Type
		>
	>::type
>
{
	typedef
	typename
	boost::mpl::fold<
		Type,
		boost::mpl::vector0<>,
		boost::mpl::copy<
			fcppt::mpl::detail::flatten<
				boost::mpl::_2
			>,
			boost::mpl::back_inserter<
				boost::mpl::_1
			>
		>
	>::type
	type;
};

template<
	typename Type
>
struct flatten<
	Type,
	typename
	boost::disable_if<
		boost::mpl::is_sequence<
			Type
		>
	>::type
>
{
	typedef
	boost::mpl::vector1<
		Type
	>
	type;
};

}
}
}

#endif


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * 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 Regents or University of California 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 HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

// Author: Steffen Urban (steffen.urban@googlemail.com), March 2021

#include "theia/sfm/global_pose_estimation/LiGT_position_estimator.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <algorithm>
#include <ceres/rotation.h>
#include <glog/logging.h>
#include <memory>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include "spectra/include/SymEigsShiftSolver.h"

#include "theia/math/graph/triplet_extractor.h"
#include "theia/math/matrix/spectra_linear_operator.h"
#include "theia/sfm/find_common_tracks_in_views.h"
#include "theia/sfm/global_pose_estimation/compute_triplet_baseline_ratios.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_triplet.h"
#include "theia/util/map_util.h"
#include "theia/util/threadpool.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace {

Eigen::Matrix3d GetSkew(const Eigen::Vector3d &f) {
  Eigen::Matrix3d skew_mat;
  skew_mat << 0.0, -f(2), f(1), f(2), 0.0, -f(0), -f(1), f(0), 0.0;
  return skew_mat;
}

Eigen::Matrix3d GetRij(const Eigen::Matrix3d &i, const Eigen::Matrix3d &j) {
  return j * i.transpose();
}

double GetThetaSq(const Eigen::Vector3d &feat_i, const Eigen::Vector3d &feat_j,
                  const Eigen::Matrix3d &Rij) {
  return (GetSkew(feat_j) * Rij * feat_i).squaredNorm();
}

Eigen::Vector3d Get_aij(const Eigen::Matrix3d &Rij, const Eigen::Vector3d Xi,
                        const Eigen::Vector3d Xj) {
  return (GetSkew(Rij * Xi) * Xj).transpose() * GetSkew(Xj);
}

std::vector<ViewIdTriplet> GetLargetConnectedTripletGraph(
    const std::unordered_map<ViewIdPair, TwoViewInfo> &view_pairs) {
  static const int kLargestCCIndex = 0;

  // Get a list of all edges in the view graph.
  std::unordered_set<ViewIdPair> view_id_pairs;
  view_id_pairs.reserve(view_pairs.size());
  for (const auto &view_pair : view_pairs) {
    view_id_pairs.insert(view_pair.first);
  }

  // Extract connected triplets.
  TripletExtractor<ViewId> extractor;
  std::vector<std::vector<ViewIdTriplet>> triplets;
  CHECK(extractor.ExtractTriplets(view_id_pairs, &triplets));
  CHECK_GT(triplets.size(), 0);
  return triplets[kLargestCCIndex];
}

// Adds the constraint from the triplet to the symmetric matrix. Our standard
// constraint matrix A is a 3M x 3N matrix with M triplet constraints and N
// cameras. We seek to construct A^t * A directly. For each triplet constraint
// in our matrix A (i.e. a 3-row block), we can compute the corresponding
// entries in A^t * A with the following summation:
//
//   A^t * A += Row(i)^t * Row(i)
//
// for each triplet constraint i.
void AddTripletConstraintToSymmetricMatrix(
    const std::vector<Matrix3d> &constraints,
    const std::vector<int> &view_indices,
    std::unordered_map<std::pair<int, int>, double> *sparse_matrix_entries) {
  // Construct Row(i)^t * Row(i). If we denote the row as a block matrix:
  //
  //   Row(i) = [A | B | C]
  //
  // then we have:
  //
  //   Row(i)^t * Row(i) = [A | B | C]^t * [A | B | C]
  //                     = [ A^t * A  |  A^t * B  |  A^t * C]
  //                       [ B^t * A  |  B^t * B  |  B^t * C]
  //                       [ C^t * A  |  C^t * B  |  C^t * C]
  //
  // Since A^t * A is symmetric, we only store the upper triangular portion.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      // Skip any block entries that correspond to the lower triangular portion
      // of the matrix.
      if (view_indices[i] > view_indices[j]) {
        continue;
      }

      // Compute the A^t * B, etc. matrix.
      const Eigen::Matrix3d symmetric_constraint =
          constraints[i].transpose() * constraints[j];

      // Add to the 3x3 block corresponding to (i, j)
      for (int r = 0; r < 3; r++) {
        for (int c = 0; c < 3; c++) {
          const std::pair<int, int> row_col(view_indices[i] + r,
                                            view_indices[j] + c);
          (*sparse_matrix_entries)[row_col] += symmetric_constraint(r, c);
        }
      }
    }
  }
}

inline Matrix3d AngleAxisToRotationMatrix(const Vector3d angle_axis) {
  const double angle = angle_axis.norm();
  const Eigen::AngleAxisd rotation_aa(angle, angle_axis / angle);
  return rotation_aa.toRotationMatrix();
}

// Returns true if the vector R1 * (c2 - c1) is in the same direction as t_12.
bool VectorsAreSameDirection(const Vector3d &position1,
                             const Vector3d &position2,
                             const Vector3d &rotation1,
                             const Vector3d &relative_position12) {
  const Vector3d global_relative_position =
      (position2 - position1).normalized();
  Vector3d rotated_relative_position;
  ceres::AngleAxisRotatePoint(rotation1.data(), global_relative_position.data(),
                              rotated_relative_position.data());
  return rotated_relative_position.dot(relative_position12) > 0;
}

} // namespace

LiGTPositionEstimator::LiGTPositionEstimator(
    const Options &options, const Reconstruction &reconstruction)
    : options_(options), reconstruction_(reconstruction) {
  CHECK_GT(options.num_threads, 0);
}

bool LiGTPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo> &view_pairs,
    const std::unordered_map<ViewId, Vector3d> &orientations,
    std::unordered_map<ViewId, Vector3d> *positions) {
  CHECK_NOTNULL(positions)->clear();
  view_pairs_ = &view_pairs;
  orientations_ = &orientations;

  VLOG(2) << "Extracting triplets from tracks.";
  FindTripletsForTracks();

  VLOG(2) << "Calculating BCD for tracks.";
  std::unordered_map<TrackId,
                     std::vector<std::tuple<Matrix3d, Matrix3d, Matrix3d>>>
      BCDs;
  for (const auto &t : triplets_for_tracks_) {
    auto t_id = t.first;
    auto view_ids = t.second;
    for (const auto &vids : view_ids) {
      const auto view1 = reconstruction_.View(std::get<0>(vids));
      const auto view2 = reconstruction_.View(std::get<1>(vids));
      const auto view3 = reconstruction_.View(std::get<2>(vids));
      std::tuple<Matrix3d, Matrix3d, Matrix3d> BCD;
      CalculateBCDForTrack(view1, view2, view3, t_id, BCD);
      BCDs[t_id].push_back(BCD);
    }
  }

  //  // Extract triplets from the view pairs. As of now, we only consider the
  //  // largest connected triplet in the viewing graph.
  //  VLOG(2) << "Extracting triplets from the viewing graph.";
  //  triplets_ = GetLargetConnectedTripletGraph(view_pairs);

  //  VLOG(2) << "Determining baseline ratios within each triplet...";
  //  // Baselines where (x, y, z) corresponds to the baseline of the first,
  //  // second, and third view pair in the triplet.
  //  std::unique_ptr<ThreadPool> pool(new ThreadPool(options_.num_threads));
  //  baselines_.resize(triplets_.size());
  //  for (int i = 0; i < triplets_.size(); i++) {
  //    AddTripletConstraint(triplets_[i]);
  //    pool->Add(&LiGTPositionEstimator::ComputeBaselineRatioForTriplet, this,
  //              triplets_[i], &baselines_[i]);
  //  }
  //  pool.reset(nullptr);

  //  VLOG(2) << "Building the constraint matrix...";
  //  // Create the linear system based on triplet constraints.
  //  Eigen::SparseMatrix<double> constraint_matrix;
  //  CreateLinearSystem(&constraint_matrix);

  //  // Solve for positions by examining the smallest eigenvalues. Since we
  //  have
  //  // set one position constant at the origin, we only need to solve for the
  //  // eigenvector corresponding to the smallest eigenvalue. This can be done
  //  // efficiently with inverse power iterations.
  //  VLOG(2) << "Solving for positions from the sparse eigenvalue problem...";
  //  SparseSymShiftSolveLLT op(constraint_matrix);
  //  Spectra::SymEigsShiftSolver<double, Spectra::LARGEST_MAGN,
  //                              SparseSymShiftSolveLLT>
  //      eigs(&op, 1, 6, 0.0);
  //  eigs.init();
  //  eigs.compute();

  //  // Compute with power iterations.
  //  const Eigen::VectorXd solution = eigs.eigenvectors().col(0);

  //  // Add the solutions to the output. Set the position with an index of -1
  //  to
  //  // be at the origin.
  //  for (const auto &view_index : linear_system_index_) {
  //    if (view_index.second < 0) {
  //      (*positions)[view_index.first].setZero();
  //    } else {
  //      (*positions)[view_index.first] =
  //          solution.segment<3>(view_index.second * 3);
  //    }
  //  }

  //  // Flip the sign of the positions if necessary.
  //  FlipSignOfPositionsIfNecessary(positions);

  return true;
}

void LiGTPositionEstimator::CalculateBCDForTrack(
    const theia::View *view1, const theia::View *view2,
    const theia::View *view3, const TrackId &track_id,
    std::tuple<Eigen::Matrix3d, Eigen::Matrix3d, Eigen::Matrix3d> &BCD) {

  const Vector3d feature1 =
      GetNormalizedFeature(*view1, track_id).point_.homogeneous();
  const Vector3d feature2 =
      GetNormalizedFeature(*view2, track_id).point_.homogeneous();
  const Vector3d feature3 =
      GetNormalizedFeature(*view3, track_id).point_.homogeneous();

  const Eigen::Matrix3d R1 = view1->Camera().GetOrientationAsRotationMatrix();
  const Eigen::Matrix3d R2 = view2->Camera().GetOrientationAsRotationMatrix();
  const Eigen::Matrix3d R3 = view3->Camera().GetOrientationAsRotationMatrix();

  const Matrix3d R31 = GetRij(R3, R1);
  const Matrix3d R32 = GetRij(R3, R2);

  const Vector3d a32 = Get_aij(R32, feature3, feature2);

  const Matrix3d skew_feat1 = GetSkew(feature1);
  // equation 18
  std::get<0>(BCD) = skew_feat1 * R31 * feature3 * a32.transpose() * R2;

  const double theta = GetThetaSq(feature3, feature2, R32);
  std::get<1>(BCD) = theta * skew_feat1 * R1;

  std::get<2>(BCD) = -(std::get<0>(BCD) + std::get<1>(BCD));
}

void LiGTPositionEstimator::FindTripletsForTracks() {
  std::vector<TrackId> track_ids = reconstruction_.TrackIds();
  for (auto t = 0; t < track_ids.size(); ++t) {
    auto view_ids_for_track = reconstruction_.Track(track_ids[t])->ViewIds();
    if (view_ids_for_track.size() < 3) {
      continue;
    }
    // todo implement eq 29
    for (auto v = 1; v < view_ids_for_track.size() - 1; ++v) {
      triplets_for_tracks_[track_ids[t]].push_back(
          std::make_tuple(*std::next(view_ids_for_track.begin(), v-1),
                          *std::next(view_ids_for_track.begin(), v),
                          *std::next(view_ids_for_track.begin(), v+1)));
    }
  }
}

// An alternative interface is to instead add triplets one by one to linear
// estimator. This allows for adding redundant observations of triplets, which
// may be useful if there are multiple estimates of the data.
void LiGTPositionEstimator::AddTripletConstraint(
    const ViewIdTriplet &view_triplet) {
  num_triplets_for_view_[std::get<0>(view_triplet)] += 1;
  num_triplets_for_view_[std::get<1>(view_triplet)] += 1;
  num_triplets_for_view_[std::get<2>(view_triplet)] += 1;

  // Determine the order of the views in the linear system. We subtract 1 from
  // the linear system index so that the first position added to the system
  // will be set constant (index of -1 is intentionally not evaluated later).
  InsertIfNotPresent(&linear_system_index_, std::get<0>(view_triplet),
                     linear_system_index_.size() - 1);
  InsertIfNotPresent(&linear_system_index_, std::get<1>(view_triplet),
                     linear_system_index_.size() - 1);
  InsertIfNotPresent(&linear_system_index_, std::get<2>(view_triplet),
                     linear_system_index_.size() - 1);
}

void LiGTPositionEstimator::ComputeBaselineRatioForTriplet(
    const ViewIdTriplet &triplet, Vector3d *baseline) {
  baseline->setZero();

  const View &view1 = *reconstruction_.View(std::get<0>(triplet));
  const View &view2 = *reconstruction_.View(std::get<1>(triplet));
  const View &view3 = *reconstruction_.View(std::get<2>(triplet));

  // Find common tracks.
  const std::vector<ViewId> triplet_view_ids = {
      std::get<0>(triplet), std::get<1>(triplet), std::get<2>(triplet)};
  const std::vector<TrackId> &common_tracks =
      FindCommonTracksInViews(reconstruction_, triplet_view_ids);

  // Normalize all features.
  std::vector<Vector3d> feature1, feature2, feature3;
  feature1.reserve(common_tracks.size());
  feature2.reserve(common_tracks.size());
  feature3.reserve(common_tracks.size());
  for (const TrackId track_id : common_tracks) {
    feature1.emplace_back(GetNormalizedFeature(view1, track_id).point_.homogeneous());
    feature2.emplace_back(GetNormalizedFeature(view2, track_id).point_.homogeneous());
    feature3.emplace_back(GetNormalizedFeature(view3, track_id).point_.homogeneous());
  }

  //  Eigen::Matrix3d R1 = view1.Camera().GetOrientationAsRotationMatrix();
  //  Eigen::Matrix3d R2 = view2.Camera().GetOrientationAsRotationMatrix();
  //  Eigen::Matrix3d R3 = view3.Camera().GetOrientationAsRotationMatrix();

  //  for (int i = 0; i < feature3.size(); ++i) {
  //    Matrix3d R31 = GetRij(R3, R1);
  //    Matrix3d R32 = GetRij(R3, R2);

  //    Vector3d a32 = Get_aij(R32, feature3[i], feature2[i]);

  //    Matrix3d B =
  //        GetSkew(feature1[i]) * R31 * feature3[i] * a32.transpose() * R2;

  //    double theta = GetThetaSq(feature3[i], feature2[i], R32);
  //    Matrix3d C = theta * GetSkew(feature1[i]) *
  //                 view1.Camera().GetOrientationAsRotationMatrix();

  //    Matrix3d D = -(B + C);

  //    Eigen::Vector3d shouldbenull = B * view2.Camera().GetPosition() +
  //                                   C * view1.Camera().GetPosition() +
  //                                   D * view3.Camera().GetPosition();

  //    std::cout << shouldbenull << "\n";
  //  }

  //  // Get the baseline ratios.
  //  ViewTriplet view_triplet;
  //  view_triplet.view_ids[0] = std::get<0>(triplet);
  //  view_triplet.view_ids[1] = std::get<1>(triplet);
  //  view_triplet.view_ids[2] = std::get<2>(triplet);
  //  view_triplet.info_one_two = FindOrDieNoPrint(
  //      *view_pairs_,
  //      ViewIdPair(view_triplet.view_ids[0], view_triplet.view_ids[1]));
  //  view_triplet.info_one_three = FindOrDieNoPrint(
  //      *view_pairs_,
  //      ViewIdPair(view_triplet.view_ids[0], view_triplet.view_ids[2]));
  //  view_triplet.info_two_three = FindOrDieNoPrint(
  //      *view_pairs_,
  //      ViewIdPair(view_triplet.view_ids[1], view_triplet.view_ids[2]));

  //  ComputeTripletBaselineRatios(
  //      view_triplet, feature1, feature2, feature3, baseline);
}

// Sets up the linear system with the constraints that each triplet adds.
void LiGTPositionEstimator::CreateLinearSystem(
    Eigen::SparseMatrix<double> *constraint_matrix) {
  const int num_views = num_triplets_for_view_.size();

  //  std::unordered_map<std::pair<int, int>, double> sparse_matrix_entries;
  //  sparse_matrix_entries.reserve(27 * num_triplets_for_view_.size());
  //  for (int i = 0; i < triplets_.size(); i++) {
  //    const ViewId &view_id1 = std::get<0>(triplets_[i]);
  //    const ViewId &view_id2 = std::get<1>(triplets_[i]);
  //    const ViewId &view_id3 = std::get<2>(triplets_[i]);
  //    AddTripletConstraintToSparseMatrix(view_id1, view_id2, view_id3,
  //                                       baselines_[i],
  //                                       &sparse_matrix_entries);
  //  }

  //  // Set the sparse matrix from the container of the accumulated entries.
  //  std::vector<Eigen::Triplet<double>> triplet_list;
  //  triplet_list.reserve(sparse_matrix_entries.size());
  //  for (const auto &sparse_matrix_entry : sparse_matrix_entries) {
  //    // Skip this entry if the indices are invalid. This only occurs when we
  //    // encounter a constraint with the constant camera (which has a view
  //    index
  //    // of -1).
  //    if (sparse_matrix_entry.first.first < 0 ||
  //        sparse_matrix_entry.first.second < 0) {
  //      continue;
  //    }
  //    triplet_list.emplace_back(sparse_matrix_entry.first.first,
  //                              sparse_matrix_entry.first.second,
  //                              sparse_matrix_entry.second);
  //  }

  //  // We construct the constraint matrix A^t * A directly, which is an
  //  // N - 1 x N - 1 matrix where N is the number of cameras (and 3 entries
  //  per
  //  // camera, corresponding to the camera position entries).
  //  constraint_matrix->resize((num_views - 1) * 3, (num_views - 1) * 3);
  //  constraint_matrix->setFromTriplets(triplet_list.begin(),
  //  triplet_list.end());
}

void LiGTPositionEstimator::ComputeRotatedRelativeTranslationRotations(
    const ViewId view_id0, const ViewId view_id1, const ViewId view_id2,
    Eigen::Matrix3d *r012, Eigen::Matrix3d *r201, Eigen::Matrix3d *r120) {
  // Relative camera positions.
  const Eigen::Vector3d &orientation0_aa =
      FindOrDieNoPrint(*orientations_, view_id0);
  const Eigen::Vector3d &orientation1_aa =
      FindOrDieNoPrint(*orientations_, view_id1);
  const Matrix3d orientation0 = AngleAxisToRotationMatrix(orientation0_aa);
  const Matrix3d orientation1 = AngleAxisToRotationMatrix(orientation1_aa);
  const Vector3d t01 =
      -orientation0.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id0, view_id1)).position_2;
  const Vector3d t02 =
      -orientation0.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id0, view_id2)).position_2;
  const Vector3d t12 =
      -orientation1.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id1, view_id2)).position_2;

  // Rotations between the translation vectors.
  *r012 = Eigen::Quaterniond::FromTwoVectors(t12, -t01).toRotationMatrix();
  *r201 = Eigen::Quaterniond::FromTwoVectors(t01, t02).toRotationMatrix();
  *r120 = Eigen::Quaterniond::FromTwoVectors(-t02, -t12).toRotationMatrix();
}

// Adds a triplet constraint to the linear system. The weight of the constraint
// (w), the global orientations, baseline (ratios), and view triplet information
// are needed to form the constraint.
void LiGTPositionEstimator::AddTripletConstraintToSparseMatrix(
    const ViewId view_id0, const ViewId view_id1, const ViewId view_id2,
    const Eigen::Vector3d &baselines,
    std::unordered_map<std::pair<int, int>, double> *sparse_matrix_entries) {
  // Weight each term by the inverse of the # of triplet that the nodes
  // participate in.
  const double w =
      1.0 / std::sqrt(std::min({num_triplets_for_view_[view_id0],
                                num_triplets_for_view_[view_id1],
                                num_triplets_for_view_[view_id2]}));

  // Get the index of each camera in the sparse matrix.
  const std::vector<int> view_indices = {
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id0)),
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id1)),
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id2))};

  // Compute B
  // Eigen::Matrix3d B = GetSkewFeature()

  // Compute the rotations between relative translations.
  Eigen::Matrix3d r012, r201, r120;
  ComputeRotatedRelativeTranslationRotations(view_id0, view_id1, view_id2,
                                             &r012, &r201, &r120);

  // Baselines ratios.
  const double s_012 = baselines[0] / baselines[2];
  const double s_201 = baselines[1] / baselines[0];
  const double s_120 = baselines[2] / baselines[1];

  // Assume that t01 is perfect and solve for c2.
  std::vector<Eigen::Matrix3d> constraints(3);
  constraints[0] =
      (-s_201 * r201 + r012.transpose() / s_012 + Matrix3d::Identity()) * w;
  constraints[1] =
      (s_201 * r201 - r012.transpose() / s_012 + Matrix3d::Identity()) * w;
  constraints[2] = -2.0 * w * Matrix3d::Identity();
  AddTripletConstraintToSymmetricMatrix(constraints, view_indices,
                                        sparse_matrix_entries);

  // Assume t02 is perfect and solve for c1.
  constraints[0] =
      (-r201.transpose() / s_201 + s_120 * r120 + Matrix3d::Identity()) * w;
  constraints[1] = -2.0 * w * Matrix3d::Identity();
  constraints[2] =
      (r201.transpose() / s_201 - s_120 * r120 + Matrix3d::Identity()) * w;
  AddTripletConstraintToSymmetricMatrix(constraints, view_indices,
                                        sparse_matrix_entries);

  // Assume t12 is perfect and solve for c0.
  constraints[0] = -2.0 * w * Matrix3d::Identity();
  constraints[1] =
      (-s_012 * r012 + r120.transpose() / s_120 + Matrix3d::Identity()) * w;
  constraints[2] =
      (s_012 * r012 - r120.transpose() / s_120 + Matrix3d::Identity()) * w;
  AddTripletConstraintToSymmetricMatrix(constraints, view_indices,
                                        sparse_matrix_entries);
}

Feature LiGTPositionEstimator::GetNormalizedFeature(const View& view,
                                                      const TrackId track_id) {
  Feature feature = *view.GetFeature(track_id);
  const Camera& camera = view.Camera();
  Eigen::Vector3d ray =
      camera.PixelToNormalizedCoordinates(feature.point_);
  Feature normalized_Feature(ray.hnormalized());
  // todo normalized covariance?
  return normalized_Feature;
}

void LiGTPositionEstimator::FlipSignOfPositionsIfNecessary(
    std::unordered_map<ViewId, Vector3d> *positions) {
  // If this value is below zero, then we should flip the sign.
  int correct_sign_votes = 0;
  for (const auto &view_pair : *view_pairs_) {
    // Only count the votes for edges where both positions were successfully
    // estimated.
    const Vector3d *position1 = FindOrNull(*positions, view_pair.first.first);
    const Vector3d *position2 = FindOrNull(*positions, view_pair.first.second);
    if (position1 == nullptr || position2 == nullptr) {
      continue;
    }

    // Check the relative translation of views 1 and 2 in the triplet.
    if (VectorsAreSameDirection(
            *position1, *position2,
            FindOrDieNoPrint(*orientations_, view_pair.first.first),
            view_pair.second.position_2)) {
      correct_sign_votes += 1;
    } else {
      correct_sign_votes -= 1;
    }
  }

  // If the sign of the votes is below zero, we must flip the sign of all
  // position estimates.
  if (correct_sign_votes < 0) {
    const int num_correct_votes =
        (view_pairs_->size() + correct_sign_votes) / 2;
    VLOG(2) << "Sign of the positions was incorrect: " << num_correct_votes
            << " of " << view_pairs_->size()
            << " relative translations had the correct sign. "
               "Flipping the sign of the camera positions.";
    for (auto &position : *positions) {
      position.second *= -1.0;
    }
  }
}

} // namespace theia


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/table.hpp>
#include <nt2/include/functions/sub2ind.hpp>
#include <nt2/include/functions/size.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/dispatch/meta/nth_hierarchy.hpp>

NT2_TEST_CASE( hierarchy )
{
  using boost::mpl::_;
  using boost::mpl::int_;
  using boost::dispatch::meta::nth_hierarchy;

  NT2_TEST_EXPR_TYPE( (nt2::tag::sub2ind_() )
                    , (nth_hierarchy<_,int_<0> >)
                    , (nt2::tag::sub2ind_)
                    );

  NT2_TEST_EXPR_TYPE( (nt2::tag::sub2ind_() )
                    , (nth_hierarchy<_,int_<1> >)
                    , (nt2::ext::elementwise_< nt2::tag::sub2ind_ >
                      )
                    );

  NT2_TEST_EXPR_TYPE( (nt2::tag::sub2ind_() )
                    , (nth_hierarchy<_,int_<2> >)
                    , (nt2::ext::unspecified_<nt2::tag::sub2ind_>)
                    );
}

NT2_TEST_CASE_TPL( value_type, NT2_TYPES )
{
  using boost::mpl::_;
  using nt2::table;
  using nt2::size;
  using nt2::of_size;
  using nt2::meta::value_type_;

  nt2::table<T>   ss( of_size(4,4) );

  // Size with an extent
  NT2_TEST_EXPR_TYPE( ( nt2::sub2ind(of_size(4,4),T(1),T(1)) )
                    , (value_type_<_>)
                    , (T)
                    );

  NT2_TEST_EXPR_TYPE( (nt2::sub2ind(of_size(4,4),ss,ss) )
                    , (value_type_<_>)
                    , (T)
                    );

  // Size with a size
  NT2_TEST_EXPR_TYPE( ( nt2::sub2ind(size(ss),T(1),T(1)) )
                    , (value_type_<_>)
                    , (T)
                    );

  NT2_TEST_EXPR_TYPE( ( nt2::sub2ind(size(ss),ss,ss) )
                    , (value_type_<_>)
                    , (T)
                    );
}

NT2_TEST_CASE_TPL( sub2ind_2D, NT2_TYPES )
{
  using nt2::sub2ind;
  using nt2::of_size;

  nt2::_2D sz = of_size(7,5);
  nt2::table<T> x( of_size(1,35) );
  nt2::table<T> y( of_size(1,35) );

  for(std::size_t i=1;i<=nt2::numel(sz);i++) x(i) = (i-1)%7+1;
  for(std::size_t i=1;i<=nt2::numel(sz);i++) y(i) = (i-1)/7+1;

  NT2_TEST_EQUAL( nt2::extent(sub2ind(sz,x,y)), nt2::extent(x) );
  NT2_TEST_EQUAL( sub2ind(sz,x,y), nt2::_(T(1),T(nt2::numel(sz))) );
}

NT2_TEST_CASE_TPL( sub2ind_3D, NT2_TYPES )
{
  using nt2::sub2ind;
  using nt2::of_size;

  nt2::_3D sz = of_size(3,4,3);
  nt2::table<T> x( of_size(1,36) );
  nt2::table<T> y( of_size(1,36) );
  nt2::table<T> z( of_size(1,36) );

  for(std::size_t i=1;i<=nt2::numel(sz);i++) x(i) = (i-1)%3+1;
  for(std::size_t i=1;i<=nt2::numel(sz);i++) y(i) = ((i-1)/3)%4+1;
  for(std::size_t i=1;i<=nt2::numel(sz);i++) z(i) = (i-1)/12+1;

  NT2_TEST_EQUAL( nt2::extent(sub2ind(sz,x,y,z)), nt2::extent(x) );
  NT2_TEST_EQUAL( sub2ind(sz,x,y,z), nt2::_(T(1),T(nt2::numel(sz))) );
}

NT2_TEST_CASE_TPL( sub2ind_4D, NT2_TYPES )
{
  using nt2::sub2ind;
  using nt2::of_size;

  nt2::_4D sz = of_size(3,2,3,2);
  nt2::table<T> x( of_size(1,36) );
  nt2::table<T> y( of_size(1,36) );
  nt2::table<T> z( of_size(1,36) );
  nt2::table<T> w( of_size(1,36) );

  for(std::size_t i=1;i<=nt2::numel(sz);i++) x(i) = (i-1)%3+1;
  for(std::size_t i=1;i<=nt2::numel(sz);i++) y(i) = ((i-1)/3)%2+1;
  for(std::size_t i=1;i<=nt2::numel(sz);i++) z(i) = ((i-1)/6)%3+1;
  for(std::size_t i=1;i<=nt2::numel(sz);i++) w(i) = ((i-1)/18)+1;

  NT2_TEST_EQUAL( nt2::extent(sub2ind(sz,x,y,z,w)), nt2::extent(x) );
  NT2_TEST_EQUAL( sub2ind(sz,x,y,z,w), nt2::_(T(1),T(nt2::numel(sz))) );
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/functional.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>

#include <tuple>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto ...x) {
    return std::make_tuple(1, x...);
};

BOOST_HANA_CONSTEXPR_LAMBDA auto g = [](auto ...x) {
    return std::make_tuple(2, x...);
};

BOOST_HANA_CONSTEXPR_LAMBDA auto h = [](auto ...x) {
    return std::make_tuple(3, x...);
};

template <int i>
constexpr auto x = detail::minimal::comparable<>(i);

int main() {
    BOOST_HANA_CONSTEXPR_ASSERT(compose(f, g)(x<0>) == f(g(x<0>)));
    BOOST_HANA_CONSTEXPR_ASSERT(compose(f, g)(x<0>, x<1>) == f(g(x<0>), x<1>));
    BOOST_HANA_CONSTEXPR_ASSERT(compose(f, g)(x<0>, x<1>, x<2>) == f(g(x<0>), x<1>, x<2>));

    BOOST_HANA_CONSTEXPR_ASSERT(compose(f, g, h)(x<0>) == f(g(h(x<0>))));
    BOOST_HANA_CONSTEXPR_ASSERT(compose(f, g, h)(x<0>, x<1>) == f(g(h(x<0>)), x<1>));
    BOOST_HANA_CONSTEXPR_ASSERT(compose(f, g, h)(x<0>, x<1>, x<2>) == f(g(h(x<0>)), x<1>, x<2>));
}


//
// Created by yche on 1/22/18.
//

#include <boost/program_options.hpp>

#include  "ground_truth/simrank.h"
#include "local_push/local_push_yche.h"

using namespace std;
using namespace boost::program_options;

int main(int argc, char *argv[]) {
    FILE *log_f = nullptr;
    if (argc >= 5) {
        log_f = fopen(argv[4], "a+");
        log_set_fp(log_f);
    }
    // 1st: compute reduce local push
    string g_name = argv[1];
    double eps = atof(argv[2]);
    string path = get_edge_list_path(g_name);
    GraphYche g(path);
    double c = atof(argv[3]);
//    double eps = 0.0001;

    auto n = g.n;
#ifdef FLP
    auto lp = new Full_LocalPush(g, g_name, c, eps, n);
#else
    auto lp = new Reduced_LocalPush(g, g_name, c, eps, n);
#endif
    auto start_time = std::chrono::high_resolution_clock::now();
    lp->local_push(g);
    auto end_time = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = end_time - start_time;
    cout << format("computation time: %s s") % elapsed.count() << endl; // record the pre-processing time
    cout << format("push num: %s") % lp->n_push << endl;
    cout << format("max queue size #: %s") % (lp->n_push / lp->max_q_size) << endl;
    cout << format("mem size: %s KB") % getValue() << endl;

    cout << format("P size: %s") % lp->P.size() << endl;
    cout << format("R size: %s") % lp->R.size() << endl;
    cout << format("avg push merge #: %s") % (lp->n_push / lp->R.size()) << endl;

    log_info("Computation Time: %.9lfs", elapsed.count());
    log_info("Final Memory Consumption: %.9lf MB", getValue() / 1024.0);
    // 2nd: verify the correcness
    if (n < 10000) {
        TruthSim ts(g_name, g, c, eps);
        auto max_err = 0.0;
        double err = 0;

#pragma omp parallel for reduction(max:max_err), reduction(+:err) schedule(dynamic, 1)
        for (auto i = 0u; i < n; i++) {
            for (auto j = i; j < n; j++) {
                auto res = lp->query_P(i, j);
                max_err = max(max_err, abs(ts.sim(i, j) - res));
                err += abs(ts.sim(i, j) - res);

                if (abs(ts.sim(i, j) - res) > eps + pow(10, -6)) {
#pragma omp critical
                    cout << i << ", " << j << "," << ts.sim(i, j) << "," << res << endl;
                }
            }
        }
        cout << "max err:" << max_err << endl;
        cout << "mean err:" << (err / ((n + 1) * n / 2)) << endl;
    };

    if (log_f != nullptr) {
        log_info("Flush File and Close...");
        fflush(log_f);
        fclose(log_f);
    }
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 1, 1> test_double_1_1(const Eigen::Matrix<double, 1, 1> & M)
{
	return M;
}
void export_double_1_1()
{
	boost::python::def("test_double_1_1",test_double_1_1);
}