/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/builders/swap.hpp>
#include <ored/portfolio/enginedata.hpp>
#include <ored/portfolio/portfolio.hpp>
#include <ored/portfolio/swap.hpp>
#include <ored/utilities/indexparser.hpp>
#include <oret/toplevelfixture.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/unitedstates.hpp>
#include <ql/time/daycounters/actual365fixed.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <qle/cashflows/equitycoupon.hpp>
#include <qle/indexes/equityindex.hpp>

using namespace QuantLib;
using namespace QuantExt;
using namespace boost::unit_test_framework;
using namespace ore::data;
using std::vector;

namespace {

class TestMarket : public MarketImpl {
public:
    TestMarket() {
        // valuation date
        asof_ = Date(18, July, 2016);

        spotSP5 = Handle<Quote>(boost::shared_ptr<SimpleQuote>(new SimpleQuote(2100)));
        forecastSP5 = flatRateYts(0.03);
        dividendSP5 = flatRateYts(0.01);

        // build vectors with dates and discount factors for GBP
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "USD")] = forecastSP5;
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Yield, "SP5")] = forecastSP5;
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::EquityDividend, "SP5")] = dividendSP5;

        // build USD Libor index
        hUSD = Handle<IborIndex>(parseIborIndex("USD-LIBOR-3M", flatRateYts(0.035)));
        iborIndices_[make_pair(Market::defaultConfiguration, "USD-LIBOR-3M")] = hUSD;

        // build SP5 Equity Curve
        hSP5 = Handle<EquityIndex>(
            boost::shared_ptr<EquityIndex>(new EquityIndex("SP5", UnitedStates(), parseCurrency("USD"), spotSP5, forecastSP5, dividendSP5)));
        equityCurves_[make_pair(Market::defaultConfiguration, "SP5")] = hSP5;

        // add fixings
        hUSD->addFixing(Date(14, July, 2016), 0.035);
    }

    Handle<IborIndex> hUSD;
    Handle<EquityIndex> hSP5;
    Handle<Quote> spotSP5;
    Handle<YieldTermStructure> forecastSP5;
    Handle<YieldTermStructure> dividendSP5;

private:
    Handle<YieldTermStructure> flatRateYts(Real forward) {
        boost::shared_ptr<YieldTermStructure> yts(new FlatForward(0, UnitedStates(), forward, ActualActual()));
        return Handle<YieldTermStructure>(yts);
    }
};

struct CommonVars {
    // global data
    string ccy;
    bool isPayer;
    string start;
    string end;
    string floattenor;
    string eqtenor;
    string calStr;
    string conv;
    string rule;
    Natural days;
    string fixDC;
    string index;
    string eqName;
    Real dividendFactor;
    Real initialPrice;
    Integer settlementDays;
    bool isinarrears;
    vector<double> notionals;
    vector<double> spread;

    // utilities
    boost::shared_ptr<ore::data::Swap> makeEquitySwap(string returnType) {
        ScheduleData floatSchedule(ScheduleRules(start, end, floattenor, calStr, conv, conv, rule));
        ScheduleData eqSchedule(ScheduleRules(start, end, eqtenor, calStr, conv, conv, rule));

        // build EquitySwap
        LegData floatLegData(boost::make_shared<FloatingLegData>(index, days, isinarrears, spread), !isPayer, ccy,
                             floatSchedule, fixDC, notionals);
        LegData eqLegData(boost::make_shared<EquityLegData>(returnType, dividendFactor, eqName, initialPrice, false, settlementDays),
                          isPayer, ccy, eqSchedule, fixDC, notionals);

        Envelope env("CP1");
        boost::shared_ptr<ore::data::Swap> swap(new ore::data::Swap(env, floatLegData, eqLegData));
        return swap;
    }

    boost::shared_ptr<QuantLib::Swap> qlEquitySwap(string returnType) {

        boost::shared_ptr<TestMarket> market = boost::make_shared<TestMarket>();
        // check Equity swap NPV against pure QL pricing
        Schedule floatSchedule(parseDate(start), parseDate(end), parsePeriod(floattenor), parseCalendar(calStr),
                               parseBusinessDayConvention(conv), parseBusinessDayConvention(conv),
                               parseDateGenerationRule(rule), false);
        Schedule eqSchedule(parseDate(start), parseDate(end), parsePeriod(eqtenor), parseCalendar(calStr),
                            parseBusinessDayConvention(conv), parseBusinessDayConvention(conv),
                            parseDateGenerationRule(rule), false);
        Leg floatLeg = IborLeg(floatSchedule, *market->hUSD)
                           .withNotionals(notionals)
                           .withFixingDays(days)
                           .withSpreads(spread)
                           .withPaymentDayCounter(parseDayCounter(fixDC))
                           .withPaymentAdjustment(parseBusinessDayConvention(conv));
        Leg eqLeg = EquityLeg(eqSchedule, *market->equityCurve("SP5"))
                        .withNotionals(notionals)
                        .withPaymentDayCounter(parseDayCounter(fixDC))
                        .withPaymentAdjustment(parseBusinessDayConvention(conv))
                        .withTotalReturn(returnType == "Total")
                        .withInitialPrice(initialPrice)
                        .withNotionalReset(false);

        boost::shared_ptr<QuantLib::Swap> swap(new QuantLib::Swap(floatLeg, eqLeg));
        return swap;
    }

    CommonVars() {
        ccy = "USD";
        isPayer = false;
        start = "20160718";
        end = "20210718";
        floattenor = "3M";
        eqtenor = "3M";
        calStr = "USD";
        conv = "MF";
        rule = "Forward";
        fixDC = "ACT/ACT";
        index = "USD-LIBOR-3M";
        eqName = "SP5";
        dividendFactor = 1.0;
        initialPrice = 2100.0;
        settlementDays = 0;
        days = 0;
        isinarrears = false;
        notionals.push_back(10000000);
        spread.push_back(0.0);
    }
};

} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(EquitySwapTests)

BOOST_AUTO_TEST_CASE(testEquitySwapPriceReturn) {

    BOOST_TEST_MESSAGE("Testing Equity Swap Price Return...");

    // build market
    boost::shared_ptr<TestMarket> market = boost::make_shared<TestMarket>();
    Date today = market->asofDate();
    Settings::instance().evaluationDate() = today;

    CommonVars vars;
    boost::shared_ptr<ore::data::Swap> eqSwap = vars.makeEquitySwap("Price");

    // engine data and factory
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("Swap") = "DiscountedCashflows";
    engineData->engine("Swap") = "DiscountingSwapEngine";
    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    engineFactory->registerBuilder(boost::make_shared<SwapEngineBuilder>());

    // build swaps and portfolio
    boost::shared_ptr<Portfolio> portfolio(new Portfolio());
    eqSwap->id() = "EQ_Swap";

    portfolio->add(eqSwap);
    portfolio->build(engineFactory);

    boost::shared_ptr<QuantLib::Swap> qlSwap = vars.qlEquitySwap("Price");

    auto dscEngine = boost::make_shared<DiscountingSwapEngine>(market->discountCurve("USD"));
    qlSwap->setPricingEngine(dscEngine);
    BOOST_TEST_MESSAGE("Leg 1 NPV: ORE = "
                       << boost::static_pointer_cast<QuantLib::Swap>(eqSwap->instrument()->qlInstrument())->legNPV(0)
                       << " QL = " << qlSwap->legNPV(0));
    BOOST_TEST_MESSAGE("Leg 2 NPV: ORE = "
                       << boost::static_pointer_cast<QuantLib::Swap>(eqSwap->instrument()->qlInstrument())->legNPV(1)
                       << " QL = " << qlSwap->legNPV(1));
    BOOST_CHECK_CLOSE(eqSwap->instrument()->NPV(), qlSwap->NPV(), 1E-8); // this is 1E-10 rel diff
}

BOOST_AUTO_TEST_CASE(testEquitySwapTotalReturn) {

    BOOST_TEST_MESSAGE("Testing Equity Swap Total Return...");

    // build market
    boost::shared_ptr<TestMarket> market = boost::make_shared<TestMarket>();
    Date today = market->asofDate();
    Settings::instance().evaluationDate() = today;

    CommonVars vars;
    boost::shared_ptr<ore::data::Swap> eqSwap = vars.makeEquitySwap("Total");

    // engine data and factory
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("Swap") = "DiscountedCashflows";
    engineData->engine("Swap") = "DiscountingSwapEngine";
    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    engineFactory->registerBuilder(boost::make_shared<SwapEngineBuilder>());

    // build swaps and portfolio
    boost::shared_ptr<Portfolio> portfolio(new Portfolio());
    eqSwap->id() = "EQ_Swap";

    portfolio->add(eqSwap);
    portfolio->build(engineFactory);

    boost::shared_ptr<QuantLib::Swap> qlSwap = vars.qlEquitySwap("Total");

    auto dscEngine = boost::make_shared<DiscountingSwapEngine>(market->discountCurve("USD"));
    qlSwap->setPricingEngine(dscEngine);
    BOOST_TEST_MESSAGE("Leg 1 NPV: ORE = "
                       << boost::static_pointer_cast<QuantLib::Swap>(eqSwap->instrument()->qlInstrument())->legNPV(0)
                       << " QL = " << qlSwap->legNPV(0));
    BOOST_TEST_MESSAGE("Leg 2 NPV: ORE = "
                       << boost::static_pointer_cast<QuantLib::Swap>(eqSwap->instrument()->qlInstrument())->legNPV(1)
                       << " QL = " << qlSwap->legNPV(1));
    BOOST_CHECK_CLOSE(eqSwap->instrument()->NPV(), qlSwap->NPV(), 1E-8);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#ifndef MLT_MODELS_REGRESSORS_OPTIMIZABLE_LINEAR_REGRESSOR_HPP
#define MLT_MODELS_REGRESSORS_OPTIMIZABLE_LINEAR_REGRESSOR_HPP

#include <type_traits>

#include <Eigen/Core>

#include "../optimizable_linear_model.hpp"
#include "linear_regressor.hpp"

namespace mlt {
namespace models {
namespace regressors {
	template <class Loss, class Optimizer>
	class OptimizableLinearRegressor : public OptimizableLinearModel<LinearRegressor<OptimizableLinearRegressor<Loss, Optimizer>>, Loss, Optimizer> {
	public:
		template <class L, class O, class = enable_if<is_same<decay_t<L>, Loss>::value && is_convertible<decay_t<O>, Optimizer>::value>>
		explicit OptimizableLinearRegressor(L&& loss, O&& optimizer, double regularization,
			bool fit_intercept = true) : OptimizableLinearModel(forward<L>(loss), forward<O>(optimizer), regularization, fit_intercept) {}
	};
}
}
}
#endif


#include <cstring>
#include <fstream>
#include <unistd.h>

#include <NTL/ZZX.h>
#include <NTL/vector.h>

#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"

/**
 *  키를 생성하여 파일로 내보내는 모듈.
 *
 *  원래 RSA 방식과는 다르게 비밀키에서 공개키를 생성하는 구조임.
 *  공개키로 암호화를 하고, 비밀키로 복호화를 함.
 *
 *  키마다 txt파일(ascii)과 bin파일의 두 파일로 내보내짐
 *  공개키 : FHEcontext와 FHEPubKey 두 객체를 내보냄
 *  비밀키 : FHEcontext, FHESecKey, FHEPubKey 세 객체를 내보냄
 *
 */
int main(int argc, char *argv[]) {

    ArgMapping amap;

    long r = 1;
    long c = 2;
    long w = 64;
    long k = 80;
    long d = 1;

    long p = 257;
    long L = 8;
    string owner = "owner";
    string dir = "data";

    amap.arg("p", p, "plaintext base");
    amap.arg("L", L, "number of levels wanted");
    amap.arg("o", owner, "owner's address");
    amap.arg("dir", dir, "save directory");
    amap.parse(argc, argv);

    // file names
    const string secretKeyBinaryFile = dir + "/secretKey/" + owner + ".bin";
    const string publicKeyBinaryFile = dir + "/publicKey/" + owner + ".bin";

    ofstream secretBinFile(secretKeyBinaryFile.c_str(), ios::binary);
    assert(secretBinFile.is_open());

    ofstream publicBinFile(publicKeyBinaryFile.c_str(), ios::binary);
    assert(publicBinFile.is_open());

    // create context
    long m = FindM(k, L, c, p, d, 0, 0);
    std::unique_ptr<FHEcontext> context(new FHEcontext(m, p, r));
    buildModChain(*context, L, c);  // Set the modulus chain

    // create key
    std::unique_ptr<FHESecKey> secKey(new FHESecKey(*context));
    FHEPubKey *pubKey = (FHEPubKey *) secKey.get();
    secKey->GenSecKey(w);
    addSome1DMatrices(*secKey);
    addFrbMatrices(*secKey);

    // Secret Bin
    cout << "\tWriting Secret Binary file " << secretKeyBinaryFile << endl;
    writeContextBaseBinary(secretBinFile, *context);
    writeContextBinary(secretBinFile, *context);
    writePubKeyBinary(secretBinFile, *pubKey);
    writeSecKeyBinary(secretBinFile, *secKey);

    // Public Bin
    cout << "\tWriting Public Binary file " << publicKeyBinaryFile << endl;
    writeContextBaseBinary(publicBinFile, *context);
    writeContextBinary(publicBinFile, *context);
    writePubKeyBinary(publicBinFile, *pubKey);

    secretBinFile.close();
    publicBinFile.close();

    cout << "createKey successful.\n\n";
}


/*
 * Copyright (c) 2015-2016, Luca Fulchir<luca@fulchir.it>, All rights reserved.
 *
 * This file is part of "libRaptorQ".
 *
 * libRaptorQ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation, either version 3
 * of the License, or (at your option) any later version.
 *
 * libRaptorQ is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * and a copy of the GNU Lesser General Public License
 * along with libRaptorQ.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#include "RaptorQ/v1/util/Bitmask.hpp"
#include "RaptorQ/v1/common.hpp"
#include "RaptorQ/v1/multiplication.hpp"
#include "RaptorQ/v1/Operation.hpp"
#include "RaptorQ/v1/Octet.hpp"
#include "RaptorQ/v1/Parameters.hpp"
#include "RaptorQ/v1/Thread_Pool.hpp"
#include <Eigen/Dense>
#include <deque>
#include <memory>

namespace RaptorQ__v1 {
namespace Impl {

using DenseMtx = Eigen::Matrix<Octet, Eigen::Dynamic, Eigen::Dynamic,
                                                            Eigen::RowMajor>;
enum class RAPTORQ_API Save_Computation : uint8_t {
    OFF = 0,
    ON = 1
};

enum class RAPTORQ_LOCAL Precode_Result : uint8_t {
    DONE = 0,
    STOPPED = 1,
    FAILED = 2
};

template<Save_Computation IS_OFFLINE>
class RAPTORQ_API Precode_Matrix
{
    using Op_Vec = std::deque<Operation>;
public:
    const Parameters _params;

    Precode_Matrix(const Parameters &params)
        :_params (params)
    {}
    Precode_Matrix() = delete;
    Precode_Matrix (const Precode_Matrix&) = default;
    Precode_Matrix& operator= (const Precode_Matrix&) = default;
    Precode_Matrix (Precode_Matrix&&) = default;
    Precode_Matrix& operator= (Precode_Matrix&&) = default;
    ~Precode_Matrix() = default;

    void gen (const uint32_t repair_overhead);

    std::pair<Precode_Result, DenseMtx> intermediate (DenseMtx &D, Op_Vec &ops,
                                        bool &keep_working,
                                        const Work_State *thread_keep_working);
    std::pair<Precode_Result, DenseMtx> intermediate (DenseMtx &D,
                                        const Bitmask &mask,
                                        const std::vector<uint32_t> &repair_esi,
                                        Op_Vec &ops, bool &keep_working,
                                        const Work_State *thread_keep_working);
    DenseMtx get_missing (const DenseMtx &C, const Bitmask &mask) const;
    DenseMtx encode (const DenseMtx &C, const uint32_t ISI) const;

private:
    DenseMtx A;
    uint32_t _repair_overhead = 0;

    // indenting here prepresent which function needs which other.
    // not standard, ask me if I care.
    void init_LDPC1 (DenseMtx &_A, const uint16_t S, const uint16_t B) const;
    void init_LDPC2 (DenseMtx &_A, const uint16_t skip, const uint16_t rows,
                                                    const uint16_t cols) const;
    void add_identity (DenseMtx &_A, const uint16_t size,
                                                const uint16_t skip_row,
                                                const uint16_t skip_col) const;

    void init_HDPC (DenseMtx &_A) const;
        DenseMtx make_MT() const;       // rfc 6330, pgg 24, used for HDPC
        DenseMtx make_GAMMA() const;    // rfc 6330, pgg 24, used for HDPC
    void add_G_ENC (DenseMtx &_A) const;

    //DenseMtx intermediate (DenseMtx &D, Op_Vec &ops, bool &keep_working);
    void decode_phase0 (const Bitmask &mask,
                                    const std::vector<uint32_t> &repair_esi);
    std::tuple<bool, uint16_t, uint16_t> decode_phase1 (DenseMtx &X,DenseMtx &D,
                                        std::vector<uint16_t> &c,
                                        Op_Vec &ops, bool &keep_working,
                                        const Work_State *thread_keep_working);
    bool decode_phase2 (DenseMtx &D, const uint16_t i,const uint16_t u,
                                        Op_Vec &ops, bool &keep_working,
                                        const Work_State *thread_keep_working);
    void decode_phase3 (const DenseMtx &X, DenseMtx &D, const uint16_t i,
                                        Op_Vec &ops);
    void decode_phase4 (DenseMtx &D, const uint16_t i, const uint16_t u,
                                        Op_Vec &ops, bool &keep_working,
                                        const Work_State *thread_keep_working);
    void decode_phase5 (DenseMtx &D, const uint16_t i, Op_Vec &ops,
                                        bool &keep_working,
                                        const Work_State *thread_keep_working);

    inline bool stop (bool keep_working, const Work_State *thread_keep_working)
    {
        return false == keep_working ||
                            Work_State::KEEP_WORKING != *thread_keep_working;
    }
};

}   // namespace Impl
}   // namespace RaptorQ

#include "Precode_Matrix_Init.hpp" // above template implementation
#include "Precode_Matrix_Solver.hpp" // above template implementation


/**
 * Some part of the source code is from the SHOC benchmark suite.
 *
 * Original authors:
 * Jeremy Meredith
 */

#include <boost/math/special_functions/next.hpp>
#include <boost/program_options.hpp>
#include <cstdlib>
#include <iostream>
#include <tuple>

#include <omp.h>
#include <sys/mman.h>

#include "../device_hopper/core.h"

using namespace device_hopper;

#define REPOSITORY_PATH std::string(std::getenv("PLASTICITY_ROOT"))
#define PREFERRED_DEVICE GPU

#define LEFTROTATE(x, c) (((x) << (c)) | ((x) >> (32 - (c))))
#define F(x, y, z) ((x & y) | ((~x) & z))
#define G(x, y, z) ((x & z) | ((~z) & y))
#define H(x, y, z) (x ^ y ^ z)
#define I(x, y, z) (y ^ (x | (~z)))

// This version ignores the mapping of a/b/c/d to v/x/y/z and simply
// uses a temporary variable to keep the interpretation of a/b/c/d
// consistent.  Whether this one or the previous one performs better
// probably depends on the compiler....
#define ROUND_USING_TEMP_VARS(w, r, k, v, x, y, z, func)         \
{                                                                \
    a = a + func(b,c,d) + k + w;                                 \
    unsigned int temp = d;                                       \
    d = c;                                                       \
    c = b;                                                       \
    b = b + LEFTROTATE(a, r);                                    \
    a = temp;                                                    \
}

// Here, we pick which style of ROUND we use.
#define ROUND ROUND_USING_TEMP_VARS

void IndexToKey(unsigned int index,
                int byteLength,
                int valsPerByte,
                unsigned char vals[8]) {
    // loop pointlessly unrolled to avoid CUDA compiler complaints
    // about unaligned accesses (!?) on older compute capabilities
    vals[0] = index % valsPerByte;
    index /= valsPerByte;

    vals[1] = index % valsPerByte;
    index /= valsPerByte;

    vals[2] = index % valsPerByte;
    index /= valsPerByte;

    vals[3] = index % valsPerByte;
    index /= valsPerByte;

    vals[4] = index % valsPerByte;
    index /= valsPerByte;

    vals[5] = index % valsPerByte;
    index /= valsPerByte;

    vals[6] = index % valsPerByte;
    index /= valsPerByte;

    vals[7] = index % valsPerByte;
    //index /= valsPerByte;
}

int FindKeyspaceSize(int byteLength, int valsPerByte) {
    int keyspace = 1;
    for (int i = 0; i < byteLength; ++i) {
        if (keyspace >= 0x7fffffff / valsPerByte) {
            // error, we're about to overflow a signed int
            return -1;
        }
        keyspace *= valsPerByte;
    }
    return keyspace;
}

inline void md5_2words(unsigned int *words,
                       unsigned int len,
                       unsigned int *digest) {
    // For any block but the first one, these should be passed in, not
    // initialized, but we are assuming we only operate on a single block.
    unsigned int h0 = 0x67452301;
    unsigned int h1 = 0xefcdab89;
    unsigned int h2 = 0x98badcfe;
    unsigned int h3 = 0x10325476;

    unsigned int a = h0;
    unsigned int b = h1;
    unsigned int c = h2;
    unsigned int d = h3;

    unsigned int WL = len * 8;
    unsigned int W0 = words[0];
    unsigned int W1 = words[1];

    switch (len) {
        case 0:
            W0 |= 0x00000080;
            break;
        case 1:
            W0 |= 0x00008000;
            break;
        case 2:
            W0 |= 0x00800000;
            break;
        case 3:
            W0 |= 0x80000000;
            break;
        case 4:
            W1 |= 0x00000080;
            break;
        case 5:
            W1 |= 0x00008000;
            break;
        case 6:
            W1 |= 0x00800000;
            break;
        case 7:
            W1 |= 0x80000000;
            break;
    }

    // args: word data, per-round shift amt, constant, 4 vars, function macro
    ROUND(W0, 7, 0xd76aa478, a, b, c, d, F);
    ROUND(W1, 12, 0xe8c7b756, d, a, b, c, F);
    ROUND(0, 17, 0x242070db, c, d, a, b, F);
    ROUND(0, 22, 0xc1bdceee, b, c, d, a, F);
    ROUND(0, 7, 0xf57c0faf, a, b, c, d, F);
    ROUND(0, 12, 0x4787c62a, d, a, b, c, F);
    ROUND(0, 17, 0xa8304613, c, d, a, b, F);
    ROUND(0, 22, 0xfd469501, b, c, d, a, F);
    ROUND(0, 7, 0x698098d8, a, b, c, d, F);
    ROUND(0, 12, 0x8b44f7af, d, a, b, c, F);
    ROUND(0, 17, 0xffff5bb1, c, d, a, b, F);
    ROUND(0, 22, 0x895cd7be, b, c, d, a, F);
    ROUND(0, 7, 0x6b901122, a, b, c, d, F);
    ROUND(0, 12, 0xfd987193, d, a, b, c, F);
    ROUND(WL, 17, 0xa679438e, c, d, a, b, F);
    ROUND(0, 22, 0x49b40821, b, c, d, a, F);

    ROUND(W1, 5, 0xf61e2562, a, b, c, d, G);
    ROUND(0, 9, 0xc040b340, d, a, b, c, G);
    ROUND(0, 14, 0x265e5a51, c, d, a, b, G);
    ROUND(W0, 20, 0xe9b6c7aa, b, c, d, a, G);
    ROUND(0, 5, 0xd62f105d, a, b, c, d, G);
    ROUND(0, 9, 0x02441453, d, a, b, c, G);
    ROUND(0, 14, 0xd8a1e681, c, d, a, b, G);
    ROUND(0, 20, 0xe7d3fbc8, b, c, d, a, G);
    ROUND(0, 5, 0x21e1cde6, a, b, c, d, G);
    ROUND(WL, 9, 0xc33707d6, d, a, b, c, G);
    ROUND(0, 14, 0xf4d50d87, c, d, a, b, G);
    ROUND(0, 20, 0x455a14ed, b, c, d, a, G);
    ROUND(0, 5, 0xa9e3e905, a, b, c, d, G);
    ROUND(0, 9, 0xfcefa3f8, d, a, b, c, G);
    ROUND(0, 14, 0x676f02d9, c, d, a, b, G);
    ROUND(0, 20, 0x8d2a4c8a, b, c, d, a, G);

    ROUND(0, 4, 0xfffa3942, a, b, c, d, H);
    ROUND(0, 11, 0x8771f681, d, a, b, c, H);
    ROUND(0, 16, 0x6d9d6122, c, d, a, b, H);
    ROUND(WL, 23, 0xfde5380c, b, c, d, a, H);
    ROUND(W1, 4, 0xa4beea44, a, b, c, d, H);
    ROUND(0, 11, 0x4bdecfa9, d, a, b, c, H);
    ROUND(0, 16, 0xf6bb4b60, c, d, a, b, H);
    ROUND(0, 23, 0xbebfbc70, b, c, d, a, H);
    ROUND(0, 4, 0x289b7ec6, a, b, c, d, H);
    ROUND(W0, 11, 0xeaa127fa, d, a, b, c, H);
    ROUND(0, 16, 0xd4ef3085, c, d, a, b, H);
    ROUND(0, 23, 0x04881d05, b, c, d, a, H);
    ROUND(0, 4, 0xd9d4d039, a, b, c, d, H);
    ROUND(0, 11, 0xe6db99e5, d, a, b, c, H);
    ROUND(0, 16, 0x1fa27cf8, c, d, a, b, H);
    ROUND(0, 23, 0xc4ac5665, b, c, d, a, H);

    ROUND(W0, 6, 0xf4292244, a, b, c, d, I);
    ROUND(0, 10, 0x432aff97, d, a, b, c, I);
    ROUND(WL, 15, 0xab9423a7, c, d, a, b, I);
    ROUND(0, 21, 0xfc93a039, b, c, d, a, I);
    ROUND(0, 6, 0x655b59c3, a, b, c, d, I);
    ROUND(0, 10, 0x8f0ccc92, d, a, b, c, I);
    ROUND(0, 15, 0xffeff47d, c, d, a, b, I);
    ROUND(W1, 21, 0x85845dd1, b, c, d, a, I);
    ROUND(0, 6, 0x6fa87e4f, a, b, c, d, I);
    ROUND(0, 10, 0xfe2ce6e0, d, a, b, c, I);
    ROUND(0, 15, 0xa3014314, c, d, a, b, I);
    ROUND(0, 21, 0x4e0811a1, b, c, d, a, I);
    ROUND(0, 6, 0xf7537e82, a, b, c, d, I);
    ROUND(0, 10, 0xbd3af235, d, a, b, c, I);
    ROUND(0, 15, 0x2ad7d2bb, c, d, a, b, I);
    ROUND(0, 21, 0xeb86d391, b, c, d, a, I);

    h0 += a;
    h1 += b;
    h2 += c;
    h3 += d;

    // write the final result out
    digest[0] = h0;
    digest[1] = h1;
    digest[2] = h2;
    digest[3] = h3;
}

DEVICE_HOPPER_MAIN(int argc, char *argv[]) {
    DEVICE_HOPPER_SETUP

    // Parse CLI parameters
    boost::program_options::options_description desc("Options");
    desc.add_options()("problem-size", boost::program_options::value<long>(), "Sample count");
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
    size_t problem_size = 0;
    if (vm.count("problem-size") == 0) {
        std::cerr << "Error: Problem size is missing." << std::endl;
        std::exit(EXIT_FAILURE);
    } else {
        // This parameter is passed to the application by the Python scripts that automate the experiments.
        problem_size = vm["problem-size"].as<long>();
    }

#if defined(SOURCE_TO_SOURCE_TRANSLATED)
    // This sets the number of blocks in a slice as slice size as expected by the parallel for.
    // The hand implementation uses the number of iteration points in a slice as slice size.
    // To keep the database tables and configs consistent we continue to use the old slice sizes in terms of iteration points
    // in the configs and databases but convert them then here in the source code.
    context.sched.set_cpu_slice_sizes(context.sched.get_cpu_x_slice_size() / 256, 1);
    context.sched.set_gpu_slice_sizes(context.sched.get_gpu_x_slice_size() / 256, 1);
#endif

    int byte_length = 0;
    int vals_per_byte = 0;
    if (problem_size == 24010240) {
        byte_length = 5;
        vals_per_byte = 70;
    } else if (problem_size == 9765632) {
        byte_length = 6;
        vals_per_byte = 25;
    } else if (problem_size == 5153792) {
        byte_length = 6;
        vals_per_byte = 22;
    } else if (problem_size == 1500672) {
        byte_length = 5;
        vals_per_byte = 35;
    } else if (problem_size == 1000192) {
        byte_length = 7;
        vals_per_byte = 10;
    } else {
        std::cerr << "Error this problem size is not supported" << std::endl;
        std::exit(EXIT_FAILURE);
    }

    // Compute kernel parameters and the key and index that we are looking for.
    // The key and index that we want to compute is stored in "random_key" and "random_index".
    const int keyspace = FindKeyspaceSize(byte_length, vals_per_byte);
    srandom(1234);
    int random_index = rand() % keyspace;
    unsigned char random_key[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    unsigned int random_digest[4];
    IndexToKey(random_index, byte_length, vals_per_byte, random_key);
    md5_2words((unsigned int *) random_key, byte_length, random_digest);

    // These arrays and variables will hold the results
    size_t found_digest_size   = 4;
    unsigned int *found_digest = (unsigned int*) device_hopper::malloc(found_digest_size, sizeof(unsigned int));
    for (int i = 0; i < found_digest_size; ++i) found_digest[i] = 0;
    size_t found_index_size    = 1;
    int *found_index           = (int*) device_hopper::malloc(found_index_size, sizeof(int));
    *found_index               = 0;
    size_t foound_key_size     = 8;
    unsigned char *found_key   = (unsigned char*) device_hopper::malloc(foound_key_size, sizeof(unsigned char));
    for (int i = 0; i < foound_key_size; ++i) found_key[i] = 0;
    unsigned int random_digest_0 = random_digest[0];
    unsigned int random_digest_1 = random_digest[1];
    unsigned int random_digest_2 = random_digest[2];
    unsigned int random_digest_3 = random_digest[3];

    parallel_for pf(0, problem_size, [&]DEVICE_HOPPER_LAMBDA() {
        int threadid = GET_ITERATION();

        int startindex = threadid * vals_per_byte;
        unsigned char key[8] = {0, 0, 0, 0, 0, 0, 0, 0};
        IndexToKey(startindex, byte_length, vals_per_byte, key);

        unsigned int digest[4];
        int j;
        for (j = 0; j < vals_per_byte && startindex + j < keyspace; ++j) {
            //unsigned int digest[4];
            md5_2words((unsigned int *) key, byte_length, digest);
            if (digest[0] == random_digest_0 &&
                digest[1] == random_digest_1 &&
                digest[2] == random_digest_2 &&
                digest[3] == random_digest_3) {
                *found_index = startindex + j;
                found_key[0] = key[0];
                found_key[1] = key[1];
                found_key[2] = key[2];
                found_key[3] = key[3];
                found_key[4] = key[4];
                found_key[5] = key[5];
                found_key[6] = key[6];
                found_key[7] = key[7];
                found_digest[0] = digest[0];
                found_digest[1] = digest[1];
                found_digest[2] = digest[2];
                found_digest[3] = digest[3];
            }
            ++key[0];
        }
    });
    // Register buffers and specify access patterns
    pf.add_buffer_access_patterns(
        device_hopper::buf(found_digest, direction::OUT, pattern::ALL_OR_ANY),
        device_hopper::buf(found_key,    direction::OUT, pattern::ALL_OR_ANY),
        device_hopper::buf(found_index,  direction::OUT, pattern::ALL_OR_ANY));
    // Add scalar kernel parameters
    pf.add_scalar_parameters(random_digest_0, random_digest_1, random_digest_2, random_digest_3, keyspace, byte_length, vals_per_byte);
    // Set optional tuning parameters and call run()
    pf.opt_set_batch_size(256).opt_set_is_idempotent(true).run();

    if (*found_index < 0) {
        std::cout << "Error: Could not find an index" << std::endl;
        std::cout << "Error: The results are incorrect" << std::endl;
        return EXIT_FAILURE;
    } else if (*found_index != random_index) {
        std::cout << "Error: The computed index is not correct" << std::endl;
        std::cout << "Computed index: " << *found_index << std::endl;
        std::cout << "Expected index: " << random_index << std::endl;
        std::cout << "Error: The results are incorrect" << std::endl;
        return EXIT_FAILURE;
    } else if (found_key[0] != random_key[0] ||
               found_key[1] != random_key[1] ||
               found_key[2] != random_key[2] ||
               found_key[3] != random_key[3] ||
               found_key[4] != random_key[4] ||
               found_key[5] != random_key[5] ||
               found_key[6] != random_key[6] ||
               found_key[7] != random_key[7]) {
        std::cout << "Error: The computed key is not correct" << std::endl;
        std::cout << "Computed key, expected key " << std::endl;
        for (size_t i = 0; i < 8; ++i) std::cout << found_key[i] << ", " << random_key[i] << std::endl;
        std::cout << std::endl;
        std::cout << "Error: The results are incorrect" << std::endl;
        return EXIT_FAILURE;
    } else if (found_digest[0] != random_digest[0] ||
               found_digest[1] != random_digest[1] ||
               found_digest[2] != random_digest[2] ||
               found_digest[3] != random_digest[3]) {
        std::cout << "Error: The computed digest is not correct" << std::endl;
        std::cout << "Computed digest, expected digest " << std::endl;
        for (size_t i = 0; i < 4; ++i) std::cout << found_digest[i] << ", " << random_digest[i] << std::endl;
        std::cout << std::endl;
        std::cout << "Error: The results are incorrect" << std::endl;
        return EXIT_FAILURE;
    }
    std::cout << "Info: The results are correct" << std::endl;

    return EXIT_SUCCESS;
}


#include "engine/plugins/tile.hpp"
#include "engine/plugins/plugin_base.hpp"

#include "util/coordinate_calculation.hpp"
#include "util/vector_tile.hpp"
#include "util/web_mercator.hpp"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>

#include <protozero/pbf_writer.hpp>
#include <protozero/varint.hpp>

#include <string>
#include <utility>
#include <vector>

#include <cmath>
#include <cstdint>

namespace osrm
{
namespace engine
{
namespace plugins
{
namespace detail
{
// Simple container class for WGS84 coordinates
template <typename T> struct Point final
{
    Point(T _x, T _y) : x(_x), y(_y) {}

    const T x;
    const T y;
};

// from mapnik-vector-tile
namespace pbf
{
inline unsigned encode_length(const unsigned len) { return (len << 3u) | 2u; }
}

struct BBox final
{
    BBox(const double _minx, const double _miny, const double _maxx, const double _maxy)
        : minx(_minx), miny(_miny), maxx(_maxx), maxy(_maxy)
    {
    }

    double width() const { return maxx - minx; }
    double height() const { return maxy - miny; }

    const double minx;
    const double miny;
    const double maxx;
    const double maxy;
};

// Simple container for integer coordinates (i.e. pixel coords)
struct point_type_i final
{
    point_type_i(std::int64_t _x, std::int64_t _y) : x(_x), y(_y) {}

    const std::int64_t x;
    const std::int64_t y;
};

using FixedLine = std::vector<detail::Point<std::int32_t>>;
using FloatLine = std::vector<detail::Point<double>>;

typedef boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian> point_t;
typedef boost::geometry::model::linestring<point_t> linestring_t;
typedef boost::geometry::model::box<point_t> box_t;
typedef boost::geometry::model::multi_linestring<linestring_t> multi_linestring_t;
const static box_t clip_box(point_t(-util::vector_tile::BUFFER, -util::vector_tile::BUFFER),
                            point_t(util::vector_tile::EXTENT + util::vector_tile::BUFFER,
                                    util::vector_tile::EXTENT + util::vector_tile::BUFFER));

// from mapnik-vector-tile
// Encodes a linestring using protobuf zigzag encoding
inline bool encodeLinestring(const FixedLine &line,
                             protozero::packed_field_uint32 &geometry,
                             std::int32_t &start_x,
                             std::int32_t &start_y)
{
    const std::size_t line_size = line.size();
    if (line_size < 2)
    {
        return false;
    }

    const unsigned line_to_length = static_cast<const unsigned>(line_size) - 1;

    auto pt = line.begin();
    geometry.add_element(9); // move_to | (1 << 3)
    geometry.add_element(protozero::encode_zigzag32(pt->x - start_x));
    geometry.add_element(protozero::encode_zigzag32(pt->y - start_y));
    start_x = pt->x;
    start_y = pt->y;
    geometry.add_element(detail::pbf::encode_length(line_to_length));
    for (++pt; pt != line.end(); ++pt)
    {
        const std::int32_t dx = pt->x - start_x;
        const std::int32_t dy = pt->y - start_y;
        geometry.add_element(protozero::encode_zigzag32(dx));
        geometry.add_element(protozero::encode_zigzag32(dy));
        start_x = pt->x;
        start_y = pt->y;
    }
    return true;
}

FixedLine coordinatesToTileLine(const util::Coordinate start,
                                const util::Coordinate target,
                                const detail::BBox &tile_bbox)
{
    FloatLine geo_line;
    geo_line.emplace_back(static_cast<double>(util::toFloating(start.lon)),
                          static_cast<double>(util::toFloating(start.lat)));
    geo_line.emplace_back(static_cast<double>(util::toFloating(target.lon)),
                          static_cast<double>(util::toFloating(target.lat)));

    linestring_t unclipped_line;

    for (auto const &pt : geo_line)
    {
        double px_merc = pt.x * util::web_mercator::DEGREE_TO_PX;
        double py_merc = util::web_mercator::latToY(util::FloatLatitude(pt.y)) *
                         util::web_mercator::DEGREE_TO_PX;
        // convert lon/lat to tile coordinates
        const auto px = std::round(
            ((px_merc - tile_bbox.minx) * util::web_mercator::TILE_SIZE / tile_bbox.width()) *
            util::vector_tile::EXTENT / util::web_mercator::TILE_SIZE);
        const auto py = std::round(
            ((tile_bbox.maxy - py_merc) * util::web_mercator::TILE_SIZE / tile_bbox.height()) *
            util::vector_tile::EXTENT / util::web_mercator::TILE_SIZE);

        boost::geometry::append(unclipped_line, point_t(px, py));
    }

    multi_linestring_t clipped_line;

    boost::geometry::intersection(clip_box, unclipped_line, clipped_line);

    FixedLine tile_line;

    // b::g::intersection might return a line with one point if the
    // original line was very short and coords were dupes
    if (!clipped_line.empty() && clipped_line[0].size() == 2)
    {
        if (clipped_line[0].size() == 2)
        {
            for (const auto &p : clipped_line[0])
            {
                tile_line.emplace_back(p.get<0>(), p.get<1>());
            }
        }
    }

    return tile_line;
}
}

Status TilePlugin::HandleRequest(const api::TileParameters &parameters, std::string &pbf_buffer)
{
    BOOST_ASSERT(parameters.IsValid());

    double min_lon, min_lat, max_lon, max_lat;

    // Convert the z,x,y mercator tile coordinates into WGS84 lon/lat values
    util::web_mercator::xyzToWGS84(
        parameters.x, parameters.y, parameters.z, min_lon, min_lat, max_lon, max_lat);

    util::Coordinate southwest{util::FloatLongitude(min_lon), util::FloatLatitude(min_lat)};
    util::Coordinate northeast{util::FloatLongitude(max_lon), util::FloatLatitude(max_lat)};

    // Fetch all the segments that are in our bounding box.
    // This hits the OSRM StaticRTree
    const auto edges = facade.GetEdgesInBox(southwest, northeast);

    std::vector<int> used_weights;
    std::unordered_map<int, std::size_t> weight_offsets;
    uint8_t max_datasource_id = 0;
    std::vector<std::string> names;
    std::unordered_map<std::string, std::size_t> name_offsets;

    // Loop over all edges once to tally up all the attributes we'll need.
    // We need to do this so that we know the attribute offsets to use
    // when we encode each feature in the tile.
    for (const auto &edge : edges)
    {
        int forward_weight = 0, reverse_weight = 0;
        uint8_t forward_datasource = 0;
        uint8_t reverse_datasource = 0;

        if (edge.forward_packed_geometry_id != SPECIAL_EDGEID)
        {
            std::vector<EdgeWeight> forward_weight_vector;
            facade.GetUncompressedWeights(edge.forward_packed_geometry_id, forward_weight_vector);
            forward_weight = forward_weight_vector[edge.fwd_segment_position];

            std::vector<uint8_t> forward_datasource_vector;
            facade.GetUncompressedDatasources(edge.forward_packed_geometry_id,
                                              forward_datasource_vector);
            forward_datasource = forward_datasource_vector[edge.fwd_segment_position];

            if (weight_offsets.find(forward_weight) == weight_offsets.end())
            {
                used_weights.push_back(forward_weight);
                weight_offsets[forward_weight] = used_weights.size() - 1;
            }
        }

        if (edge.reverse_packed_geometry_id != SPECIAL_EDGEID)
        {
            std::vector<EdgeWeight> reverse_weight_vector;
            facade.GetUncompressedWeights(edge.reverse_packed_geometry_id, reverse_weight_vector);

            BOOST_ASSERT(edge.fwd_segment_position < reverse_weight_vector.size());

            reverse_weight =
                reverse_weight_vector[reverse_weight_vector.size() - edge.fwd_segment_position - 1];

            if (weight_offsets.find(reverse_weight) == weight_offsets.end())
            {
                used_weights.push_back(reverse_weight);
                weight_offsets[reverse_weight] = used_weights.size() - 1;
            }
            std::vector<uint8_t> reverse_datasource_vector;
            facade.GetUncompressedDatasources(edge.reverse_packed_geometry_id,
                                              reverse_datasource_vector);
            reverse_datasource = reverse_datasource_vector[reverse_datasource_vector.size() -
                                                           edge.fwd_segment_position - 1];
        }
        // Keep track of the highest datasource seen so that we don't write unnecessary
        // data to the layer attribute values
        max_datasource_id = std::max(max_datasource_id, forward_datasource);
        max_datasource_id = std::max(max_datasource_id, reverse_datasource);

        std::string name = facade.GetNameForID(edge.name_id);

        if (name_offsets.find(name) == name_offsets.end())
        {
          names.push_back(name);
          name_offsets[name] = names.size() - 1;
        }
    }

    // TODO: extract speed values for compressed and uncompressed geometries

    // Convert tile coordinates into mercator coordinates
    util::web_mercator::xyzToMercator(
        parameters.x, parameters.y, parameters.z, min_lon, min_lat, max_lon, max_lat);
    const detail::BBox tile_bbox{min_lon, min_lat, max_lon, max_lat};

    // Protobuf serialized blocks when objects go out of scope, hence
    // the extra scoping below.
    protozero::pbf_writer tile_writer{pbf_buffer};
    {
        // Add a layer object to the PBF stream.  3=='layer' from the vector tile spec (2.1)
        protozero::pbf_writer layer_writer(tile_writer, util::vector_tile::LAYER_TAG);
        // TODO: don't write a layer if there are no features

        layer_writer.add_uint32(util::vector_tile::VERSION_TAG, 2); // version
        // Field 1 is the "layer name" field, it's a string
        layer_writer.add_string(util::vector_tile::NAME_TAG, "speeds"); // name
        // Field 5 is the tile extent.  It's a uint32 and should be set to 4096
        // for normal vector tiles.
        layer_writer.add_uint32(util::vector_tile::EXTEND_TAG, util::vector_tile::EXTENT); // extent

        // Begin the layer features block
        {
            // Each feature gets a unique id, starting at 1
            unsigned id = 1;
            for (const auto &edge : edges)
            {
                // Get coordinates for start/end nodes of segmet (NodeIDs u and v)
                const auto a = facade.GetCoordinateOfNode(edge.u);
                const auto b = facade.GetCoordinateOfNode(edge.v);
                // Calculate the length in meters
                const double length = osrm::util::coordinate_calculation::haversineDistance(a, b);

                int forward_weight = 0;
                int reverse_weight = 0;

                uint8_t forward_datasource = 0;
                uint8_t reverse_datasource = 0;

                std::string name = facade.GetNameForID(edge.name_id);

                if (edge.forward_packed_geometry_id != SPECIAL_EDGEID)
                {
                    std::vector<EdgeWeight> forward_weight_vector;
                    facade.GetUncompressedWeights(edge.forward_packed_geometry_id,
                                                  forward_weight_vector);
                    forward_weight = forward_weight_vector[edge.fwd_segment_position];

                    std::vector<uint8_t> forward_datasource_vector;
                    facade.GetUncompressedDatasources(edge.forward_packed_geometry_id,
                                                      forward_datasource_vector);
                    forward_datasource = forward_datasource_vector[edge.fwd_segment_position];
                }

                if (edge.reverse_packed_geometry_id != SPECIAL_EDGEID)
                {
                    std::vector<EdgeWeight> reverse_weight_vector;
                    facade.GetUncompressedWeights(edge.reverse_packed_geometry_id,
                                                  reverse_weight_vector);

                    BOOST_ASSERT(edge.fwd_segment_position < reverse_weight_vector.size());

                    reverse_weight = reverse_weight_vector[reverse_weight_vector.size() -
                                                           edge.fwd_segment_position - 1];

                    std::vector<uint8_t> reverse_datasource_vector;
                    facade.GetUncompressedDatasources(edge.reverse_packed_geometry_id,
                                                      reverse_datasource_vector);
                    reverse_datasource =
                        reverse_datasource_vector[reverse_datasource_vector.size() -
                                                  edge.fwd_segment_position - 1];
                }

                // Keep track of the highest datasource seen so that we don't write unnecessary
                // data to the layer attribute values
                max_datasource_id = std::max(max_datasource_id, forward_datasource);
                max_datasource_id = std::max(max_datasource_id, reverse_datasource);

                const auto encode_tile_line = [&layer_writer, &edge, &id, &max_datasource_id, &used_weights](
                    const detail::FixedLine &tile_line,
                    const std::uint32_t speed_kmh,
                    const std::size_t duration,
                    const std::uint8_t datasource,
                    const std::size_t name,
                    std::int32_t &start_x,
                    std::int32_t &start_y) {
                    // Here, we save the two attributes for our feature: the speed and the
                    // is_small
                    // boolean.  We onl serve up speeds from 0-139, so all we do is save the
                    // first
                    protozero::pbf_writer feature_writer(layer_writer,
                                                         util::vector_tile::FEATURE_TAG);
                    // Field 3 is the "geometry type" field.  Value 2 is "line"
                    feature_writer.add_enum(util::vector_tile::GEOMETRY_TAG,
                                            util::vector_tile::GEOMETRY_TYPE_LINE); // geometry type
                    // Field 1 for the feature is the "id" field.
                    feature_writer.add_uint64(util::vector_tile::ID_TAG, id++); // id
                    {
                        // When adding attributes to a feature, we have to write
                        // pairs of numbers.  The first value is the index in the
                        // keys array (written later), and the second value is the
                        // index into the "values" array (also written later).  We're
                        // not writing the actual speed or bool value here, we're saving
                        // an index into the "values" array.  This means many features
                        // can share the same value data, leading to smaller tiles.
                        protozero::packed_field_uint32 field(
                            feature_writer, util::vector_tile::FEATURE_ATTRIBUTES_TAG);

                        field.add_element(0); // "speed" tag key offset
                        field.add_element(
                            std::min(speed_kmh, 127u)); // save the speed value, capped at 127
                        field.add_element(1);           // "is_small" tag key offset
                        field.add_element(128 +
                                          (edge.component.is_tiny ? 0 : 1)); // is_small feature
                        field.add_element(2);                // "datasource" tag key offset
                        field.add_element(130 + datasource); // datasource value offset
                        field.add_element(3);                // "duration" tag key offset
                        field.add_element(130 + max_datasource_id + 1 +
                                          duration); // duration value offset
                        field.add_element(4); // "name" tag key offset

                        field.add_element(130 + max_datasource_id + 1 +
                                          used_weights.size() + name); // name value offset
                    }
                    {

                        // Encode the geometry for the feature
                        protozero::packed_field_uint32 geometry(
                            feature_writer, util::vector_tile::FEATURE_GEOMETRIES_TAG);
                        encodeLinestring(tile_line, geometry, start_x, start_y);
                    }
                };

                // If this is a valid forward edge, go ahead and add it to the tile
                if (forward_weight != 0 && edge.forward_segment_id.enabled)
                {
                    std::int32_t start_x = 0;
                    std::int32_t start_y = 0;

                    // Calculate the speed for this line
                    std::uint32_t speed_kmh =
                        static_cast<std::uint32_t>(round(length / forward_weight * 10 * 3.6));

                    auto tile_line = coordinatesToTileLine(a, b, tile_bbox);
                    if (!tile_line.empty())
                    {
                        encode_tile_line(tile_line,
                                         speed_kmh,
                                         weight_offsets[forward_weight],
                                         forward_datasource,
                                         name_offsets[name],
                                         start_x,
                                         start_y);
                    }
                }

                // Repeat the above for the coordinates reversed and using the `reverse`
                // properties
                if (reverse_weight != 0 && edge.reverse_segment_id.enabled)
                {
                    std::int32_t start_x = 0;
                    std::int32_t start_y = 0;

                    // Calculate the speed for this line
                    std::uint32_t speed_kmh =
                        static_cast<std::uint32_t>(round(length / reverse_weight * 10 * 3.6));

                    auto tile_line = coordinatesToTileLine(b, a, tile_bbox);
                    if (!tile_line.empty())
                    {
                        encode_tile_line(tile_line,
                                         speed_kmh,
                                         weight_offsets[reverse_weight],
                                         reverse_datasource,
                                         name_offsets[name],
                                         start_x,
                                         start_y);
                    }
                }
            }
        }

        // Field id 3 is the "keys" attribute
        // We need two "key" fields, these are referred to with 0 and 1 (their array indexes)
        // earlier
        layer_writer.add_string(util::vector_tile::KEY_TAG, "speed");
        layer_writer.add_string(util::vector_tile::KEY_TAG, "is_small");
        layer_writer.add_string(util::vector_tile::KEY_TAG, "datasource");
        layer_writer.add_string(util::vector_tile::KEY_TAG, "duration");
        layer_writer.add_string(util::vector_tile::KEY_TAG, "name");

        // Now, we write out the possible speed value arrays and possible is_tiny
        // values.  Field type 4 is the "values" field.  It's a variable type field,
        // so requires a two-step write (create the field, then write its value)
        for (std::size_t i = 0; i < 128; i++)
        {
            // Writing field type 4 == variant type
            protozero::pbf_writer values_writer(layer_writer, util::vector_tile::VARIANT_TAG);
            // Attribute value 5 == uin64 type
            values_writer.add_uint64(util::vector_tile::VARIANT_TYPE_UINT32, i);
        }
        {
            protozero::pbf_writer values_writer(layer_writer, util::vector_tile::VARIANT_TAG);
            // Attribute value 7 == bool type
            values_writer.add_bool(util::vector_tile::VARIANT_TYPE_BOOL, true);
        }
        {
            protozero::pbf_writer values_writer(layer_writer, util::vector_tile::VARIANT_TAG);
            // Attribute value 7 == bool type
            values_writer.add_bool(util::vector_tile::VARIANT_TYPE_BOOL, false);
        }
        for (std::size_t i = 0; i <= max_datasource_id; i++)
        {
            // Writing field type 4 == variant type
            protozero::pbf_writer values_writer(layer_writer, util::vector_tile::VARIANT_TAG);
            // Attribute value 1 == string type
            values_writer.add_string(util::vector_tile::VARIANT_TYPE_STRING,
                                     facade.GetDatasourceName(i));
        }
        for (auto weight : used_weights)
        {
            // Writing field type 4 == variant type
            protozero::pbf_writer values_writer(layer_writer, util::vector_tile::VARIANT_TAG);
            // Attribute value 2 == float type
            // Durations come out of OSRM in integer deciseconds, so we convert them
            // to seconds with a simple /10 for display
            values_writer.add_double(util::vector_tile::VARIANT_TYPE_DOUBLE, weight / 10.);
        }

        for (const auto& name : names)
        {
            // Writing field type 4 == variant type
            protozero::pbf_writer values_writer(layer_writer, util::vector_tile::VARIANT_TAG);
            // Attribute value 1 == string type
            values_writer.add_string(util::vector_tile::VARIANT_TYPE_STRING, name);
        }
    }

    return Status::Ok;
}
}
}
}


// Copyright University of Warwick 2014
// 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)

// Authors:
// John Back

/*! \file LpcLineClusterAlgorithm.hh
    \brief File containing the declaration of the LpcLineClusterAlgorithm class
*/

/*! \class LpcLineClusterAlgorithm
    \brief Class that defines the (extended) line vertexing algorithm
*/

#ifndef LPC_LINE_CLUSTER_ALGORITHM_HH
#define LPC_LINE_CLUSTER_ALGORITHM_HH

#include "LACE/LpcAbsClusterAlgorithm.hh"

#include "LACE/LpcBinRange.hh"
#include "LACE/LpcClusterData.hh"
#include "LACE/LpcFunctions.hh"
#include "LACE/LpcLineFitter.hh"
#include "LACE/LpcLineSegment.hh"
#include "LACE/LpcPoint.hh"
#include "LACE/LpcResiduals.hh"

#include <Eigen/Dense>
#include <map>
#include <string>
#include <vector>

class LpcAbsCurve;
class LpcCurve;
class LpcCluster;
class LpcHitCollection;
class LpcParameters;
class LpcVertex;

class LpcLineClusterAlgorithm : public LpcAbsClusterAlgorithm {

public:

    //! Constructor
    /*!
      \param [in] thePars a pointer to the constant lpc parameters
    */
    LpcLineClusterAlgorithm(const LpcParameters* thePars);

    //! Destructor
    virtual ~LpcLineClusterAlgorithm();

    //! Process the main lpc curve (and its branches), finding vertices and clusters
    /*!
      \param [in] theCurve The pointer to the constant main lpc curve (read-only access)
      \returns an LpcClusterData object, which stores vectors of vertex and cluster pointers
    */
    virtual LpcClusterData findClusters(const LpcCurve* theCurve);

protected:

    //! Various functions and methods

    //! Typedef for a map of matched hits for each line segment
    typedef std::map<int, std::vector<int> > LpcLineIntMap;

    //! Process either the main curve or one of its branches
    /*!
      \param [in] theCurve The pointer to the (read-only) main lpc or one of its branches
      \returns the paired vector<LpcVertex*>, vector<LpcCluster*> for the curve
    */
    LpcClusterData processCurve(const LpcAbsCurve* theCurve);

    //! Find the lpc ranges that straddle the lpc feature points.
    //! This function fills the lpcRanges_ vector
    void findLpcRanges();

    //! Find the ranges of the line segments that are near the feature points.
    //! This function fills the lineRanges_ vector
    void findLineRanges();

    //! Find the line segments that are near the feature points.
    //! This function fills the lineSegments_ vector
    void findLineSegments();

    //! Find the vertex points for the given line segments
    /*!
      \param [in] lineSegments The vector of line segments
      \returns an Eigen MatrixXd of the vertices (row = vertex, col = co-ordinates)
    */
    Eigen::MatrixXd findVtxPoints(const std::vector<LpcLineSegment>& lineSegments) const;

    //! Find the extended line segments that are near the feature points.
    //! This function fills the extLineSegments_ vector
    /*!
      \param [in] initVtxPoints A MatrixXd of initial vertices (row = vtx, col = co-ords)
    */
    void findExtendedLines(const Eigen::MatrixXd& initVtxPoints);

    //! Find the hits associated to the initial line segments
    /*!
      \param [in] lineSegments Vector of LpcLineSegments that we want to find the associated hits for
      \param [in] uniqueHits Boolean to specify if any previously used hit is to be ignored
      \returns the LpcLineIntMap dictionary, key = line segment number, value = vector of hit ids
    */
    LpcLineIntMap findAssociatedHits(const std::vector<LpcLineSegment>& lineSegments,
				     bool uniqueHits = false);

    //! Create clusters using the associated hits, line segments and vertices
    /*!
      \param [in] vtxMatrix An Eigen MatrixXd of the vertices (row = vtx, col = coords)
      \returns a vector of LpcCluster pointers
    */
    std::vector<LpcCluster*> formClusters(const Eigen::MatrixXd& vtxMatrix);

    //! Create a complete list of cluster hits (indices) using extended lines, lpcRange and residual cut
    /*!
      \param [in] matchedHits The vector of hit indices to be considered for the cluster
      \param [in] lpcRange The range of the lpc points to be considered for the cluster
      \param [in] residualCut The maximum allowed residual for a hit to be part of the cluster
    */
    void formClusterHits(std::vector<int>& matchedHits,
			 const LpcBinRange& lpcRange,
			 double residualCut);

    //! Create a new LpcCluster pointer from the required hits
    /*!
      \param [in] index The index integer number of the cluster
      \param [in] matchedHits The vector of hit indices to be used for the cluster
      \param [in] lpcRange The range of lpc points used for the cluster
      \returns a new LpcCluster pointer
    */
    LpcCluster* createCluster(int index, const std::vector<int>& matchedHits,
			      const LpcBinRange& lpcRange) const;

    //! Create a new single LpcCluster pointer from all available hits
    /*!
      \param [in] index The index integer number of the cluster
      \returns a new LpcCluster pointer
    */
    LpcCluster* createSingleCluster(int index) const;

    //! Create a new single LpcVertex pointer as the position of the hit with the lowest 1st co-ord ("x")
    /*!
      \returns a new LpcVertex pointer
    */
    LpcVertex* createSingleVertex() const;

    //! Add any remaining hits to the clusters by finding the closest
    //! distance to the cluster principal axis
    /*!
      \param [in] lpcClusters The vector of LpcCluster pointers
    */
    void addRemainingHits(const std::vector<LpcCluster*>& lpcClusters);

    //! Create a vector of LpcVertex pointers containing the vertex information
    /*!
      \param [in] vtxPointMatrix The matrix of the vertex co-ordinates (row = vtx, col = x,y,z..)
      \return a vector of new LpcVertex pointers
    */
    std::vector<LpcVertex*> createVertices(const Eigen::MatrixXd& vtxPointMatrix) const;

    //! Merge clusters
    /*!
      \param [in] vtxPointMatrix The matrix of the vertex co-ordinates (row = vtx, col = x,y,z..)
      \param [in] lpcClusters The vector of original LpcCluster pointers
      \returns the pair of merged vertex and (a copy of the) cluster vectors
    */
    LpcClusterData mergeClusters(const Eigen::MatrixXd& vtxPointMatrix,
				 const std::vector<LpcCluster*>& lpcClusters);

    //! Check and potentially correct the allowed lpc index number
    /*!
      \param [in] lpcInt The initial lpc index point number
      \returns the index point number within the range [0, nLpcPoints1_]
    */
    int checkLpcInt(int lpcInt) const;

    //! Get the residual cut given the residual mean and rms
    /*!
      \param [in] residualMean The mean of the hit-to-lpc residuals
      \param [in] residualRms The rms of the hit-to-lpc residuals
      \returns the residual cut
    */
    double getResidualCut(double residualMean, double residualRms) const;

    //! Print out a vector of LpcBinRange objects
    /*!
      \param [in] preamble A starting string for the printout line
      \param [in] binRanges The given vector of LpcBinRange objects
    */
    void printBinRanges(const std::string& preamble,
			const std::vector<LpcBinRange>& binRanges) const;

private:

    //! Private default constructor
    LpcLineClusterAlgorithm();

    //! Copy constructor
    LpcLineClusterAlgorithm(const LpcLineClusterAlgorithm& other);

    //! Assignment operator
    LpcLineClusterAlgorithm& operator=(const LpcLineClusterAlgorithm& other);

    //! The pointer to the constant hit collection
    const LpcHitCollection* theHits_;

    //! The line fitter used to find the regression line for the set of hits
    LpcLineFitter lineFitter_;

    //! Number of lpc points to consider for each line segment
    int nLpcSegment_;

    //! Number of sigma for residual cut
    double nResSigma_;

    //! The minimum selection cut for lpc-to-hit residuals for the vertexing
    double minVtxResCut_;

    //! Maximum distance to associate any remaining hits with a cluster
    double maxDist_;

    //! Specify if we want to merge neighbouring clusters
    bool doClusterMerging_;

    //! Specify if we want to do vertexing and clustering for branches
    int doBranchVtx_;

    //! Specify the minimum angle for merging clusters
    double mergeDegAngle_;

    //! The cosine of the merging cluster angle
    double mergeCosAngle_;

    //! The LpcFunctions object
    LpcFunctions functions_;

    //! The curve index number
    int curveId_;

    //! The branch index number
    int branchId_;

    //! Hit and lpc point information

    //! The number of hit points
    int nHitPoints_;

    //! The number of co-ordinate dimensions
    int nDim_;

    //! The number of lpc points
    int nLpcPoints_;

    //! The number of lpc points - 1
    int nLpcPoints1_;

    //! The Eigen matrix of unscaled hit positions
    Eigen::MatrixXd hitPoints_;

    //! The vector of LpcPoints
    std::vector<LpcPoint> lpcPoints_;

    //! The lpc residual object
    LpcResiduals lpcResiduals_;

    //! The hit-to-lpc residuals
    Eigen::VectorXd hitResiduals_;

    //! The closest lpc point integer for each hit
    Eigen::VectorXi hitNearLpc_;

    //! An array to keep track of which hits have already been used for clusters
    Eigen::VectorXi usedHits_;

    //! Internal vectors of various lpc point bin ranges

    //! The number of cosine peaks
    int nPeaks_;

    //! The vector of lpc (min,max) point ranges for each cosine feature peak
    std::vector<LpcBinRange> cosPeakRanges_;

    //! The vector of lpc (min,max) point ranges that straddle the feature points
    std::vector<LpcBinRange> lpcRanges_;

    //! The vector of lpc (min,max) point ranges that are near the feature points
    std::vector<LpcBinRange> lineRanges_;

    //! The vector of line segments
    std::vector<LpcLineSegment> lineSegments_;

    //! The vector of extended line segments
    std::vector<LpcLineSegment> extLineSegments_;

    //! An internal boolean to specify if cluster merging was successful
    bool mergedOK_;

};

#endif


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>

#include <iostream>
#include <fstream>

#include <boost/graph/prim_minimum_spanning_tree.hpp>

typedef CGAL::Simple_cartesian<double>                       Kernel;
typedef Kernel::Point_3                                      Point;
typedef CGAL::Surface_mesh<Point>                            Mesh;

typedef boost::graph_traits<Mesh>::vertex_descriptor vertex_descriptor;

int main(int argc, char* argv[])
{
  Mesh P;
  //std::cin >> P;
  std::ifstream in((argc>1)?argv[1]:"data/prim.off");
  in >> P;
  Mesh::Property_map<vertex_descriptor,vertex_descriptor> predecessor;
  predecessor = P.add_property_map<vertex_descriptor,vertex_descriptor>("v:predecessor").first;

  boost::prim_minimum_spanning_tree(P, predecessor, boost::root_vertex(*vertices(P).first));

  std::cout << "#VRML V2.0 utf8\n"
    "DirectionalLight {\n"
    "direction 0 -1 0\n"
    "}\n"
    "Shape {\n"
    "  appearance Appearance {\n"
    "    material Material { emissiveColor 1 0 0}}\n"
    "    geometry\n"
    "    IndexedLineSet {\n"
    "      coord Coordinate {\n"
    "        point [ \n";

  for(vertex_descriptor vd : vertices(P)){
    std::cout <<  "        " << P.point(vd) << "\n";
  }

  std::cout << "        ]\n"
    "     }\n"
    "      coordIndex [\n";
  for(vertex_descriptor vd : vertices(P)){
    if(predecessor[vd]!=vd){
      std::cout << "      " << std::size_t(vd) << ", " << std::size_t(predecessor[vd]) <<  ", -1\n";
    }
  }

  std::cout << "]\n"
    "  }#IndexedLineSet\n"
    "}# Shape\n";

  P.remove_property_map(predecessor);
  return 0;
}


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <vw/Core/Log.h>
#include <vw/config.h>
#include <vw/Math/Matrix.h>
#include <vw/Math/Vector.h>
#include <vw/Math/Quaternion.h>
#include <vw/Camera/PinholeModel.h>
#include <vw/Camera/LensDistortion.h>

#if defined(VW_HAVE_PKG_LAPACK) && VW_HAVE_PKG_LAPACK==1
#include <vw/Math/LinearAlgebra.h>
#endif

#include <algorithm>
#include <sstream>
#include <iomanip>
#include <string>

// TODO: One day remove this protobuf dependency.
#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
#include <vw/Camera/TsaiFile.pb.h>
using google::protobuf::RepeatedFieldBackInserter;
#endif

#include <boost/filesystem/convenience.hpp>
namespace fs = boost::filesystem;

using namespace vw;
using namespace camera;

PinholeModel::PinholeModel() : m_distortion(DistortPtr(new NullLensDistortion)),
                               m_camera_center(Vector3(0,0,0)),
                               m_fu(1), m_fv(1), m_cu(0), m_cv(0),
                               m_u_direction(Vector3(1,0,0)),
                               m_v_direction(Vector3(0,1,0)),
                               m_w_direction(Vector3(0,0,1)), m_pixel_pitch(1) {

  m_rotation.set_identity();
  this->rebuild_camera_matrix();
}

/// Initialize from a file on disk.
PinholeModel::PinholeModel(std::string const& filename) : m_distortion(DistortPtr(new NullLensDistortion)) {
  read(filename);
}

PinholeModel::PinholeModel(Vector3 camera_center, Matrix<double,3,3> rotation,
                           double f_u, double f_v, double c_u, double c_v,
                           Vector3 u_direction, Vector3 v_direction,
                           Vector3 w_direction,
                           LensDistortion const& distortion_model,
                           double pixel_pitch) : m_distortion(DistortPtr(distortion_model.copy())),
                                                 m_camera_center(camera_center),
                                                 m_rotation(rotation),
                                                 m_fu(f_u), m_fv(f_v), m_cu(c_u), m_cv(c_v),
                                                 m_u_direction(u_direction),
                                                 m_v_direction(v_direction),
                                                 m_w_direction(w_direction),
                                                 m_pixel_pitch(pixel_pitch) {
  this->rebuild_camera_matrix();
}

PinholeModel::PinholeModel(Vector3 camera_center, Matrix<double,3,3> rotation,
                           double f_u, double f_v, double c_u, double c_v,
                           LensDistortion const& distortion_model,
                           double pixel_pitch) : m_distortion(DistortPtr(distortion_model.copy())),
                                                 m_camera_center(camera_center),
                                                 m_rotation(rotation),
                                                 m_fu(f_u), m_fv(f_v), m_cu(c_u), m_cv(c_v),
                                                 m_u_direction(Vector3(1,0,0)),
                                                 m_v_direction(Vector3(0,1,0)),
                                                 m_w_direction(Vector3(0,0,1)),
                                                 m_pixel_pitch(pixel_pitch) {
  rebuild_camera_matrix();
}

/// Construct a basic pinhole model with no lens distortion
PinholeModel::PinholeModel(Vector3 camera_center, Matrix<double,3,3> rotation,
                           double f_u, double f_v,
                           double c_u, double c_v,
                           double pixel_pitch) : m_distortion(DistortPtr(new NullLensDistortion)),
                                                 m_camera_center(camera_center),
                                                 m_rotation(rotation),
                                                 m_fu(f_u), m_fv(f_v), m_cu(c_u), m_cv(c_v),
                                                 m_u_direction(Vector3(1,0,0)),
                                                 m_v_direction(Vector3(0,1,0)),
                                                 m_w_direction(Vector3(0,0,1)),
                                                 m_pixel_pitch(pixel_pitch) {
  rebuild_camera_matrix();
}

void PinholeModel::read(std::string const& filename) {

  // Handle deprecated .pinhole protobuf files
  fs::path filename_path( filename );
  if ( filename_path.extension() == ".pinhole" )
    return read_protobuf_file(filename);

  // Open the input file
  std::ifstream cam_file;
  cam_file.open(filename.c_str());
  if (cam_file.fail())
    vw_throw( IOErr() << "PinholeModel::read_file: Could not open file: " << filename );

  // Check for version number on the first line
  int file_version = 1; // The default version written before the 2016 changes
  std::string line;
  std::getline(cam_file, line);
  if (line.find("VERSION") != std::string::npos) {
    sscanf(line.c_str(),"VERSION_%d", &file_version); // Parse the version of the input file

    // Right now there is only one version (VERSION_3) so if we find the version
    //  we just skip it and move on to the next line.  If the version is changed,
    //  handler logic needs to be implemented here.
    std::getline(cam_file, line);
  }

  // Start parsing all the parameters from the lines.
  if (!cam_file.good() || sscanf(line.c_str(),"fu = %lf", &m_fu) != 1) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read x focal length\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"fv = %lf", &m_fv) != 1) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read y focal length\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"cu = %lf", &m_cu) != 1) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read x principal point\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"cv = %lf", &m_cv) != 1) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read y principal point\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"u_direction = %lf %lf %lf",
        &m_u_direction(0), &m_u_direction(1), &m_u_direction(2)) != 3) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read u direction vector\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"v_direction = %lf %lf %lf",
        &m_v_direction(0), &m_v_direction(1), &m_v_direction(2)) != 3) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read v direction vector\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"w_direction = %lf %lf %lf",
        &m_w_direction(0), &m_w_direction(1), &m_w_direction(2)) != 3) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read w direction vector\n" );
  }

  // Read extrinsic parameters
  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"C = %lf %lf %lf",
        &m_camera_center(0), &m_camera_center(1), &m_camera_center(2)) != 3) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file: Could not read C (camera center) vector\n" );
  }

  std::getline(cam_file, line);
  if ( !cam_file.good() ||
       sscanf(line.c_str(), "R = %lf %lf %lf %lf %lf %lf %lf %lf %lf",
              &m_rotation(0,0), &m_rotation(0,1), &m_rotation(0,2),
              &m_rotation(1,0), &m_rotation(1,1), &m_rotation(1,2),
              &m_rotation(2,0), &m_rotation(2,1), &m_rotation(2,2)) != 9 ) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read rotation matrix\n" );
  }

  // The pitch line does not exist in older files, so don't fail if it is missing.
  // - At this point we need to hang on to the position immediately before the
  int lens_start = cam_file.tellg();
  std::getline(cam_file, line);
  if (line.find("pitch") != std::string::npos) {
    if (!cam_file.good() || sscanf(line.c_str(),"pitch = %lf", &m_pixel_pitch) != 1) {
      cam_file.close();
      vw_throw( IOErr() << "PinholeModel::read_file(): Could not read pixel pitch\n" );
    }
    lens_start = cam_file.tellg();
    std::getline(cam_file, line); // After reading the pitch, read the next line.
  }
  else // Pixel pitch not specified, use 1.0 as the default.
  {
    if (file_version > 2){
      cam_file.close();
      vw_throw( IOErr() << "PinholeModel::read_file(): Pitch value required in this file version!\n" );
    }
    m_pixel_pitch = 1.0;
  }

  // Now that we have loaded all the parameters, update the dependend class members.
  this->rebuild_camera_matrix();

  // This creates m_distortion but we still need to read the parameters.
  bool found_name = construct_lens_distortion(line);

  if (!found_name && (file_version > 2)){
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Distortion name required in this file version!\n" );
  }

  // If there was no line containing the distortion model name (true for old files)
  //  then we need to back up to before the distortion parameters begin in the file.
  if (!found_name)
    cam_file.seekg(lens_start ,std::ios_base::beg);

  // The lens distortion class knows how to parse the rest of the input stream.
  m_distortion->read(cam_file);

  cam_file.close();
}

bool PinholeModel::construct_lens_distortion(std::string const& config_line) {

  // Check if the passed in string contains the string for any of the
  //  recognized lens distortion models.
  if (config_line.find(NullLensDistortion::class_name()) != std::string::npos) {
    m_distortion.reset(new NullLensDistortion());
    return true;
  }
  if (config_line.find(BrownConradyDistortion::class_name()) != std::string::npos) {
    m_distortion.reset(new BrownConradyDistortion());
    return true;
  }
  if (config_line.find(AdjustableTsaiLensDistortion::class_name()) != std::string::npos) {
    m_distortion.reset(new AdjustableTsaiLensDistortion());
    return true;
  }
  if (config_line.find(PhotometrixLensDistortion::class_name()) != std::string::npos) {
    m_distortion.reset(new PhotometrixLensDistortion());
    return true;
  }

  // TSAI is the default model.  Older files which do not have a specifier string
  //  contain TSAI parameters.
  m_distortion.reset(new TsaiLensDistortion());

  if (config_line.find(TsaiLensDistortion::class_name()) != std::string::npos)
    return true;
  else
    return false;
}

// This file type is DEPRECATED!
void PinholeModel::read_protobuf_file(std::string const& filename) {

  fs::path filename_path( filename );
  if ( filename_path.extension() != ".pinhole" )
    vw_throw( IOErr() << "Not a protobuf file extension: " << filename_path.extension() );

#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
  std::fstream input( filename.c_str(), std::ios::in | std::ios::binary );
  if ( !input )
    vw_throw( IOErr() << "Pinhole::read_file: Could not open " << filename << "\n" );
  TsaiFile file;
  if ( !file.ParseFromIstream( &input ) )
    vw_throw( IOErr() << "Pinhole::read_file: Protocol buffer failed to parse \"" << filename << "\"\n" );
  input.close();

  // Making sure protobuf seems correct
  VW_ASSERT( file.focal_length_size() == 2,
             IOErr() << "Pinhole::read_file: Unexpected amount of focal lengths." );
  VW_ASSERT( file.center_point_size() == 2,
             IOErr() << "Pinhole::read_file: Unexpected amount of center points." );
  VW_ASSERT( file.u_direction_size() == 3,
             IOErr() << "Pinhole::read_file: Unexpected size of u vector." );
  VW_ASSERT( file.v_direction_size() == 3,
             IOErr() << "Pinhole::read_file: Unexpected size of v vector." );
  VW_ASSERT( file.w_direction_size() == 3,
             IOErr() << "Pinhole::read_file: Unexpected size of w vector." );
  VW_ASSERT( file.camera_center_size() == 3,
             IOErr() << "Pinhole::read_file: Unexpected size of camera vector." );
  VW_ASSERT( file.camera_rotation_size() == 9,
             IOErr() << "Pinhole::read_file: Unexpected size of rotation matrix." );

  typedef VectorProxy<double,3> Vector3P;
  m_u_direction = Vector3P(file.mutable_u_direction()->mutable_data());
  m_v_direction = Vector3P(file.mutable_v_direction()->mutable_data());
  m_w_direction = Vector3P(file.mutable_w_direction()->mutable_data());
  m_camera_center = Vector3P(file.mutable_camera_center()->mutable_data());
  m_fu = file.focal_length(0);
  m_fv = file.focal_length(1);
  m_cu = file.center_point(0);
  m_cv = file.center_point(1);
  m_rotation = MatrixProxy<double,3,3>(file.mutable_camera_rotation()->mutable_data());
  m_pixel_pitch = file.pixel_pitch();

  this->rebuild_camera_matrix();

  if ( file.distortion_name() == "NULL" ) {
    VW_ASSERT( file.distortion_vector_size() == 0,
               IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
    m_distortion.reset( new NullLensDistortion());
  } else if ( file.distortion_name() == "TSAI" ) {
    VW_ASSERT( file.distortion_vector_size() == 4,
               IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
    m_distortion.reset( new TsaiLensDistortion(VectorProxy<double,4>(file.mutable_distortion_vector()->mutable_data())));
  } else if ( file.distortion_name() == "BROWNCONRADY" ) {
    VW_ASSERT( file.distortion_vector_size() == 8,
               IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
    m_distortion.reset( new BrownConradyDistortion(VectorProxy<double,8>(file.mutable_distortion_vector()->mutable_data())));
  } else if ( file.distortion_name() == "AdjustableTSAI" ) {
    VW_ASSERT( file.distortion_vector_size() > 3,
               IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
    m_distortion.reset( new AdjustableTsaiLensDistortion(VectorProxy<double>(file.distortion_vector_size(),file.mutable_distortion_vector()->mutable_data())));
  }
#else
  // If you hit this point, you need to install Google Protobuffers to to read this file type.
  vw_throw( IOErr() << "Pinhole::write_file: Camera IO not supported without Google Protobuffers" );
#endif

}

void PinholeModel::write(std::string const& filename) const {

  // Update this field whenever there is a significant change to the file.
  // - It can be used to keep backwards compatibility with future plain text changes.
  const std::string PINHOLE_VERSION = "VERSION_3";

  // Set the path an open the output file for writing
  std::string file_path = fs::path(filename).replace_extension(".tsai").string();
  std::ofstream cam_file(file_path.c_str());
  if( !cam_file.is_open() )
    vw_throw( IOErr() << "PinholeModel::write: Could not open file: " << file_path );

  // Write the pinhole camera model parts
  //   # digits to survive double->text->double conversion
  const size_t ACCURATE_DIGITS = 17; // = std::numeric_limits<double>::max_digits10
  cam_file << std::setprecision(ACCURATE_DIGITS);
  cam_file << PINHOLE_VERSION << "\n";
  cam_file << "fu = " << m_fu << "\n";
  cam_file << "fv = " << m_fv << "\n";
  cam_file << "cu = " << m_cu << "\n";
  cam_file << "cv = " << m_cv << "\n";
  cam_file << "u_direction = " << m_u_direction[0] << " " << m_u_direction[1] << " " << m_u_direction[2] << "\n";
  cam_file << "v_direction = " << m_v_direction[0] << " " << m_v_direction[1] << " " << m_v_direction[2] << "\n";
  cam_file << "w_direction = " << m_w_direction[0] << " " << m_w_direction[1] << " " << m_w_direction[2] << "\n";
  cam_file << "C = " << m_camera_center[0] << " " << m_camera_center[1] << " " << m_camera_center[2] << "\n";
  cam_file << "R = " << m_rotation(0,0) << " " << m_rotation(0,1) << " " << m_rotation(0,2) << " " << m_rotation(1,0) << " " << m_rotation(1,1) << " " << m_rotation(1,2) << " " << m_rotation(2,0) << " " << m_rotation(2,1) << " " << m_rotation(2,2) << "\n";
  cam_file << "pitch = " << m_pixel_pitch << "\n";

  // Write the name of the distortion model, then use the distortion model
  //  << overload to write it to the file.
  cam_file << m_distortion->name() << std::endl;
  cam_file << *m_distortion;
  cam_file.close();
}

Vector2 PinholeModel::point_to_pixel(Vector3 const& point) const {

  // Multiply the pixel location by the 3x4 camera matrix.
  // - The pixel coordinate is de-homogenized by dividing by the denominator.
  double denominator = m_camera_matrix(2,0)*point(0) + m_camera_matrix(2,1)*point(1) +
                       m_camera_matrix(2,2)*point(2) + m_camera_matrix(2,3);
  Vector2 pixel = Vector2( (m_camera_matrix(0,0)*point(0) + m_camera_matrix(0,1)*point(1) +
                            m_camera_matrix(0,2)*point(2) + m_camera_matrix(0,3)           ) / denominator,
                           (m_camera_matrix(1,0)*point(0) + m_camera_matrix(1,1)*point(1) +
                            m_camera_matrix(1,2)*point(2) + m_camera_matrix(1,3)           ) / denominator);

  // Apply the lens distortion model
  // - Divide by pixel pitch to convert from metric units to pixels if the intrinsic
  //   values were not specified in pixel units (in that case m_pixel_pitch == 1.0)
  return m_distortion->distorted_coordinates(*this, pixel)/m_pixel_pitch;
}

Vector2 PinholeModel::point_to_pixel_no_distortion(Vector3 const& point) const {

  // Multiply the pixel location by the 3x4 camera matrix.
  // - The pixel coordinate is de-homogenized by dividing by the denominator.
  double denominator = m_camera_matrix(2,0)*point(0) + m_camera_matrix(2,1)*point(1) +
                       m_camera_matrix(2,2)*point(2) + m_camera_matrix(2,3);
  Vector2 pixel = Vector2( (m_camera_matrix(0,0)*point(0) + m_camera_matrix(0,1)*point(1) +
                            m_camera_matrix(0,2)*point(2) + m_camera_matrix(0,3)           ) / denominator,
                           (m_camera_matrix(1,0)*point(0) + m_camera_matrix(1,1)*point(1) +
                            m_camera_matrix(1,2)*point(2) + m_camera_matrix(1,3)           ) / denominator);

  // Divide by pixel pitch to convert from metric units to pixels if the intrinsic
  //   values were not specified in pixel units (in that case m_pixel_pitch == 1.0)
  return pixel/m_pixel_pitch;
}

bool PinholeModel::projection_valid(Vector3 const& point) const {
  // z coordinate after extrinsic transformation
  double z = m_extrinsics(2, 0)*point(0) + m_extrinsics(2, 1)*point(1) +
    m_extrinsics(2, 2)*point(2) + m_extrinsics(2,3);
  return z > 0;
}

Vector3 PinholeModel::pixel_to_vector (Vector2 const& pix) const {
  // Apply the inverse lens distortion model
  Vector2 undistorted_pix = m_distortion->undistorted_coordinates(*this, pix*m_pixel_pitch);

  // Compute the direction of the ray emanating from the camera center.
  Vector3 p(0,0,1);
  subvector(p,0,2) = undistorted_pix;
  return normalize( m_inv_camera_transform * p);
}

Vector3 PinholeModel::camera_center(Vector2 const& /*pix*/ ) const {
  return m_camera_center;
};

void PinholeModel::set_camera_center(Vector3 const& position) {
  m_camera_center = position;
  rebuild_camera_matrix();
}

Quaternion<double> PinholeModel::camera_pose(Vector2 const& /*pix*/ ) const {
  return Quaternion<double>(m_rotation);
}

void PinholeModel::set_camera_pose(Quaternion<double> const& pose) {
  m_rotation = pose.rotation_matrix();
  rebuild_camera_matrix();
}

void PinholeModel::set_camera_pose(Matrix<double,3,3> const& pose) {
  m_rotation = pose;
  rebuild_camera_matrix();
}

void PinholeModel::coordinate_frame(Vector3 &u_vec, Vector3 &v_vec, Vector3 &w_vec) const {
  u_vec = m_u_direction;
  v_vec = m_v_direction;
  w_vec = m_w_direction;
}

void PinholeModel::set_coordinate_frame(Vector3 u_vec, Vector3 v_vec, Vector3 w_vec) {
  m_u_direction = u_vec;
  m_v_direction = v_vec;
  m_w_direction = w_vec;

  rebuild_camera_matrix();
}

Vector3 PinholeModel::coordinate_frame_u_direction() const { return m_u_direction; }
Vector3 PinholeModel::coordinate_frame_v_direction() const { return m_v_direction; }
Vector3 PinholeModel::coordinate_frame_w_direction() const { return m_w_direction; }

const LensDistortion* PinholeModel::lens_distortion() const { return m_distortion.get(); };
void PinholeModel::set_lens_distortion(LensDistortion const& distortion) {
  m_distortion = distortion.copy();
}

void PinholeModel::intrinsic_parameters(double& f_u, double& f_v,
                                        double& c_u, double& c_v) const {
  f_u = m_fu;  f_v = m_fv;  c_u = m_cu;  c_v = m_cv;
}

void PinholeModel::set_intrinsic_parameters(double f_u, double f_v,
                                            double c_u, double c_v) {
  m_fu = f_u;  m_fv = f_v;  m_cu = c_u;  m_cv = c_v;
  rebuild_camera_matrix();
}

Vector2 PinholeModel::focal_length() const { return Vector2(m_fu,m_fv); }
void PinholeModel::set_focal_length(Vector2 const& f, bool rebuild ) {
  m_fu = f[0]; m_fv = f[1];
  if (rebuild) rebuild_camera_matrix();
}
Vector2 PinholeModel::point_offset() const { return Vector2(m_cu,m_cv); }
void PinholeModel::set_point_offset(Vector2 const& c, bool rebuild ) {
  m_cu = c[0]; m_cv = c[1];
  if (rebuild) rebuild_camera_matrix();
}
double PinholeModel::pixel_pitch() const { return m_pixel_pitch; }
void PinholeModel::set_pixel_pitch( double pitch ) { m_pixel_pitch = pitch; }

void PinholeModel::set_camera_matrix( Matrix<double,3,4> const& p ) {
#if defined(VW_HAVE_PKG_LAPACK) && VW_HAVE_PKG_LAPACK==1
  // Solving for camera center
  Matrix<double> cam_nullsp = nullspace(p);
  Vector<double> cam_center = select_col(cam_nullsp,0);
  cam_center /= cam_center[3];
  m_camera_center = subvector(cam_center,0,3);

  // Solving for intrinsics with RQ decomposition
  Matrix<double> M = submatrix(p,0,0,3,3);
  Matrix<double> R,Q;
  rqd( M, R, Q );
  Matrix<double> sign_fix(3,3);
  sign_fix.set_identity();
  if ( R(0,0) < 0 )
    sign_fix(0,0) = -1;
  if ( R(1,1) < 0 )
    sign_fix(1,1) = -1;
  if ( R(2,2) < 0 )
    sign_fix(2,2) = -1;
  R = R*sign_fix;
  Q = sign_fix*Q;
  R /= R(2,2);

  // Pulling out intrinsic and last extrinsic
  Matrix<double,3,3> uvwRotation;
  select_row(uvwRotation,0) = m_u_direction;
  select_row(uvwRotation,1) = m_v_direction;
  select_row(uvwRotation,2) = m_w_direction;
  m_rotation = inverse(uvwRotation*Q);
  m_fu = R(0,0);
  m_fv = R(1,1);
  m_cu = R(0,2);
  m_cv = R(1,2);

  if ( fabs(R(0,1)) >= 1.2 )
    vw_out(WarningMessage,"camera") << "Significant skew not modelled by pinhole camera\n";

  // Rebuild
  rebuild_camera_matrix();
#else
  vw_throw( NoImplErr() << "PinholeModel::set_Camera_Matrix is unavailable without LAPACK" );
#endif
}

Matrix<double,3,4> PinholeModel::camera_matrix() const {
  return m_camera_matrix;
}

void PinholeModel::rebuild_camera_matrix() {

  /// The intrinsic portion of the camera matrix is stored as
  ///
  ///    [  fx   0   cx  ]
  /// K= [  0    fy  cy  ]
  ///    [  0    0   1   ]
  ///
  /// with fx, fy the focal length of the system (in horizontal and
  /// vertical pixels), and (cx, cy) the pixel coordinates of the
  /// central pixel (the principal point on the image plane).

  m_intrinsics(0,0) = m_fu;
  m_intrinsics(0,1) = 0;
  m_intrinsics(0,2) = m_cu;
  m_intrinsics(1,0) = 0;
  m_intrinsics(1,1) = m_fv;
  m_intrinsics(1,2) = m_cv;
  m_intrinsics(2,0) = 0;
  m_intrinsics(2,1) = 0;
  m_intrinsics(2,2) = 1;

  // The extrinsics are normally built as the matrix:  [ R | -R*C ].
  // To allow for user-specified coordinate frames, the
  // extrinsics are now build to include the u,v,w rotation
  //
  //               | u_0  u_1  u_2  |
  //     Extr. =   | v_0  v_1  v_2  | * [ R | -R*C]
  //               | w_0  w_1  w_2  |
  //
  // The vectors u,v, and w must be orthonormal.

  /*   check for orthonormality of u,v,w              */
  VW_LINE_ASSERT( dot_prod(m_u_direction, m_v_direction) == 0 );
  VW_LINE_ASSERT( dot_prod(m_u_direction, m_w_direction) == 0 );
  VW_LINE_ASSERT( dot_prod(m_v_direction, m_w_direction) == 0 );
  VW_LINE_ASSERT( fabs( norm_2(m_u_direction) - 1 ) < 0.001 );
  VW_LINE_ASSERT( fabs( norm_2(m_v_direction) - 1 ) < 0.001 );
  VW_LINE_ASSERT( fabs( norm_2(m_w_direction) - 1 ) < 0.001 );

  Matrix<double,3,3> uvwRotation;

  select_row(uvwRotation,0) = m_u_direction;
  select_row(uvwRotation,1) = m_v_direction;
  select_row(uvwRotation,2) = m_w_direction;

  Matrix<double,3,3> rotation_inverse = transpose(m_rotation);
  submatrix(m_extrinsics,0,0,3,3) = uvwRotation * rotation_inverse;
  select_col(m_extrinsics,3) = uvwRotation * -rotation_inverse * m_camera_center;

  m_camera_matrix = m_intrinsics * m_extrinsics;
  m_inv_camera_transform = inverse(uvwRotation*rotation_inverse) * inverse(m_intrinsics);
}

// Apply a given rotation + translation + scale transform to a pinhole camera
void PinholeModel::apply_transform(vw::Matrix3x3 const & rotation,
                                   vw::Vector3   const & translation,
                                   double                scale) {

  // Extract current parameters
  vw::Vector3 position = this->camera_center();
  vw::Quat    pose     = this->camera_pose();

  vw::Quat rotation_quaternion(rotation);

  // New position and rotation
  position = scale*rotation*position + translation;
  pose     = rotation_quaternion*pose;
  this->set_camera_center(position);
  this->set_camera_pose  (pose);
}

PinholeModel
camera::scale_camera(PinholeModel const& camera_model, float scale) {
  if (scale == 0)
    vw_throw( ArgumentErr() << "PinholeModel::scale_camera cannot have zero scale value!" );
  // Scaling the camera is easy, just update the pixel pitch to account for the new image size.
  Vector2 focal  = camera_model.focal_length();
  Vector2 offset = camera_model.point_offset();
  boost::shared_ptr<LensDistortion> lens = camera_model.lens_distortion()->copy();
  return PinholeModel( camera_model.camera_center(),
                       camera_model.camera_pose().rotation_matrix(),
                       focal[0], focal[1], offset[0], offset[1],
                       camera_model.coordinate_frame_u_direction(),
                       camera_model.coordinate_frame_v_direction(),
                       camera_model.coordinate_frame_w_direction(),
                       *lens,
                       camera_model.pixel_pitch()/scale);
}

PinholeModel
camera::strip_lens_distortion(PinholeModel const& camera_model) {
  Vector2 focal  = camera_model.focal_length();
  Vector2 offset = camera_model.point_offset();
  NullLensDistortion distortion;
  return PinholeModel(camera_model.camera_center(),
                      camera_model.camera_pose().rotation_matrix(),
                      focal[0], focal[1], offset[0], offset[1],
                      camera_model.coordinate_frame_u_direction(),
                      camera_model.coordinate_frame_v_direction(),
                      camera_model.coordinate_frame_w_direction(),
                      distortion, camera_model.pixel_pitch());
}

std::ostream& camera::operator<<(std::ostream& str,
                                 PinholeModel const& model) {
  str << "Pinhole camera: \n";
  str << "\tCamera Center: " << model.camera_center() << "\n";
  str << "\tRotation Matrix: " << model.camera_pose() << "\n";
  str << "\tIntrinsics:\n";
  str << "\t  focal: "       << model.focal_length() << "\n";
  str << "\t  offset: "      << model.point_offset() << "\n";
  str << "\t  pixel pitch: " << model.pixel_pitch()  << "\n";
  str << "\tu direction: " << model.coordinate_frame_u_direction() << "\n";
  str << "\tv direction: " << model.coordinate_frame_v_direction() << "\n";
  str << "\tw direction: " << model.coordinate_frame_w_direction() << "\n";
  str << "\tDistortion Model: " << model.lens_distortion()->name() << "\n";
  str << *(model.lens_distortion()); // this will be multiple lines

  return str;
}