/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Field variables computation
// Compute variables (velocity, speed of sound, density, mach number) in the field
//
// I/O:
// - Minf: freestream Mach number
// - vInf: freestream velocity vector
// - bPan: (network of) body panels (structure)
// - fPan: field panels (structure)
// - sp: sub-panel (structure)
// - b2fAIC: body to field AIC (structure)
// - f2fAIC: field to field AIC (structure)
// - spAIC: body to field AIC for subpanels (structure)

#include <iostream>
#include <Eigen/Dense>
#include "compute_fVars.h"
#include "interp_ctv.h"
#include "interp_sp.h"

#define GAMMA 1.4
#define NV 4 // number of vertices

using namespace std;
using namespace Eigen;

void compute_fVars(double Minf, Vector3d &vInf, Network &bPan, Field &fPan, Subpanel &sp,
                       Body_AIC &b2fAIC, Field2field_AIC &f2fAIC, Subpanel_AIC &spAIC) {

    //// Initialization
    // Temporary variables
    MatrixXd vSing; // corner interpolated singularities
    vSing.resize(NV,2);
    MatrixXd sInterp; // sub-panel interoplated singularities
    sInterp.resize(sp.NS,2);

    //// Potential
    // Perturbation potential
    fPan.phi = b2fAIC.B * bPan.tau + b2fAIC.A * bPan.mu + f2fAIC.C * fPan.sigma;

    // Perturbation potential (with sub-paneling technique)
    // TODO Subpaneling induces small assymmetry. Check why?
    for (int jj = 0; jj < sp.sI.size(); jj++) {
        int j = sp.sI[jj]; // panel global index
        int n = j % bPan.nC_; // panel chordwise index
        int m = j / bPan.nC_; // panel spanwise index
        // Interpolation of singularities from centers to vertices
        vSing = interp_ctv(j, n, m, bPan);
        // Interpolation of singularities from vertices to sub-panel
        sInterp = interp_sp(j, bPan, sp,
                            vSing(0,0), vSing(1,0), vSing(2,0), vSing(3,0),
                            vSing(0,1), vSing(1,1), vSing(2,1), vSing(3,1));

        for (int ii = 0; ii < sp.fI[jj].size(); ii++) {
            int i = sp.fI[jj][ii]; // cell index
            // Velocity induces by each sub-panel
            for (int k = 0; k < sp.NS; k++) {
                fPan.phi(i) += spAIC.A[jj](ii,k) * sInterp(k,0) + spAIC.B[jj](ii,k) * sInterp(k,1);
            }
        }
    }

    //// Velocity
    // Perturbation velocity
    for (int i = 0; i < fPan.nE; i++) {
        int f = fPan.eIdx(i);

        // X-derivative
        double fb=0, ff=0;
        if (fPan.fbdMap(f,0) && fPan.fwdMap(f,0))
            fb = (fPan.phi(f) - fPan.phi(f - 1)) / fPan.deltaX;
        if (fPan.fbdMap(f,1) && fPan.fwdMap(f,1))
            ff = (fPan.phi(f + 1) - fPan.phi(f)) / fPan.deltaX;
        if (fPan.fbdMap(f,0) && fPan.fwdMap(f,0) && fPan.fbdMap(f,1) && fPan.fwdMap(f,1))
            fPan.U(f,0) = 0.5*(fb+ff);
        else
            fPan.U(f,0) = fb+ff;

        // Y-derivative
        fb=0; ff=0;
        if (fPan.fbdMap(f,2) && fPan.fwdMap(f,2))
            fb = (fPan.phi(f) - fPan.phi(f - fPan.nX*fPan.nZ)) / fPan.deltaY;
        if (fPan.fbdMap(f,3) && fPan.fwdMap(f,3))
            ff = (fPan.phi(f + fPan.nX*fPan.nZ) - fPan.phi(f)) / fPan.deltaY;
        if (fPan.fbdMap(f,2) && fPan.fwdMap(f,2) && fPan.fbdMap(f,3) && fPan.fwdMap(f,3))
            fPan.U(f,1) = 0.5*(fb+ff);
        else
            fPan.U(f,1) = fb+ff;

        // Z-derivative
        fb=0; ff=0;
        if (fPan.fbdMap(f,4) && fPan.fwdMap(f,4))
            fb = (fPan.phi(f) - fPan.phi(f - fPan.nX)) / fPan.deltaZ;
        if (fPan.fbdMap(f,5) && fPan.fwdMap(f,5))
            ff = (fPan.phi(f + fPan.nX) - fPan.phi(f)) / fPan.deltaZ;
        if (fPan.fbdMap(f,4) && fPan.fwdMap(f,4) && fPan.fbdMap(f,5) && fPan.fwdMap(f,5))
            fPan.U(f,2) = 0.5*(fb+ff);
        else
            fPan.U(f,2) = fb+ff;
    }
    // Freestream component
    for (int i = 0; i < fPan.nE; ++i) {
        fPan.U.row(fPan.eIdx(i)) += vInf.transpose();
    }

    //// Thermodynamic variables
    for (int i = 0; i < fPan.nE; ++i) {
        int f = fPan.eIdx(i);
        // Speed of sound
        fPan.a(f) = sqrt(
                1/(Minf * Minf) + (GAMMA - 1) / 2 - (GAMMA - 1) / 2 * fPan.U.row(f).dot(fPan.U.row(f)));
        // Mach number
        fPan.M(f) = fPan.U.row(f).norm() / fPan.a(f);
        // Density
        fPan.rho(f) = pow(
                1 + (GAMMA - 1) / 2 * Minf * Minf * (1 - fPan.U.row(f).dot(fPan.U.row(f))),
                1 / (GAMMA - 1));
    }
}


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

#include "inference_test_image.hpp"

#define STB_IMAGE_IMPLEMENTATION
#include "../third-party/stb/stb_image.h"

#define STB_IMAGE_RESIZE_IMPLEMENTATION
#include "../third-party/stb/stb_image_resize.h"

#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "../third-party/stb/stb_image_write.h"

#include <armnn/Exceptions.hpp>

#include <boost/numeric/conversion/cast.hpp>
#include <boost/format.hpp>

#include <memory>
#include <algorithm>

inline float Lerp(float a, float b, float w)
{
    return w * b + (1.f - w) * a;
}

inline void PutData(std::vector<float>& data,
                    unsigned int width,
                    unsigned int x,
                    unsigned int y,
                    unsigned int c,
                    float value)
{
    data[(3 * ((y * width) + x)) + c] = value;
}

// Resize and normalize the given image
std::vector<float> ResizeBilinearAndNormalize(const InferenceTestImage& image,
                                              unsigned int outputWidth,
                                              unsigned int outputHeight,
                                              float scale,
                                              const std::array<float, 3>& mean,
                                              const std::array<float, 3>& stddev)
{
    std::vector<float> out(outputWidth * outputHeight * 3, 0.f);

    // We follow the definition of TensorFlow and AndroidNN: the top-left corner of a texel in the output
    // image is projected into the input image to figure out the interpolants and weights. Note that this
    // will yield different results than if projecting the centre of output texels.

    const unsigned int inputWidth = image.GetWidth();
    const unsigned int inputHeight = image.GetHeight();

    // How much to scale pixel coordinates in the output image to get the corresponding pixel coordinates
    // in the input image.
    const float scaleY = boost::numeric_cast<float>(inputHeight) / boost::numeric_cast<float>(outputHeight);
    const float scaleX = boost::numeric_cast<float>(inputWidth) / boost::numeric_cast<float>(outputWidth);

    uint8_t rgb_x0y0[3] = {};
    uint8_t rgb_x1y0[3] = {};
    uint8_t rgb_x0y1[3] = {};
    uint8_t rgb_x1y1[3] = {};

    for (unsigned int y = 0; y < outputHeight; ++y)
    {
        // Corresponding real-valued height coordinate in input image.
        const float iy = boost::numeric_cast<float>(y) * scaleY;

        // Discrete height coordinate of top-left texel (in the 2x2 texel area used for interpolation).
        const float fiy = floorf(iy);
        const unsigned int y0 = boost::numeric_cast<unsigned int>(fiy);

        // Interpolation weight (range [0,1])
        const float yw = iy - fiy;

        for (unsigned int x = 0; x < outputWidth; ++x)
        {
            // Real-valued and discrete width coordinates in input image.
            const float ix = boost::numeric_cast<float>(x) * scaleX;
            const float fix = floorf(ix);
            const unsigned int x0 = boost::numeric_cast<unsigned int>(fix);

            // Interpolation weight (range [0,1]).
            const float xw = ix - fix;

            // Discrete width/height coordinates of texels below and to the right of (x0, y0).
            const unsigned int x1 = std::min(x0 + 1, inputWidth - 1u);
            const unsigned int y1 = std::min(y0 + 1, inputHeight - 1u);

            std::tie(rgb_x0y0[0], rgb_x0y0[1], rgb_x0y0[2]) = image.GetPixelAs3Channels(x0, y0);
            std::tie(rgb_x1y0[0], rgb_x1y0[1], rgb_x1y0[2]) = image.GetPixelAs3Channels(x1, y0);
            std::tie(rgb_x0y1[0], rgb_x0y1[1], rgb_x0y1[2]) = image.GetPixelAs3Channels(x0, y1);
            std::tie(rgb_x1y1[0], rgb_x1y1[1], rgb_x1y1[2]) = image.GetPixelAs3Channels(x1, y1);

            for (unsigned c = 0; c < 3; ++c)
            {
                const float ly0 = Lerp(float(rgb_x0y0[c]), float(rgb_x1y0[c]), xw);
                const float ly1 = Lerp(float(rgb_x0y1[c]), float(rgb_x1y1[c]), xw);
                const float l   = Lerp(ly0, ly1, yw);
                PutData(out, outputWidth, x, y, c, ((l / scale) - mean[c]) / stddev[c]);
            }
        }
    }
    return out;
}

InferenceTestImage::InferenceTestImage(char const* filePath)
    : m_Width(0u)
    , m_Height(0u)
    , m_NumChannels(0u)
{
    int width = 0;
    int height = 0;
    int channels = 0;

    using StbImageDataPtr = std::unique_ptr<unsigned char, decltype(&stbi_image_free)>;
    StbImageDataPtr stbData(stbi_load(filePath, &width, &height, &channels, 0), &stbi_image_free);

    if (stbData == nullptr)
    {
        throw armnn::Exception(boost::str(boost::format("Could not load the image at %1%") % filePath));
    }

    if (width == 0 || height == 0)
    {
        throw armnn::Exception(boost::str(boost::format("Could not load empty image at %1%") % filePath));
    }

    m_Width = boost::numeric_cast<unsigned int>(width);
    m_Height = boost::numeric_cast<unsigned int>(height);
    m_NumChannels = boost::numeric_cast<unsigned int>(channels);

    const unsigned int sizeInBytes = GetSizeInBytes();
    m_Data.resize(sizeInBytes);
    memcpy(m_Data.data(), stbData.get(), sizeInBytes);
}

std::tuple<uint8_t, uint8_t, uint8_t> InferenceTestImage::GetPixelAs3Channels(unsigned int x, unsigned int y) const
{
    if (x >= m_Width || y >= m_Height)
    {
        throw armnn::InvalidArgumentException(
                    boost::str(boost::format("Attempted out of bounds image access. "
                                             "Requested (%1%, %2%). Maximum valid coordinates (%3%, %4%).")
                               % x
                               % y
                               % (m_Width - 1)
                               % (m_Height - 1)));
    }

    const unsigned int pixelOffset = x * GetNumChannels() + y * GetWidth() * GetNumChannels();
    const uint8_t* const pixelData = m_Data.data() + pixelOffset;
    BOOST_ASSERT(pixelData <= (m_Data.data() + GetSizeInBytes()));

    std::array<uint8_t, 3> outPixelData;
    outPixelData.fill(0);

    const unsigned int maxChannelsInPixel = std::min(GetNumChannels(), static_cast<unsigned int>(outPixelData.size()));
    for (unsigned int c = 0; c < maxChannelsInPixel; ++c)
    {
        outPixelData[c] = pixelData[c];
    }

    return std::make_tuple(outPixelData[0], outPixelData[1], outPixelData[2]);
}

std::vector<float> InferenceTestImage::Resize(unsigned int newWidth,
                                              unsigned int newHeight,
                                              const std::array<float, 3>& mean,
                                              const std::array<float, 3>& stddev,
                                              float scale)
{
    if (newWidth == 0 || newHeight == 0)
    {
        throw armnn::InvalidArgumentException(
                    boost::str(boost::format("None of the dimensions passed to a resize operation can be zero. "
                                             "Requested width: %1%. Requested height: %2%.")
                               % newWidth
                               % newHeight));
    }

    return ResizeBilinearAndNormalize(*this, newWidth, newHeight, scale, mean, stddev);
}


/** \example orplanning_door.cpp
    \author Rosen Diankov

    \image html cppexample_orplanning_door.jpg "Robot opening door for different initial/goal configurations."
    \image latex cppexample_orplanning_door.jpg "Robot opening door for different initial/goal configurations." width=10cm

    Shows how to use a planner two plan for a robot opening a door.
    The configuration space consists of the robot's manipulator joints and the door joint.
    A generic planner is just passed in this configuration space inside its planner parameters.

    <b>Full Example Code:</b>
 */
#include <openrave-core.h>
#include <vector>
#include <sstream>
#include <boost/format.hpp>

#include <openrave/planningutils.h>

#include "orexample.h"

using namespace OpenRAVE;
using namespace std;

namespace cppexamples {

/// \brief builds up the configuration space of a robot and a door
class DoorConfiguration : public boost::enable_shared_from_this<DoorConfiguration>
{
    static dReal TransformDistance2(const Transform& t1, const Transform& t2, dReal frotweight=1, dReal ftransweight=1)
    {
        //dReal facos = RaveAcos(min(dReal(1),RaveFabs(dot4(t1.rot,t2.rot))));
        dReal facos = min((t1.rot-t2.rot).lengthsqr4(),(t1.rot+t2.rot).lengthsqr4());
        return (t1.trans-t2.trans).lengthsqr3() + frotweight*facos; //*facos;
    }

public:
    DoorConfiguration(RobotBase::ManipulatorPtr pmanip, KinBody::JointPtr pdoorjoint) : _pmanip(pmanip), _pdoorjoint(pdoorjoint) {
        _probot = pmanip->GetRobot();
        _ptarget = RaveInterfaceCast<RobotBase>(pdoorjoint->GetParent());
        _pdoorlink = _pdoorjoint->GetHierarchyChildLink();
        _tgrasp = Transform(Vector(0.65839364,  0.68616871, -0.22320624, -0.21417118),Vector(0.23126595, -0.01218956,  0.1084143));
        BOOST_ASSERT(_pdoorjoint->GetDOF()==1);
    }

    virtual ~DoorConfiguration() {
    }

    int GetDOF() {
        return (int)_pmanip->GetArmIndices().size()+1;
    }

    dReal ComputeDistance(const std::vector<dReal>& c0, const std::vector<dReal>& c1) {
        dReal d1 = _robotdistmetric->Eval(c0,c1);
        std::vector<dReal> door0(1), door1(1);
        door0[0] = c0.back();
        door1[0] = c1.back();
        dReal d2 = _doordistmetric->Eval(door0,door1);
        return RaveSqrt(d1*d1+d2*d2);
    }

    bool Sample(std::vector<dReal>&v) {
        for(int i = 0; i < 50; ++i) {
            _robotsamplefn->Sample(v);
            vector<dReal> vdoor;
            _doorsamplefn->Sample(vdoor);

            v.resize(GetDOF());
            v.back() = vdoor.at(0);
            if( _SetState(v) ) {
                return true;
            }
        }
        return false;
    }

    bool _SetState(std::vector<dReal>& v, int filteroptions=IKFO_CheckEnvCollisions|IKFO_IgnoreCustomFilters) {
        // save state before modifying it
        RobotBase::RobotStateSaverPtr savestate1(new RobotBase::RobotStateSaver(_probot));
        RobotBase::RobotStateSaverPtr savestate2(new RobotBase::RobotStateSaver(_ptarget));

        vector<dReal> vdoor(1); vdoor[0] = v.back();
        _ptarget->SetActiveDOFValues(vdoor);
        _probot->SetActiveDOFValues(v);

        vector<dReal> vsolution;
        bool bsuccess = _pmanip->FindIKSolution(_pdoorlink->GetTransform() * _tgrasp, vsolution, filteroptions);
        if( bsuccess ) {
            savestate1.reset();
            savestate2.reset();
            _probot->SetActiveDOFValues(vsolution);
            std::copy(vsolution.begin(),vsolution.end(),v.begin());
            _ptarget->SetActiveDOFValues(vdoor);
        }
        return bsuccess;
    }

    void SetState(const std::vector<dReal>& v)
    {
        vector<dReal> vtemp = v;
        if( !_SetState(vtemp) ) {
            throw OPENRAVE_EXCEPTION_FORMAT0("could not set state",ORE_InvalidArguments);
        }
    }

    void GetState(std::vector<dReal>& v)
    {
        _probot->GetActiveDOFValues(v);
        v.resize(GetDOF());
        v.back() = _pdoorjoint->GetValue(0);
    }

    void DiffState(std::vector<dReal>& v1,const std::vector<dReal>& v2)
    {
        _probot->SubtractActiveDOFValues(v1,v2);
        v1.back() -= v2.back();
    }

    void SubtractStates(std::vector<dReal>& q1, const std::vector<dReal>& q2)
    {
        BOOST_ASSERT(q1.size()==q2.size());
        for(size_t i = 0; i < q1.size(); ++i) {
            q1[i] -= q2[i];
        }
    }

    bool NeightState(std::vector<dReal>& v,const std::vector<dReal>& vdelta, int fromgoal)
    {
        _vprevsolution = v;
        // the previous solution should already be set on the robot, so do a sanity check
        _tmanipprev = _pmanip->GetTransform();
        Transform tdoorprev = _pdoorlink->GetTransform();
        BOOST_ASSERT( TransformDistance2(tdoorprev*_tgrasp,_tmanipprev) <= g_fEpsilon );

        {
            KinBody::KinBodyStateSaver statesaver(_ptarget);
            vector<dReal> vdoor(1);
            vdoor[0] = v.back()+0.5*vdelta.back();
            _ptarget->SetActiveDOFValues(vdoor);
            _tmanipmidreal = _pdoorlink->GetTransform()*_tgrasp;
        }

        for(int i = 0; i < GetDOF(); ++i) {
            v.at(i) += vdelta.at(i);
        }

        return _SetState(v,IKFO_CheckEnvCollisions);
    }

    // due to discontinues check that the robot midpoint is also along the door's expected trajectory
    // take the midpoint of the solutions and ikparameterization and see if they are close
    IkReturn _CheckContinuityFilter(std::vector<dReal>& vsolution, RobotBase::ManipulatorConstPtr pmanip, const IkParameterization& ikp)
    {
        Transform tmanipnew = ikp.GetTransform6D();
        std::vector<dReal> vmidsolution(_probot->GetActiveDOF());
        dReal realdist2 = TransformDistance2(_tmanipprev, tmanipnew);
        const dReal ikmidpointmaxdist2mult = 0.5;

        RobotBase::RobotStateSaver savestate(_probot);
        for(int i = 0; i < _probot->GetActiveDOF(); ++i) {
            vmidsolution.at(i) = 0.5*(_vprevsolution.at(i)+vsolution.at(i));
        }
        _probot->SetActiveDOFValues(vmidsolution);

        Transform tmanipmid = _pmanip->GetTransform();
        dReal middist2 = TransformDistance2(tmanipmid, _tmanipmidreal);
        if( middist2 > g_fEpsilon && middist2 > ikmidpointmaxdist2mult*realdist2 ) {
            RAVELOG_VERBOSE(str(boost::format("rejected due to discontinuity at mid-point %e > %e")%middist2%(ikmidpointmaxdist2mult*realdist2)));
            return IKRA_Reject;
        }
        return IKRA_Success;
    }

    void SetPlannerParameters(PlannerBase::PlannerParametersPtr params)
    {
        _probot->SetActiveDOFs(_pmanip->GetArmIndices());
        vector<int> v(1); v[0] = _pdoorjoint->GetDOFIndex();
        _ptarget->SetActiveDOFs(v);

        params->_configurationspecification = _probot->GetActiveConfigurationSpecification() + _ptarget->GetActiveConfigurationSpecification();
        _probot->GetActiveDOFLimits(params->_vConfigLowerLimit,params->_vConfigUpperLimit);
        _pdoorjoint->GetLimits(params->_vConfigLowerLimit,params->_vConfigUpperLimit,true);

        _probot->GetActiveDOFVelocityLimits(params->_vConfigVelocityLimit);
        params->_vConfigVelocityLimit.push_back(100);

        _probot->GetActiveDOFAccelerationLimits(params->_vConfigAccelerationLimit);
        params->_vConfigAccelerationLimit.push_back(100);

        _probot->GetActiveDOFResolutions(params->_vConfigResolution);
        params->_vConfigResolution.push_back(0.05);

        _robotdistmetric.reset(new planningutils::SimpleDistanceMetric(_probot));
        _doordistmetric.reset(new planningutils::SimpleDistanceMetric(_ptarget));
        params->_distmetricfn = boost::bind(&DoorConfiguration::ComputeDistance,shared_from_this(),_1,_2);
        params->_diffstatefn = boost::bind(&DoorConfiguration::DiffState,shared_from_this(),_1,_2);

        SpaceSamplerBasePtr pconfigsampler1 = RaveCreateSpaceSampler(_probot->GetEnv(),str(boost::format("robotconfiguration %s")%_probot->GetName()));
        _robotsamplefn.reset(new planningutils::SimpleNeighborhoodSampler(pconfigsampler1,boost::bind(&planningutils::SimpleDistanceMetric::Eval,_robotdistmetric,_1,_2), boost::bind(&RobotBase::SubtractActiveDOFValues,_probot,_1,_2)));
        SpaceSamplerBasePtr pconfigsampler2 = RaveCreateSpaceSampler(_probot->GetEnv(),str(boost::format("robotconfiguration %s")%_ptarget->GetName()));
        _doorsamplefn.reset(new planningutils::SimpleNeighborhoodSampler(pconfigsampler2,boost::bind(&planningutils::SimpleDistanceMetric::Eval,_doordistmetric,_1,_2), boost::bind(&DoorConfiguration::SubtractStates,shared_from_this(),_1,_2)));
        params->_samplefn = boost::bind(&DoorConfiguration::Sample,shared_from_this(),_1);
        params->_sampleneighfn.clear(); // won't be using it

        params->_setstatefn = boost::bind(&DoorConfiguration::SetState,shared_from_this(),_1);
        params->_getstatefn = boost::bind(&DoorConfiguration::GetState,shared_from_this(),_1);
        params->_neighstatefn = boost::bind(&DoorConfiguration::NeightState,shared_from_this(),_1,_2,_3);

        std::list<KinBodyPtr> listCheckCollisions;
        listCheckCollisions.push_back(_probot);
        _collision.reset(new planningutils::LineCollisionConstraint(listCheckCollisions));
        params->_checkpathconstraintsfn = boost::bind(&planningutils::LineCollisionConstraint::Check,_collision,params, _1, _2, _3, _4);

        _ikfilter = _pmanip->GetIkSolver()->RegisterCustomFilter(0, boost::bind(&DoorConfiguration::_CheckContinuityFilter, shared_from_this(), _1, _2, _3));
    }

    RobotBase::ManipulatorPtr _pmanip;
    KinBody::LinkPtr _pdoorlink;
    KinBody::JointPtr _pdoorjoint;
    Transform _tgrasp; ///< the grasp transform in the door link frame
    RobotBasePtr _probot, _ptarget;
    vector<dReal> _vprevsolution;
    Transform _tmanipprev, _tmanipmidreal;
    UserDataPtr _ikfilter;

    boost::shared_ptr<planningutils::SimpleDistanceMetric> _robotdistmetric, _doordistmetric;
    boost::shared_ptr<planningutils::SimpleNeighborhoodSampler> _robotsamplefn, _doorsamplefn;
    boost::shared_ptr<planningutils::LineCollisionConstraint> _collision;
};

typedef boost::shared_ptr<DoorConfiguration> DoorConfigurationPtr;

class PlanningDoorExample : public OpenRAVEExample
{
public:
    virtual void demothread(int argc, char ** argv) {
        string scenefilename = "data/wam_cabinet.env.xml";
        RaveSetDebugLevel(Level_Debug);
        penv->Load(scenefilename);

        RobotBasePtr probot = penv->GetRobot("BarrettWAM");
        // find the longest manipulator chain to move
        RobotBase::ManipulatorPtr pmanip = probot->GetManipulators().at(0);
        for(size_t i = 1; i < probot->GetManipulators().size(); ++i) {
            if( pmanip->GetArmIndices().size() < probot->GetManipulators()[i]->GetArmIndices().size() ) {
                pmanip = probot->GetManipulators()[i];
            }
        }
        RAVELOG_INFO(str(boost::format("planning with manipulator %s\n")%pmanip->GetName()));

        RobotBasePtr target = penv->GetRobot("Cabinet");
        KinBody::JointPtr pdoorjoint = target->GetJoint("J_right");

        std::vector<dReal> vpreshape(4);
        vpreshape[0] = 2.3; vpreshape[1] = 2.3; vpreshape[2] = 0.8; vpreshape[3] = 0;

        // create the configuration space
        DoorConfigurationPtr doorconfig(new DoorConfiguration(pmanip,pdoorjoint));
        PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
        Transform trobotorig;
        {
            EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment

            probot->SetActiveDOFs(pmanip->GetGripperIndices());
            probot->SetActiveDOFValues(vpreshape);

            doorconfig->SetPlannerParameters(params);
            params->_nMaxIterations = 150; // max iterations before failure

            trobotorig = probot->GetTransform();
        }

        PlannerBasePtr planner = RaveCreatePlanner(penv,"birrt");
        TrajectoryBasePtr ptraj;
        int iter = 0;

        while(IsOk()) {
            iter += 1;
            GraphHandlePtr pgraph;
            {
                EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment

                if( (iter%5) == 0 ) {
                    RAVELOG_INFO("find a new position for the robot\n");
                    for(int i = 0; i < 100; ++i) {
                        Transform tnew = trobotorig;
                        tnew.trans.x += 0.5*(RaveRandomFloat()-0.5);
                        tnew.trans.y += 0.5*(RaveRandomFloat()-0.5);
                        probot->SetTransform(tnew);

                        try {
                            params->_getstatefn(params->vinitialconfig);
                            params->_setstatefn(params->vinitialconfig);
                            params->_getstatefn(params->vinitialconfig);

                            params->vgoalconfig = params->vinitialconfig;
                            params->vgoalconfig.back() = RaveRandomFloat()*1.5; // in radians
                            params->_setstatefn(params->vgoalconfig);
                            params->_getstatefn(params->vgoalconfig);
                            break;
                        }
                        catch(const openrave_exception& ex) {
                            probot->SetTransform(trobotorig);
                        }
                    }
                }
                else {
                    try {
                        params->_getstatefn(params->vinitialconfig);
                        params->_setstatefn(params->vinitialconfig);
                        params->_getstatefn(params->vinitialconfig);

                        params->vgoalconfig = params->vinitialconfig;
                        params->vgoalconfig.back() = RaveRandomFloat()*1.5; // in radians
                        params->_setstatefn(params->vgoalconfig);
                        params->_getstatefn(params->vgoalconfig);
                    }
                    catch(const openrave_exception& ex) {
                        RAVELOG_WARN("initial set failed\n");
                        continue;
                    }
                }

                //params->_sPostProcessingPlanner = "lineartrajectoryretimer";
                ptraj = RaveCreateTrajectory(penv,"");
                if( !planner->InitPlan(probot,params) ) {
                    RAVELOG_WARN("plan failed to init\n");
                    continue;
                }

                // create a new output trajectory
                if( !planner->PlanPath(ptraj) ) {
                    RAVELOG_WARN("plan failed, trying again\n");
                    continue;
                }

                // draw the end effector of the trajectory
                {
                    RobotBase::RobotStateSaver saver(probot); // save the state of the robot since will be setting joint values
                    vector<RaveVector<float> > vpoints;
                    vector<dReal> vtrajdata;
                    for(dReal ftime = 0; ftime <= ptraj->GetDuration(); ftime += 0.01) {
                        ptraj->Sample(vtrajdata,ftime,probot->GetActiveConfigurationSpecification());
                        probot->SetActiveDOFValues(vtrajdata);
                        vpoints.push_back(pmanip->GetEndEffectorTransform().trans);
                    }
                    pgraph = penv->drawlinestrip(&vpoints[0].x,vpoints.size(),sizeof(vpoints[0]),1.0f);
                }

                // send the trajectory to the robot
                probot->GetController()->SetPath(ptraj);
                target->GetController()->SetPath(ptraj);
            }

            // wait for the robot to finish
            while(!probot->GetController()->IsDone() && IsOk()) {
                boost::this_thread::sleep(boost::posix_time::milliseconds(1));
            }
        }
    }
};

} // end namespace cppexamples

int main(int argc, char ** argv)
{
    cppexamples::PlanningDoorExample example;
    return example.main(argc,argv);
}


#define BOOST_TEST_MODULE pcraster com raster
#include <boost/test/unit_test.hpp>
#include "stddefx.h"
#include "csftypes.h"
#include "com_raster.h"

BOOST_AUTO_TEST_CASE(constructor_single_value)
{
  using namespace com;

  Raster<UINT1> v(4,5,8);
  BOOST_CHECK(v.nrRows()==4);
  BOOST_CHECK(v.nrCols()==5);
  BOOST_CHECK(v.cell(0, 0)==8);
  BOOST_CHECK(v.cell(3,4)==8);

  v.setMV(0,2);
  BOOST_CHECK(v.isMV(0,2));
  BOOST_CHECK(!v.isMV(3,4));
}


// A simple program that computes the square root of a number
// using a modification of Newton's method
//
#include <algorithm>
#include <iostream>
#include <sstream>
#include <string>
#include <boost/regex.hpp>

#include "MathFunctions.h"
#include "TutorialConfig.h"

int main (int argc, char *argv[]) {
    //
    // Extract the name of the executable, which is the base file name.
    //
    std::string argv0(argv[0]);
    std::string baseFilename = argv0.substr(argv0.find_last_of("/") + 1);
    //
    // Use regular expressions, via Boost::regex, to make sure it's a valid number.
    //
    boost::regex expr{"-?\\d*([.]\\d+)?"};
    bool isNumeric = argv[1] != NULL && boost::regex_match(argv[1], expr);

    if (!isNumeric) {
        std::cout << baseFilename << " Version " << Tutorial_VERSION_MAJOR << "." << Tutorial_VERSION_MINOR <<std::endl;
        std::cout << "Usage: " << baseFilename << " number" << std::endl;
        return 1;
    }

    const double inputValue = std::stod(argv[1]);
    const double outputValue = mathfunctions::sqrt(inputValue);
    std::cout << "The square root of " << inputValue << " is " << outputValue << std::endl;

    return 0;
}


#include "atom.h"
#include <algorithm>
#include <boost/range/combine.hpp>
#include "color.h"
#include <iostream>
#include <iterator>
#include <fstream>
#include "output_analysis_info.h"

using std::endl;
using std::unique_ptr;
using std::vector;

void output_analysis_info::write_info(const unique_ptr<vector<vector<const bin*>>>& vd_ptr, const unique_ptr<vector<vector<const bin*>>>& srfc_ptr) const  // voids_surface_pointer
{
	std::ofstream File(file_name);
	if (File)
	{
		File << "Volume fraction, pore size" << endl;
		File << std::setprecision(6) << std::fixed;
		const auto [nx, ny, nz] = gr_nf.get_grid_no();
		double total_bins = gr_nf.get_fiber_vol_by_bins();
		if (vd_ptr->size() != srfc_ptr->size())
		{
			std::cerr << Color::Red << "vd_ptr should have the same size with the srfc_ptr!" << endl;
			exit(EXIT_FAILURE);
		}
		for (auto iter_vd = vd_ptr->begin(), iter_surf = srfc_ptr->begin(); iter_vd != vd_ptr->end(); ++iter_vd, ++iter_surf)
		{
			double vol_frac = iter_vd->size() / total_bins;
			vector<atom*> surf_atoms;
			for (auto bin_ptr : *iter_surf)
			{
				vector<atom*> bin_atoms = bin_ptr->get_atoms();
				std::copy(bin_atoms.begin(), bin_atoms.end(), std::back_inserter(surf_atoms));
			}
			double max_distance = 0;
			for (auto iter_first_atom = surf_atoms.begin(); iter_first_atom != surf_atoms.end(); ++iter_first_atom)
			{
				for (auto iter_second_atom = iter_first_atom + 1; iter_second_atom != surf_atoms.end(); ++iter_second_atom)
				{
					double distance = distance_atom(**iter_first_atom, **iter_second_atom, sc_ptr);
					if (distance > max_distance)
					{
						max_distance = distance;
					}
				}
			}
			File << vol_frac << " " << max_distance << endl;
		}
	}
	else
	{
		error();
	}
}


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

#include <gtest/gtest.h>
#include <Eigen/Dense>

#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
// Autodiff is only supported with Eigen 3.3.0 or higher

#include <exotica_core/tools/autodiff_chain_hessian.h>
#include <exotica_core/tools/autodiff_chain_hessian_sparse.h>
#include <exotica_core/tools/autodiff_chain_jacobian.h>
#include <exotica_core/tools/autodiff_chain_jacobian_sparse.h>
#include <exotica_core/tools/finitediff_chain_hessian.h>
#include <exotica_core/tools/finitediff_chain_jacobian.h>

#include <iostream>

using namespace exotica;

constexpr int N = 1000;
constexpr double THRESHOLD_ANALYTIC = 1e-5;
constexpr double THRESHOLD_FINITE_DIFFERENCES = 1e-3;
constexpr double THRESHOLD_FINITE_DIFFERENCES_HESSIAN = 5e-1;
constexpr double INPUT_VECTOR_SCALE = 2.0;

// This function combines Function3(Function2(x)).
// The result will be used for comparison with passing the derivative of Function2 into the AutoDiff.
template <class FunctorType>
struct Function1 : public FunctorType
{
    enum
    {
        Inputs = 4,
        Values = 4,
        JacobianCols = 4
    };

    template <typename T>
    void operator()(const Eigen::Matrix<T, FunctorType::InputType::RowsAtCompileTime, 1> &x, Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> &y) const
    {
        Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> tmp(y.rows());
        // Always cast known scalar type matrices/vectors into the templated type <T>.
        // This is required for AutoDiff to work properly.
        for (int i = 0; i < 4; ++i)
        {
            y(i, 0) = (Eigen::AngleAxis<T>(x(i, 0), Eigen::Vector3d::UnitZ().cast<T>()).toRotationMatrix() * Eigen::Vector3d::UnitX().cast<T>()).dot(Eigen::Vector3d::UnitX().cast<T>());
            tmp(i, 0) = y(i, 0);
            if (i > 0) y(i, 0) += tmp(i - 1, 0);
        }
    }
};

// Function2 rotates a UnitX vector around Z axis.
// This is a helper function that will provide input for Function3.
template <class FunctorType>
struct Function2 : public FunctorType
{
    enum
    {
        Inputs = 4,
        Values = 12,
        JacobianCols = 4
    };

    template <typename T>
    void operator()(const Eigen::Matrix<T, FunctorType::InputType::RowsAtCompileTime, 1> &x, Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> &y) const
    {
        Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> tmp(y.rows());
        for (int i = 0; i < 4; ++i)
        {
            y.block(i * 3, 0, 3, 1) = Eigen::AngleAxis<T>(x(i, 0), Eigen::Vector3d::UnitZ().cast<T>()).toRotationMatrix() * Eigen::Vector3d::UnitX().cast<T>();
        }
    }
};

// This function computes dot product between the input vector and UnitX.
// The input vector will be taken from the output of Function2 (including the derivatives).
template <class FunctorType>
struct Function3 : public FunctorType
{
    enum
    {
        Inputs = 12,
        Values = 4,
        JacobianCols = 4
    };

    template <typename T>
    void operator()(const Eigen::Matrix<T, FunctorType::InputType::RowsAtCompileTime, 1> &x, Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> &y) const
    {
        Eigen::Matrix<T, FunctorType::ValueType::RowsAtCompileTime, 1> tmp(y.rows());
        for (int i = 0; i < 4; ++i)
        {
            y(i, 0) = Eigen::Vector3d::UnitX().cast<T>().dot(x.block(i * 3, 0, 3, 1));
            tmp(i, 0) = y(i, 0);
            if (i > 0) y(i, 0) += tmp(i - 1, 0);
        }
    }
};

// Typedefs for convenience
template <template <typename> class DiffType, typename _FunctorType, template <typename> class FunctionType>
struct TestingBase
{
    typedef _FunctorType FunctorType;
    typedef FunctionType<FunctorType> Function;
    typedef DiffType<Function> Diff;
    typedef typename Diff::InputType InputType;
    typedef typename Diff::ValueType ValueType;
    typedef typename Diff::JacobianType JacobianType;
};

// Compute Jacobians using AutoDiff
template <class T>
struct JacobianFull : public T
{
    void operator()(const typename T::InputType &x, typename T::ValueType &y,
                    typename T::JacobianType &j)
    {
        typename T::Function f;
        typename T::Diff autoj(f);

        if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
            y.resize(T::Function::Values, 1);
        if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
            j.resize(T::Function::Values, T::Function::JacobianCols);

        // Compute full Jacobian
        autoj(x, y, j);
    }
};

// Compute Jacobians using AutoDiff given the Jacobian of the input variables
template <class T>
struct JacobianCompound : public T
{
    void operator()(const typename T::InputType &x, typename T::ValueType &y,
                    typename T::JacobianType &j, const typename T::Diff::InputJacobianType &ij)
    {
        typename T::Function f;
        typename T::Diff autoj(f);

        if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
            y.resize(T::Function::Values, 1);
        if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
            j.resize(T::Function::Values, T::Function::JacobianCols);

        // Compute compund Jacobian
        autoj(x, y, j, ij);
    }
};

// Compute Jacobians using FiniteDiff
// Requires separate method because of additional template parameters
template <class T, Eigen::NumericalDiffMode mode>
struct JacobianFullFinite : public T
{
    typedef Eigen::FiniteDiffChainJacobian<typename T::Function, mode> diff;

    int operator()(const typename T::InputType &x, typename T::ValueType &y,
                   typename T::JacobianType &j)
    {
        typename T::Function f;
        diff autoj(f);

        if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
            y.resize(T::Function::Values, 1);
        if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
            j.resize(T::Function::Values, T::Function::JacobianCols);

        // Compute full Jacobian
        return autoj(x, y, j);
    }
};

// Compute Jacobians using FiniteDiff given the Jacobian of the input variables
// Requires separate method because of additional template parameters
// compute_intermediate is used to provide update of the inputs
template <class T, class T2, Eigen::NumericalDiffMode mode>
struct JacobianCompoundFinite : public T
{
    typedef Eigen::FiniteDiffChainJacobian<typename T::Function, mode> diff;
    typedef typename T2::Function F2;

    int operator()(const typename diff::InputJacobianRowType &x, typename T::ValueType &y,
                   typename T::JacobianType &j)
    {
        typename T::Function f;

        auto compute_intermediate = [](const typename diff::InputJacobianRowType &jx, typename T::InputType &_x) {
            if (T::InputType::RowsAtCompileTime == Eigen::Dynamic) _x.resize(F2::Values);
            F2 f;
            // This is where the chaining of the functions happens
            f(jx, _x);
        };
        diff autoj(f, compute_intermediate);

        if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
            y.resize(T::Function::Values, 1);
        if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
            j.resize(T::Function::Values, T::Function::JacobianCols);

        // Compute full Jacobian
        return autoj(x, y, j);
    }
};

// Compute Hessians using AutoDiff
template <class T>
struct HessianFull : public T
{
    void operator()(const typename T::InputType &x, typename T::ValueType &y,
                    typename T::JacobianType &j, typename T::Diff::HessianType &hess)
    {
        typename T::Function f;
        typename T::Diff autoj(f);

        if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
        {
            y.resize(T::Function::Values, 1);
            hess.resize(T::Function::Values);
        }
        if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
        {
            j.resize(T::Function::Values, T::Function::JacobianCols);
            for (int i = 0; i < hess.rows(); ++i)
            {
                hess[i].resize(T::Function::JacobianCols, T::Function::JacobianCols);
            }
        }

        // Compute full Hessian
        autoj(x, y, j, hess);
    }
};

// Compute Hessians using AutoDiff given the Jacobian of the input variables
template <class T>
struct HessianCompound : public T
{
    void operator()(const typename T::InputType &x, typename T::ValueType &y,
                    typename T::JacobianType &j, typename T::Diff::HessianType &hess,
                    const typename T::Diff::InputJacobianType &ij, const typename T::Diff::InputHessianType &ihess)
    {
        typename T::Function f;
        typename T::Diff autoj(f);

        if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
        {
            y.resize(T::Function::Values, 1);
            hess.resize(T::Function::Values);
        }
        if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
        {
            j.resize(T::Function::Values, T::Function::JacobianCols);
            for (int i = 0; i < hess.rows(); ++i)
            {
                hess[i].resize(T::Function::JacobianCols, T::Function::JacobianCols);
            }
        }

        // Compute the Jacobian and Hessian of the compound function.
        autoj(x, y, j, hess, ij, ihess);
    }
};

// Compute Hessians using FiniteDiff
// Requires separate method because of additional template parameters
template <class T, Eigen::NumericalDiffMode mode>
struct HessianFullFinite : public T
{
    typedef Eigen::FiniteDiffChainHessian<typename T::Function, mode> diff;

    int operator()(const typename T::InputType &x, typename T::ValueType &y,
                   typename T::JacobianType &j, typename T::Diff::HessianType &hess)
    {
        typename T::Function f;
        diff autoj(f);

        if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
        {
            y.resize(T::Function::Values, 1);
            hess.resize(T::Function::Values);
        }
        if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
        {
            j.resize(T::Function::Values, T::Function::JacobianCols);
            for (int i = 0; i < hess.rows(); ++i)
            {
                hess[i].resize(T::Function::JacobianCols, T::Function::JacobianCols);
            }
        }

        // Compute the Jacobian and Hessian of the compound function.
        return autoj(x, y, j, hess);
    }
};

// Compute Hessians using FiniteDiff given the Jacobian of the input variables
// Requires separate method because of additional template parameters
// compute_intermediate is used to provide update of the inputs
template <class T, class T2, Eigen::NumericalDiffMode mode>
struct HessianCompoundFinite : public T
{
    typedef Eigen::FiniteDiffChainHessian<typename T::Function, mode> diff;
    typedef typename T2::Function F2;

    int operator()(const typename diff::InputJacobianRowType &x, typename T::ValueType &y,
                   typename T::JacobianType &j, typename T::Diff::HessianType &hess)
    {
        typename T::Function f;

        auto compute_intermediate = [](const typename diff::InputJacobianRowType &jx, typename T::InputType &_x) {
            if (T::InputType::RowsAtCompileTime == Eigen::Dynamic) _x.resize(F2::Values);
            F2 f;
            // This is where the chaining of the functions happens
            f(jx, _x);
        };
        diff autoj(f, compute_intermediate);

        if (T::ValueType::RowsAtCompileTime == Eigen::Dynamic)
        {
            y.resize(T::Function::Values, 1);
            hess.resize(T::Function::Values);
        }
        if (T::JacobianType::RowsAtCompileTime == Eigen::Dynamic)
        {
            j.resize(T::Function::Values, T::Function::JacobianCols);
            for (int i = 0; i < hess.rows(); ++i)
            {
                hess[i].resize(T::Function::JacobianCols, T::Function::JacobianCols);
            }
        }

        // Compute the Jacobian and Hessian of the compound function.
        return autoj(x, y, j, hess);
    }
};

// Compile time matrix sizes
struct TestStaticTrait
{
    enum
    {
        Inputs1 = 4,
        Values1 = 4,
        JacobianCols1 = 4,
        Inputs2 = 4,
        Values2 = 12,
        JacobianCols2 = 4,
        Inputs3 = 12,
        Values3 = 4,
        JacobianCols3 = 4
    };
};

// Dynamic marix sizes
struct TestDynamicTrait
{
    enum
    {
        Inputs1 = Eigen::Dynamic,
        Values1 = Eigen::Dynamic,
        JacobianCols1 = Eigen::Dynamic,
        Inputs2 = Eigen::Dynamic,
        Values2 = Eigen::Dynamic,
        JacobianCols2 = Eigen::Dynamic,
        Inputs3 = Eigen::Dynamic,
        Values3 = Eigen::Dynamic,
        JacobianCols3 = Eigen::Dynamic
    };
};

// All tests are comparing Function1(x) == Function3(Function2(x))
template <template <typename> class DiffType, typename Dynamic, Eigen::NumericalDiffMode mode>
void TestJacobians()
{
    for (int i = 0; i < N; ++i)
    {
        // Setup functors
        typedef FunctorBase<double, Dynamic::Inputs1, Dynamic::Values1, Dynamic::JacobianCols1> MyFunctor1;
        typedef FunctorBase<double, Dynamic::Inputs2, Dynamic::Values2, Dynamic::JacobianCols2> MyFunctor2;
        typedef FunctorBase<double, Dynamic::Inputs3, Dynamic::Values3, Dynamic::JacobianCols3> MyFunctor3;
        // Define helper types
        typedef TestingBase<DiffType, MyFunctor1, Function1> Test1;
        typedef TestingBase<DiffType, MyFunctor2, Function2> Test2;
        typedef TestingBase<DiffType, MyFunctor3, Function3> Test3;

        typename Test1::InputType x1;
        if (Test1::InputType::RowsAtCompileTime == Eigen::Dynamic)
        {
            x1 = Test1::InputType::Random(4) * INPUT_VECTOR_SCALE;
        }
        else
        {
            x1 = Test1::InputType::Random() * INPUT_VECTOR_SCALE;
        }

        typename Test1::ValueType y1;
        typename Test1::JacobianType j1;

        typename Test2::ValueType y2;
        typename Test2::JacobianType j2;

        typename Test3::ValueType y3;
        typename Test3::JacobianType j3;

        // Compute Function1(x)'
        JacobianFull<Test1>()(x1, y1, j1);
        // Compute Function2(x)'
        JacobianFull<Test2>()(x1, y2, j2);
        // Compute Function3(Function2(x))'
        JacobianCompound<Test3>()(y2, y3, j3, j2);

        typename Test1::ValueType y1fd;
        typename Test1::JacobianType j1fd;
        typename Test3::ValueType y3fd;
        typename Test3::JacobianType j3fd;

        // Compute Function1(x)' using finite differences
        JacobianFullFinite<Test1, mode>()(x1, y1fd, j1fd);
        // Compute Function3(Function2(x))' using finite differences
        JacobianCompoundFinite<Test3, Test2, mode>()(x1, y3fd, j3fd);

        EXPECT_TRUE((y1 - y3).norm() < THRESHOLD_ANALYTIC);
        EXPECT_TRUE((j1 - j1fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
        EXPECT_TRUE((j3 - j3fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
        EXPECT_TRUE((y1fd - y3fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
        EXPECT_TRUE((y1 - y1fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
    }
}

// The sparse test is only different because FiniteDiff doesn't work with sparse matrices
template <template <typename> class DiffType, typename Dynamic>
void TestJacobiansSparse()
{
    for (int i = 0; i < N; ++i)
    {
        // Setup functors
        typedef FunctorBase<double, Dynamic::Inputs1, Dynamic::Values1, Dynamic::JacobianCols1> MyFunctor1;
        typedef FunctorBase<double, Dynamic::Inputs2, Dynamic::Values2, Dynamic::JacobianCols2> MyFunctor2;
        typedef FunctorBase<double, Dynamic::Inputs3, Dynamic::Values3, Dynamic::JacobianCols3> MyFunctor3;
        // Define sparse helper types
        typedef TestingBase<DiffType, MyFunctor1, Function1> Test1;
        typedef TestingBase<DiffType, MyFunctor2, Function2> Test2;
        typedef TestingBase<DiffType, MyFunctor3, Function3> Test3;

        typename Test1::InputType x1;
        if (Test1::InputType::RowsAtCompileTime == Eigen::Dynamic)
        {
            x1 = Test1::InputType::Random(4) * INPUT_VECTOR_SCALE;
        }
        else
        {
            x1 = Test1::InputType::Random() * INPUT_VECTOR_SCALE;
        }

        typename Test1::ValueType y1;
        typename Test1::JacobianType j1;

        typename Test2::ValueType y2;
        typename Test2::JacobianType j2;

        typename Test3::ValueType y3;
        typename Test3::JacobianType j3;

        // Compute sparse Function1(x)'
        JacobianFull<Test1>()(x1, y1, j1);
        // Compute sparse Function2(x)'
        JacobianFull<Test2>()(x1, y2, j2);
        // Compute sparse Function3(Function2(x))'
        JacobianCompound<Test3>()(y2, y3, j3, j2);

        // Define dense helper types
        typedef TestingBase<Eigen::AutoDiffChainJacobian, MyFunctor1, Function1> Test1D;
        typedef TestingBase<Eigen::AutoDiffChainJacobian, MyFunctor2, Function2> Test2D;
        typedef TestingBase<Eigen::AutoDiffChainJacobian, MyFunctor3, Function3> Test3D;
        typename Test1D::ValueType y1d;
        typename Test1D::JacobianType j1d;
        typename Test2D::ValueType y2d;
        typename Test2D::JacobianType j2d;
        typename Test3D::ValueType y3d;
        typename Test3D::JacobianType j3d;

        // Compute dense Function1(x)'
        JacobianFull<Test1D>()(x1, y1d, j1d);
        // Compute dense Function2(x)'
        JacobianFull<Test2D>()(x1, y2d, j2d);
        // Compute dense Function3(Function2(x))'
        JacobianCompound<Test3D>()(y2d, y3d, j3d, j2d);

        EXPECT_TRUE((y1 - y3).norm() < THRESHOLD_ANALYTIC);
        EXPECT_TRUE((j1 - j1d).norm() < THRESHOLD_ANALYTIC);
        EXPECT_TRUE((j3 - j3d).norm() < THRESHOLD_ANALYTIC);
        EXPECT_TRUE((y1d - y3d).norm() < THRESHOLD_ANALYTIC);
        EXPECT_TRUE((y1 - y1d).norm() < THRESHOLD_ANALYTIC);
    }
}

// Hessian norm
// TODO: define a better norm
template <typename HessianType1, typename HessianType2>
double diffNorm(const HessianType1 &A, const HessianType2 &B)
{
    double ret = 0;
    double tmp;
    for (int i = 0; i < A.rows(); ++i)
    {
        tmp = (A[i] - B[i]).norm();
        ret += tmp * tmp;
    }
    return ret / static_cast<double>(A.rows());
}

template <template <typename> class DiffType, typename Dynamic, Eigen::NumericalDiffMode mode>
void TestHessians()
{
    for (int i = 0; i < N; ++i)
    {
        // Setup functors
        typedef FunctorBase<double, Dynamic::Inputs1, Dynamic::Values1, Dynamic::JacobianCols1> MyFunctor1;
        typedef FunctorBase<double, Dynamic::Inputs2, Dynamic::Values2, Dynamic::JacobianCols2> MyFunctor2;
        typedef FunctorBase<double, Dynamic::Inputs3, Dynamic::Values3, Dynamic::JacobianCols3> MyFunctor3;
        // Define helper types
        typedef TestingBase<DiffType, MyFunctor1, Function1> Test1;
        typedef TestingBase<DiffType, MyFunctor2, Function2> Test2;
        typedef TestingBase<DiffType, MyFunctor3, Function3> Test3;

        typename Test1::InputType x1;
        if (Test1::InputType::RowsAtCompileTime == Eigen::Dynamic)
        {
            x1 = Test1::InputType::Random(4) * INPUT_VECTOR_SCALE;
        }
        else
        {
            x1 = Test1::InputType::Random() * INPUT_VECTOR_SCALE;
        }

        typename Test1::ValueType y1;
        typename Test1::JacobianType j1;
        typename Test1::Diff::HessianType hess1;

        typename Test1::ValueType y1fd;
        typename Test1::JacobianType j1fd;
        typename Test1::Diff::HessianType hess1fd;

        typename Test2::ValueType y2;
        typename Test2::JacobianType j2;
        typename Test2::Diff::HessianType hess2;

        typename Test3::ValueType y3;
        typename Test3::JacobianType j3;
        typename Test3::Diff::HessianType hess3;

        typename Test3::ValueType y3fd;
        typename Test3::JacobianType j3fd;
        typename Test3::Diff::HessianType hess3fd;

        // Compute Function1(x)''
        HessianFull<Test1>()(x1, y1, j1, hess1);
        // Compute Function2(x)''
        HessianFull<Test2>()(x1, y2, j2, hess2);
        // Compute Function3(Function2(x))''
        HessianCompound<Test3>()(y2, y3, j3, hess3, j2, hess2);

        // Compute Function1(x)'' using finite differences
        HessianFullFinite<Test1, mode>()(x1, y1fd, j1fd, hess1fd);
        // Compute Function3(Function2(x))'' using finite differences
        HessianCompoundFinite<Test3, Test2, mode>()(x1, y3fd, j3fd, hess3fd);

        EXPECT_TRUE((y1 - y3).norm() < THRESHOLD_ANALYTIC);
        EXPECT_TRUE((j1 - j1fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
        EXPECT_TRUE((j3 - j3fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
        EXPECT_TRUE((y1fd - y3fd).norm() < THRESHOLD_FINITE_DIFFERENCES);
        EXPECT_TRUE((y1 - y1fd).norm() < THRESHOLD_FINITE_DIFFERENCES);

        EXPECT_TRUE(diffNorm(hess1, hess3) < THRESHOLD_ANALYTIC);
        EXPECT_TRUE(diffNorm(hess1, hess1fd) < THRESHOLD_FINITE_DIFFERENCES_HESSIAN);
        EXPECT_TRUE(diffNorm(hess3, hess3fd) < THRESHOLD_FINITE_DIFFERENCES_HESSIAN);
        EXPECT_TRUE(diffNorm(hess1, hess3fd) < THRESHOLD_FINITE_DIFFERENCES_HESSIAN);
    }
}

// The sparse test is only different because FiniteDiff doesn't work with sparse matrices
template <template <typename> class DiffType, typename Dynamic>
void TestHessiansSparse()
{
    for (int i = 0; i < N; ++i)
    {
        // Setup functors
        typedef FunctorBase<double, Dynamic::Inputs1, Dynamic::Values1, Dynamic::JacobianCols1> MyFunctor1;
        typedef FunctorBase<double, Dynamic::Inputs2, Dynamic::Values2, Dynamic::JacobianCols2> MyFunctor2;
        typedef FunctorBase<double, Dynamic::Inputs3, Dynamic::Values3, Dynamic::JacobianCols3> MyFunctor3;
        // Define sparse helper types
        typedef TestingBase<DiffType, MyFunctor1, Function1> Test1;
        typedef TestingBase<DiffType, MyFunctor2, Function2> Test2;
        typedef TestingBase<DiffType, MyFunctor3, Function3> Test3;

        typename Test1::InputType x1;
        if (Test1::InputType::RowsAtCompileTime == Eigen::Dynamic)
        {
            x1 = Test1::InputType::Random(4) * INPUT_VECTOR_SCALE;
        }
        else
        {
            x1 = Test1::InputType::Random() * INPUT_VECTOR_SCALE;
        }

        typename Test1::ValueType y1;
        typename Test1::JacobianType j1;
        typename Test1::Diff::HessianType hess1;

        typename Test2::ValueType y2;
        typename Test2::JacobianType j2;
        typename Test2::Diff::HessianType hess2;

        typename Test3::ValueType y3;
        typename Test3::JacobianType j3;
        typename Test3::Diff::HessianType hess3;

        // Compute sparse Function1(x)''
        HessianFull<Test1>()(x1, y1, j1, hess1);
        // Compute sparse Function2(x)''
        HessianFull<Test2>()(x1, y2, j2, hess2);
        // Compute sparse Function3(Function2(x))''
        HessianCompound<Test3>()(y2, y3, j3, hess3, j2, hess2);

        // Define dense helper types
        typedef TestingBase<Eigen::AutoDiffChainHessian, MyFunctor1, Function1> Test1D;
        typedef TestingBase<Eigen::AutoDiffChainHessian, MyFunctor2, Function2> Test2D;
        typedef TestingBase<Eigen::AutoDiffChainHessian, MyFunctor3, Function3> Test3D;

        typename Test1D::ValueType y1d;
        typename Test1D::JacobianType j1d;
        typename Test1D::Diff::HessianType hess1d;

        typename Test2D::ValueType y2d;
        typename Test2D::JacobianType j2d;
        typename Test2D::Diff::HessianType hess2d;

        typename Test3D::ValueType y3d;
        typename Test3D::JacobianType j3d;
        typename Test3D::Diff::HessianType hess3d;

        // Compute dense Function1(x)''
        HessianFull<Test1D>()(x1, y1d, j1d, hess1d);
        // Compute dense Function2(x)''
        HessianFull<Test2D>()(x1, y2d, j2d, hess2d);
        // Compute dense Function3(Function2(x))''
        HessianCompound<Test3D>()(y2d, y3d, j3d, hess3d, j2d, hess2d);

        EXPECT_TRUE((y1 - y3).norm() < THRESHOLD_ANALYTIC);
        EXPECT_TRUE((j1 - j1d).norm() < THRESHOLD_ANALYTIC);
        EXPECT_TRUE((j3 - j3d).norm() < THRESHOLD_ANALYTIC);
        EXPECT_TRUE((y1d - y3d).norm() < THRESHOLD_ANALYTIC);
        EXPECT_TRUE((y1 - y1d).norm() < THRESHOLD_ANALYTIC);

        EXPECT_TRUE(diffNorm(hess1, hess3) < THRESHOLD_ANALYTIC);
        EXPECT_TRUE(diffNorm(hess1, hess1d) < THRESHOLD_ANALYTIC);
        EXPECT_TRUE(diffNorm(hess3, hess3d) < THRESHOLD_ANALYTIC);
        EXPECT_TRUE(diffNorm(hess1, hess3d) < THRESHOLD_ANALYTIC);
    }
}

// Through magic of tempalting, test:
// 1. AutoDiffChainJacobian Jacobian computation against finite differences
// 2. AutoDiffChainHessian Jacobian computation against finite differences
// 3. AutoDiffChainHessian Hessian computation against finite differences
// 4. Run 1 with Central differencing
// 5. Run 2 with Central differencing
// 6. Run 3 with Central differencing
// 7. AutoDiffChainJacobianSparse Jacobian computation against AutoDiffChainJacobian
// 8. AutoDiffChainHessianSparse Hessian computation against AutoDiffChainHessian
// 9. Run 1-7 with matrix sizes fixed at compile time

TEST(AutoDiffJacobian, JacobianComputationDynamicMatrix)
{
    TestJacobians<Eigen::AutoDiffChainJacobian, TestDynamicTrait, Eigen::Forward>();
}

TEST(AutoDiffJacobian, JacobianComputationDynamicMatrixCentralDifferences)
{
    TestJacobians<Eigen::AutoDiffChainJacobian, TestDynamicTrait, Eigen::Central>();
}

TEST(AutoDiffJacobian, JacobianComputationTemplatedMatrix)
{
    TestJacobians<Eigen::AutoDiffChainJacobian, TestStaticTrait, Eigen::Forward>();
}

TEST(AutoDiffJacobian, JacobianComputationTemplatedMatrixCentralDifferences)
{
    TestJacobians<Eigen::AutoDiffChainJacobian, TestStaticTrait, Eigen::Central>();
}

TEST(AutoDiffHessian, JacobianComputationDynamicMatrix)
{
    TestJacobians<Eigen::AutoDiffChainHessian, TestDynamicTrait, Eigen::Forward>();
}

TEST(AutoDiffHessian, JacobianComputationDynamicMatrixCentralDifferences)
{
    TestJacobians<Eigen::AutoDiffChainHessian, TestDynamicTrait, Eigen::Central>();
}

TEST(AutoDiffHessian, JacobianComputationTemplatedMatrix)
{
    TestJacobians<Eigen::AutoDiffChainHessian, TestStaticTrait, Eigen::Forward>();
}

TEST(AutoDiffHessian, JacobianComputationTemplatedMatrixCentralDifferences)
{
    TestJacobians<Eigen::AutoDiffChainHessian, TestStaticTrait, Eigen::Central>();
}

TEST(AutoDiffHessian, HessianComputationDynamicMatrix)
{
    TestHessians<Eigen::AutoDiffChainHessian, TestDynamicTrait, Eigen::Forward>();
}

TEST(AutoDiffHessian, HessianComputationDynamicMatrixCentralDifferences)
{
    TestHessians<Eigen::AutoDiffChainHessian, TestDynamicTrait, Eigen::Central>();
}

TEST(AutoDiffHessian, HessianComputationTemplatedMatrix)
{
    TestHessians<Eigen::AutoDiffChainHessian, TestStaticTrait, Eigen::Forward>();
}

TEST(AutoDiffHessian, HessianComputationTemplatedMatrixCentralDifferences)
{
    TestHessians<Eigen::AutoDiffChainHessian, TestStaticTrait, Eigen::Central>();
}

TEST(AutoDiffJacobianSparse, JacobianComputationDynamicMatrix)
{
    TestJacobiansSparse<Eigen::AutoDiffChainJacobianSparse, TestDynamicTrait>();
}

TEST(AutoDiffJacobianSparse, JacobianComputationTemplatedMatrix)
{
    TestJacobiansSparse<Eigen::AutoDiffChainJacobianSparse, TestStaticTrait>();
}

TEST(AutoDiffHessianSparse, HessianComputationDynamicMatrix)
{
    TestHessiansSparse<Eigen::AutoDiffChainHessianSparse, TestDynamicTrait>();
}

TEST(AutoDiffHessianSparse, HessianComputationTemplatedMatrix)
{
    TestHessiansSparse<Eigen::AutoDiffChainHessianSparse, TestStaticTrait>();
}

#endif

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    int ret = RUN_ALL_TESTS();
    return ret;
}


/*
    Copyright 2013 Adobe Systems Incorporated
    Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
    or a copy at http://stlab.adobe.com/licenses.html)
*/

/*************************************************************************************************/

#include <boost/filesystem/path.hpp>
#include <boost/filesystem/fstream.hpp>

#include <adobe/name.hpp>
#include <adobe/timer.hpp>
#include <adobe/zuid.hpp>
#include <adobe/sha.hpp>

#include <iostream>

/****************************************************************************************************/

namespace {

/****************************************************************************************************/

typedef std::size_t (*hash_proc_t)(adobe::name_t name);

/****************************************************************************************************/

std::size_t hash_value(adobe::name_t name) {
    std::size_t seed(0);

    for (const char* first(name.c_str()); *first; ++first)
        seed = 5 * seed + *first;

    return seed;
}

/****************************************************************************************************/

std::size_t fnv1a64(adobe::name_t name) {
    return adobe::fnv1a<64>(name.c_str(), adobe::logical_not());
}

/****************************************************************************************************/

std::size_t fnv1a32(adobe::name_t name) {
    return adobe::fnv1a<32>(name.c_str(), adobe::logical_not());
}

/****************************************************************************************************/

std::vector<adobe::name_t> lexed_number_corpus_init() {
    std::vector<adobe::name_t> result;

    for (std::size_t i(0); i < 500000; ++i)
        result.emplace_back(adobe::name_t(std::to_string(i).c_str()));

    return result;
}

/****************************************************************************************************/

const std::vector<adobe::name_t>& lexed_number_corpus() {
    static std::vector<adobe::name_t> result_s(lexed_number_corpus_init());

    return result_s;
}

/****************************************************************************************************/

std::vector<adobe::name_t> zuid_corpus_init() {
    adobe::zuid_t zuid("0a90862c-fb82-1fff-8749-bd8434b0eeb7");
    std::vector<adobe::name_t> result;

    for (std::size_t i(0); i < 500000; ++i) {
        result.emplace_back(adobe::name_t(zuid.c_str()));

        zuid = adobe::zuid_t(zuid, zuid.str());
    }

    return result;
}

/****************************************************************************************************/

const std::vector<adobe::name_t>& zuid_corpus() {
    static std::vector<adobe::name_t> result_s(zuid_corpus_init());

    return result_s;
}

/****************************************************************************************************/

std::vector<adobe::name_t> word_corpus_init(boost::filesystem::ifstream& stream) {
    std::vector<adobe::name_t> result;

    while (stream) {
        std::string line;

        std::getline(stream, line);

        result.emplace_back(adobe::name_t(line.c_str()));
    }

    return result;
}

/****************************************************************************************************/

const std::vector<adobe::name_t>& word_corpus(boost::filesystem::ifstream& stream) {
    static std::vector<adobe::name_t> result_s(word_corpus_init(stream));

    return result_s;
}

/****************************************************************************************************/

struct collision_t {
    adobe::name_t name1_m;
    adobe::name_t name2_m;
    std::size_t hash_m;
};

/****************************************************************************************************/

void hash_test_specific(const std::vector<adobe::name_t>& corpus, const char* corpus_name,
                        hash_proc_t hash_proc, const char* hash_name) {
    typedef std::map<std::size_t, adobe::name_t> collider_t;

    collider_t collider;
    std::vector<collision_t> collision_vector;

    std::vector<collider_t::value_type> hash_result;

    hash_result.reserve(corpus.size());

    adobe::timer_t timer;

    for (auto& datum : corpus)
        hash_result.emplace_back(collider_t::value_type{hash_proc(datum), datum});

    double time(timer.split());

    for (auto& value : hash_result) {
        collider_t::const_iterator found(collider.find(value.first));

        if (found == collider.end()) {
            collider.insert(value);
        } else {
            collision_vector.push_back(collision_t{value.second, found->second, value.first});
        }
    }

    std::cout << "----\n"
              << "Corpus: " << corpus_name << '\n' << "Hash: " << hash_name << '\n'
              << "Time: " << time << "ms (" << (time / 1e3) << " s)\n"
              << "Collisions: " << collision_vector.size() << '\n';

    if (!collision_vector.empty() && collision_vector.size() < 50) {
        std::cout << "Details:\n";

        for (auto& collision : collision_vector) {
            std::cout << "  " << collision.name1_m << "/" << collision.name2_m << ": 0x" << std::hex
                      << collision.hash_m << std::dec << '\n';
        }
    }
}

/****************************************************************************************************/

void hash_test_corpus(const std::vector<adobe::name_t>& corpus, const char* corpus_name) {
    hash_test_specific(corpus, corpus_name, &hash_value, "hash_value");

    hash_test_specific(corpus, corpus_name, &fnv1a64, "fnv1a64");

    hash_test_specific(corpus, corpus_name, &fnv1a32, "fnv1a32");
}

/****************************************************************************************************/

void hash_test(boost::filesystem::ifstream&& corpus_file) {
    hash_test_corpus(lexed_number_corpus(), "lexed numbers");
    hash_test_corpus(zuid_corpus(), "zuids");

    if (corpus_file)
        hash_test_corpus(word_corpus(corpus_file), "corpus_file");
    else
        std::cout << "INFO : Pass a corpus file to test further.\n";
}

/****************************************************************************************************/

} // namespace

/****************************************************************************************************/

int main(int argc, char** argv) try {
    boost::filesystem::path corpus_file;

    if (argc > 1)
        corpus_file = boost::filesystem::path(argv[1]);

    hash_test(boost::filesystem::ifstream(corpus_file));

    return 0;
}
catch (std::exception& error) {
    std::cout << "Error: " << error.what() << '\n';

    return 1;
}
catch (...) {
    std::cout << "Error: unknown\n";

    return 1;
}

/****************************************************************************************************/


#include "../coin.h"
#include "../params.h"

#include "../../streams.h"

#include "../../test/fixtures.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(sigma_coin_tests, ZerocoinTestingSetup200)

BOOST_AUTO_TEST_CASE(pubcoin_serialization)
{
    secp_primitives::GroupElement coin;
    coin.randomize();

    sigma::PublicCoin pubcoin(coin, sigma::CoinDenomination::SIGMA_DENOM_10);

    CDataStream serialized(SER_NETWORK, PROTOCOL_VERSION);
    serialized << pubcoin;

    sigma::PublicCoin deserialized;
    serialized >> deserialized;

    BOOST_CHECK(pubcoin == deserialized);
}

BOOST_AUTO_TEST_CASE(pubcoin_validate)
{
    auto params = sigma::Params::get_default();

    sigma::PrivateCoin privcoin(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    auto& pubcoin = privcoin.getPublicCoin();

    BOOST_CHECK(pubcoin.validate());
}

BOOST_AUTO_TEST_CASE(getter_setter_priv)
{
    auto params = sigma::Params::get_default();

    sigma::PrivateCoin privcoin(params, sigma::CoinDenomination::SIGMA_DENOM_1);
    sigma::PrivateCoin new_privcoin(params, sigma::CoinDenomination::SIGMA_DENOM_1);

    BOOST_CHECK(privcoin.getPublicCoin() != new_privcoin.getPublicCoin());
    BOOST_CHECK(privcoin.getSerialNumber() != new_privcoin.getSerialNumber());
    BOOST_CHECK(privcoin.getRandomness() != new_privcoin.getRandomness());
    BOOST_CHECK(privcoin.getVersion() == new_privcoin.getVersion());

    new_privcoin.setPublicCoin(privcoin.getPublicCoin());
    new_privcoin.setRandomness(privcoin.getRandomness());
    new_privcoin.setSerialNumber(privcoin.getSerialNumber());
    new_privcoin.setVersion(2);

    BOOST_CHECK(privcoin.getPublicCoin() == new_privcoin.getPublicCoin());
    BOOST_CHECK(privcoin.getSerialNumber() == new_privcoin.getSerialNumber());
    BOOST_CHECK(privcoin.getRandomness() == new_privcoin.getRandomness());
    BOOST_CHECK(new_privcoin.getVersion() == 2);
}

BOOST_AUTO_TEST_SUITE_END()


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#include <boost/test/unit_test.hpp>

#include "../unittest_config.h"

#include "clotho/data_spaces/allele_space/allele_space.hpp"
#include "clotho/data_spaces/population_space/genetic_space.hpp"
#include "clotho/data_spaces/phenotype_evaluator/trait_accumulator.hpp"
#include "clotho/data_spaces/phenotype_evaluator/linear_combination_method.hpp"
#include "clotho/data_spaces/phenotype_evaluator/phenotype_evaluator.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/property_tree/ptree.hpp>
#include <vector>
#include <unordered_map>

typedef boost::random::mt19937                                                  random_engine_type;
typedef double                                                                  position_type;
typedef double                                                                  weight_type;
typedef unsigned long long                                                      block_type;
typedef clotho::genetics::qtl_allele_vectorized< position_type, weight_type >   allele_type;
typedef clotho::genetics::genetic_space< allele_type, block_type, clotho::genetics::column_aligned >              genetic_space_type;
typedef clotho::genetics::TraitWeightAccumulator< genetic_space_type >          trait_accum_type;

typedef typename genetic_space_type::individual_genome_type                     individual_type;

typedef double                                                                      phenotype_type;
typedef clotho::genetics::linear_combination< trait_accum_type, phenotype_type >    linear_combination_type;
typedef clotho::genetics::phenotype_evaluator< linear_combination_type >            phenotype_eval_type;

typedef clotho::genetics::AlleleGenerator< random_engine_type, allele_type >   allele_generator_type;

BOOST_AUTO_TEST_SUITE( test_data_space )

BOOST_AUTO_TEST_CASE( single_trait_linear_combination_phenotype_test ) {

    size_t exp_alleles = 200, exp_genomes = 20, exp_traits = 1;
    genetic_space_type gs;

    gs.grow( exp_genomes, exp_alleles );
    gs.getAlleleSpace().grow( exp_alleles, exp_traits );
    gs.getSequenceSpace().clear();

    random_engine_type          rand(1234);
    boost::property_tree::ptree exp_config;

    exp_config.put( "neutral.p", 0.5 );     // qtl alleles may be neutral
    exp_config.put( "traits.mean", 0.0 );
    exp_config.put( "traits.sigma", 1.0 );

    allele_generator_type   agen( &rand, exp_config );

    size_t i = 0;
    while( i < exp_alleles ) {
        agen( gs.getAlleleSpace(), i++ );
    }

    size_t id = 10;
    individual_type exp_ind = std::make_pair(2 * id, 2 * id + 1);
    individual_type obs_ind = gs.getIndividualAt( id );

    BOOST_REQUIRE_MESSAGE( obs_ind.first == exp_ind.first, "Unexpected individual's first sequence; Observed: " << obs_ind.first << "; Expected: " << exp_ind.first );
    BOOST_REQUIRE_MESSAGE( obs_ind.second == exp_ind.second, "Unexpected individual's second sequence; Observed: " << obs_ind.second << "; Expected: " << exp_ind.second );

    typedef  std::vector< std::pair< size_t, size_t> > allele_index_type;
    typedef typename allele_index_type::iterator allele_iterator;

    std::unordered_map< size_t, weight_type > exp_trait_w;
    allele_index_type exp_a;

    exp_a.push_back( std::make_pair( exp_ind.first, 12) );
    exp_a.push_back( std::make_pair( exp_ind.first, 35) );
    exp_a.push_back( std::make_pair( exp_ind.first, 120) );
    exp_a.push_back( std::make_pair( exp_ind.second, 50) );
    exp_a.push_back( std::make_pair( exp_ind.second, 150) );
    exp_a.push_back( std::make_pair( exp_ind.second, 199) );

    phenotype_type exp_pheno = 0.0;

    allele_iterator ait = exp_a.begin();
    while( ait != exp_a.end() ) {
        std::pair< size_t, size_t > tmp = *ait++;
        gs.getSequenceSpace().flip( tmp.first, tmp.second );

        typedef typename allele_type::trait_iterator iterator;

        iterator it = gs.getAlleleSpace().getTraitIterator( tmp.second );
        weight_type w = 0.0;
        while( it.hasNext() ) {
            weight_type _w = it.next();
            w += _w;
        }

        exp_pheno += w;

        if( exp_trait_w.find( tmp.first ) != exp_trait_w.end() ) {
            w += exp_trait_w[ tmp.first ];
        }
        exp_trait_w[ tmp.first ] = w;
    }

    trait_accum_type tacc;

    tacc.update( gs );

    phenotype_eval_type ph;

    ph.update( &gs, tacc );

    weight_type exp_weight_0 = exp_trait_w[ exp_ind.first ];
    weight_type exp_weight_1 = exp_trait_w[ exp_ind.second ];

    weight_type obs_weight_0 = tacc.getTraitAt( exp_ind.first )->at( 0 );
    weight_type obs_weight_1 = tacc.getTraitAt( exp_ind.second )->at( 0 );

    BOOST_REQUIRE_MESSAGE( exp_weight_0 == obs_weight_0, "Unexpected trait weight 0; Observed: " << obs_weight_0 << "; Expected: " << exp_weight_0 );
    BOOST_REQUIRE_MESSAGE( exp_weight_1 == obs_weight_1, "Unexpected trait weight 0; Observed: " << obs_weight_1 << "; Expected: " << exp_weight_1 );

    phenotype_type obs_pheno = ph.getPhenotypeAt( id );

    BOOST_REQUIRE_MESSAGE( exp_pheno != 0.0, "Unexpected zero phenotype value" );
    BOOST_REQUIRE_MESSAGE( exp_pheno == obs_pheno, "Unexpected phenotype; Observed: " << obs_pheno << "; Expected: " << exp_pheno );
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once
#include "../argcheck.hpp"
#include "IALSLearningConfig.hpp"
#include "definitions.hpp"
#include <Eigen/Cholesky>
#include <atomic>
#include <cstddef>
#include <cstdint>
#include <future>
#include <iostream>
#include <random>
#include <sstream>
#include <stdexcept>
#include <thread>
#include <vector>

namespace ials11 {
using namespace std;

struct Solver {
  Solver(size_t K, Real reg) : reg(reg), P(K, K) {}

  inline void initialize(DenseMatrix &factor, Real init_stdev,
                         int random_seed) {
    if (init_stdev > 0) {
      std::mt19937 gen(random_seed);
      std::normal_distribution<Real> dist(0.0, init_stdev);
      for (int i = 0; i < factor.rows(); i++) {
        for (int k = 0; k < factor.cols(); k++) {
          factor(i, k) = dist(gen);
        }
      }
    }
  }

  inline void prepare_p(const DenseMatrix &other_factor,
                        const IALSLearningConfig &config) {
    const int64_t mb_size = 16;
    P = DenseMatrix::Zero(other_factor.cols(), other_factor.cols());

    std::atomic<int64_t> cursor{static_cast<size_t>(0)};

    std::vector<std::future<DenseMatrix>> workers;
    for (size_t i = 0; i < config.n_threads; i++) {
      workers.emplace_back(std::async(std::launch::async, [&other_factor,
                                                           &cursor, mb_size]() {
        DenseMatrix P_local =
            DenseMatrix::Zero(other_factor.cols(), other_factor.cols());
        while (true) {
          int64_t cursor_local = cursor.fetch_add(mb_size);
          if (cursor_local >= other_factor.rows())
            break;
          int64_t block_end =
              std::min<int64_t>(cursor_local + mb_size,
                                static_cast<int64_t>(other_factor.rows()));
          P_local.noalias() +=
              other_factor.middleRows(cursor_local, block_end - cursor_local)
                  .transpose() *
              other_factor.middleRows(cursor_local, block_end - cursor_local);
        }
        return P_local;
      }));
    }
    for (auto &w : workers) {
      P.noalias() += w.get();
    }
    for (int i = 0; i < P.rows(); i++) {
      P.coeffRef(i, i) += this->reg;
    }
  }

  inline DenseMatrix X_to_vector(const SparseMatrix &X,
                                 const DenseMatrix &other_factor,
                                 const IALSLearningConfig &config) const {
    if (X.cols() != other_factor.rows()) {
      std::cout << this << std::endl;
      std::stringstream ss;
      ss << "Shape mismatch: X.cols() = " << X.cols()
         << " but other.factor.rows() = " << other_factor.rows() << ".";
      throw std::invalid_argument(ss.str());
    }
    DenseMatrix result = DenseMatrix::Zero(X.rows(), P.rows());
    this->step(result, X, other_factor, config);

    return result;
  }

  inline void step_cg(DenseMatrix &target_factor, const SparseMatrix &X,
                      const DenseMatrix &other_factor,
                      const IALSLearningConfig &config) const {

    std::vector<std::thread> workers;

    std::atomic<int> cursor(0);
    for (size_t ind = 0; ind < config.n_threads; ind++) {
      workers.emplace_back([this, &target_factor, &cursor, &X, &other_factor,
                            &config]() {
        DenseVector b(P.rows()), x(P.rows()), r(P.rows()), p(P.rows()),
            Ap(P.rows());
        while (true) {
          int cursor_local = cursor.fetch_add(1);
          if (cursor_local >= target_factor.rows()) {
            break;
          }

          b.array() = 0;

          x = target_factor.row(cursor_local)
                  .transpose(); // use the previous value

          size_t nnz = 0;
          for (SparseMatrix::InnerIterator it(X, cursor_local); it; ++it) {
            b.noalias() += (1 + config.alpha * it.value()) *
                           other_factor.row(it.col()).transpose();
            nnz++;
          }
          if (nnz == 0u) {
            target_factor.row(cursor_local).array() = 0;
            continue;
          }
          r = b - P * x;
          for (SparseMatrix::InnerIterator it(X, cursor_local); it; ++it) {
            Real vdotx = other_factor.row(it.col()) * x;
            r.noalias() -= (it.value() * config.alpha * vdotx) *
                           other_factor.row(it.col()).transpose();
          }

          p = r;

          size_t cg_max_iter =
              config.max_cg_steps == 0u ? P.rows() : config.max_cg_steps;

          for (size_t cg_iter = 0; cg_iter < cg_max_iter; cg_iter++) {
            Real r2 = r.squaredNorm();
            Ap = P * p;
            for (SparseMatrix::InnerIterator it(X, cursor_local); it; ++it) {
              Real vdotp = other_factor.row(it.col()) * p;
              Ap += (it.value() * config.alpha * vdotp) *
                    other_factor.row(it.col()).transpose();
            }

            Real alpha_denom = p.transpose() * Ap;
            Real alpha = r2 / alpha_denom;
            x.noalias() += alpha * p;
            r.noalias() -= alpha * Ap;
            if (r.norm() <= 1e-10) {
              break;
            }
            Real beta = r.squaredNorm() / r2;
            p.noalias() = r + beta * p;
          }
          target_factor.row(cursor_local) = x.transpose();
        }
      });
    }
    for (auto &w : workers) {
      w.join();
    }
  }

  inline void step_cholesky(DenseMatrix &target_factor, const SparseMatrix &X,
                            const DenseMatrix &other_factor,
                            const IALSLearningConfig &config) const {

    std::vector<std::thread> workers;

    std::atomic<int> cursor(0);
    for (size_t ind = 0; ind < config.n_threads; ind++) {
      workers.emplace_back(
          [this, &target_factor, &cursor, &X, &other_factor, &config]() {
            DenseMatrix P_local(P.rows(), P.cols());
            DenseVector B(P.rows());
            DenseVector vcache(P.rows()), r(P.rows());
            while (true) {
              int cursor_local = cursor.fetch_add(1);
              if (cursor_local >= target_factor.rows()) {
                break;
              }
              P_local.noalias() = P;
              B.array() = static_cast<Real>(0);
              for (SparseMatrix::InnerIterator it(X, cursor_local); it; ++it) {
                int64_t other_index = it.col();
                Real alphaX = (config.alpha * it.value());
                vcache = other_factor.row(other_index).transpose();
                P_local.noalias() += alphaX * vcache * vcache.transpose();
                B.noalias() += (1 + alphaX) * vcache;
              }
              Eigen::LLT<Eigen::Ref<DenseMatrix>> llt(P_local);
              target_factor.row(cursor_local) = llt.solve(B);
            }
          });
    }
    for (auto &w : workers) {
      w.join();
    }
  }

  inline void step(DenseMatrix &target_factor, const SparseMatrix &X,
                   const DenseMatrix &other_factor,
                   const IALSLearningConfig &config) const {
    if (config.use_cg) {
      step_cg(target_factor, X, other_factor, config);
    } else {
      step_cholesky(target_factor, X, other_factor, config);
    }
  }
  Real reg;
  // DenseMatrix &factor;
  DenseMatrix P;
  DenseMatrix Pinv;
}; // namespace ials11

struct IALSTrainer {
  inline IALSTrainer(const IALSLearningConfig &config, const SparseMatrix &X)
      : config_(config), K(config.K), n_users(X.rows()), n_items(X.cols()),
        user(n_users, K), item(n_items, K), user_solver(K, config.reg),
        item_solver(K, config.reg), X(X), X_t(X.transpose()) {
    this->X.makeCompressed();
    this->X_t.makeCompressed();

    user_solver.initialize(user, config.init_stdev, config.random_seed);
    item_solver.initialize(item, config.init_stdev, config.random_seed);
  }

  inline IALSTrainer(const IALSLearningConfig &config, const DenseMatrix &user_,
                     const DenseMatrix &item_)
      : config_(config), K(user_.cols()), n_users(user_.rows()),
        n_items(item_.rows()), user(user_), item(item_),
        user_solver(K, config.reg), item_solver(K, config.reg) {
    this->user_solver.prepare_p(item, config);
    this->item_solver.prepare_p(user, config);
  }

  inline void step() {

    user_solver.prepare_p(item, config_);
    user_solver.step(user, X, item, config_);
    item_solver.prepare_p(user, config_);
    item_solver.step(item, X_t, user, config_);
  };

  inline DenseMatrix transform_user(const SparseMatrix &X) const {
    return this->user_solver.X_to_vector(X, item, config_);
  }

  inline DenseMatrix transform_item(const SparseMatrix &X) const {
    return this->item_solver.X_to_vector(X.transpose(), user, config_);
  }

  DenseMatrix user_scores(size_t userblock_begin, size_t userblock_end) {
    irspack::check_arg(
        userblock_end >= userblock_begin,
        "userblock_end must be greater than or equal to userblock_begin");
    irspack::check_arg(
        n_users >= userblock_end,
        "userblock_end must be smaller than or equal to n_users");

    int64_t result_size = userblock_end - userblock_begin;
    DenseMatrix result(result_size, n_items);
    std::vector<std::thread> workers;
    std::atomic<int64_t> cursor(0);
    for (size_t ind = 0; ind < config_.n_threads; ind++) {
      workers.emplace_back([this, userblock_begin, &cursor, result_size,
                            &result]() {
        const int64_t chunk_size = 16;
        while (true) {
          auto block_begin = cursor.fetch_add(chunk_size);
          if (block_begin >= result_size) {
            break;
          }
          auto block_size =
              std::min(block_begin + chunk_size, result_size) - block_begin;
          result.middleRows(block_begin, block_size).noalias() =
              this->user.middleRows(block_begin + userblock_begin, block_size) *
              this->item.transpose();
        }
      });
    }
    for (auto &worker : workers) {
      worker.join();
    }
    return result;
  }

public:
  const IALSLearningConfig config_;
  const size_t K;
  const size_t n_users, n_items;
  DenseMatrix user, item;
  Solver user_solver, item_solver;

private:
  SparseMatrix X, X_t;
};
} // namespace ials11


// __BEGIN_LICENSE__
// Copyright (C) 2006-2010 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#ifdef _MSC_VER
#pragma warning(disable:4244)
#pragma warning(disable:4267)
#pragma warning(disable:4996)
#endif

#ifdef NDEBUG
#undef NDEBUG
#endif

#include <cstdlib>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

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

#include <vw/Math/EulerAngles.h>
#include <vw/Image/ImageView.h>
#include <vw/Image/Algorithms.h>
#include <vw/Image/ImageViewRef.h>
#include <vw/Image/Filter.h>
#include <vw/Image/PixelMask.h>
#include <vw/Image/MaskViews.h>
#include <vw/Image/PerPixelAccessorViews.h>
#include <vw/FileIO/DiskImageView.h>
#include <vw/FileIO/DiskImageResourceGDAL.h>
#include <vw/Cartography/GeoReference.h>
#include <vw/tools/Common.h>

using namespace vw;

// Global Variables
std::string input_file_name, output_file_name = "";
double azimuth, elevation, scale;
double nodata_value;
double blur_sigma;

// Allows FileIO to correctly read/write these pixel types
namespace vw {
  template<> struct PixelFormatID<Vector3>   { static const PixelFormatEnum value = VW_PIXEL_XYZ; };
}

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

//  compute_normals()
//
// Compute a vector normal to the surface of a DEM for each given
// pixel.  The normal is computed by forming a plane with three points
// in the vicinity of the requested pixel, and then finding the vector
// normal to that plane.  The user must specify the scale in the [u,v]
// directions so that the direction of the vector in physical space
// can be properly ascertained.  This is often contained in the (0,0)
// and (1,1) entry of the georeference transform.
class ComputeNormalsFunc : public ReturnFixedType<PixelMask<Vector3> >
{
  double m_u_scale, m_v_scale;

public:
  ComputeNormalsFunc(double u_scale, double v_scale) :
    m_u_scale(u_scale), m_v_scale(v_scale) {}

  BBox2i work_area() const { return BBox2i(Vector2i(0, 0), Vector2i(1, 1)); }

  template <class PixelAccessorT>
  PixelMask<Vector3> operator() (PixelAccessorT const& accessor_loc) const {
    PixelAccessorT acc = accessor_loc;

    // Pick out the three altitude values.
    if (is_transparent(*acc))
      return PixelMask<Vector3>();
    double alt1 = *acc;

    acc.advance(1,0);
    if (is_transparent(*acc))
      return PixelMask<Vector3>();
    double alt2 = *acc;

    acc.advance(-1,1);
    if (is_transparent(*acc))
      return PixelMask<Vector3>();
    double alt3 = *acc;

    // Form two orthogonal vectors in the plane containing the three
    // altitude points
    Vector3 n1(m_u_scale, 0, alt2-alt1);
    Vector3 n2(0, m_v_scale, alt3-alt1);

    // Return the vector normal to the local plane.
    return normalize(cross_prod(n1,n2));
  }
};

template <class ViewT>
UnaryPerPixelAccessorView<EdgeExtensionView<ViewT,ConstantEdgeExtension>, ComputeNormalsFunc> compute_normals(ImageViewBase<ViewT> const& image,
                                                                                double u_scale, double v_scale) {
  return UnaryPerPixelAccessorView<EdgeExtensionView<ViewT,ConstantEdgeExtension>, ComputeNormalsFunc>(edge_extend(image.impl(), ConstantEdgeExtension()),
                                                                                ComputeNormalsFunc (u_scale, v_scale));
}

class DotProdFunc : public ReturnFixedType<PixelMask<PixelGray<double> > > {
  Vector3 m_vec;
public:
  DotProdFunc(Vector3 const& vec) : m_vec(vec) {}
  PixelMask<PixelGray<double> > operator() (PixelMask<Vector3> const& pix) const {
    if (is_transparent(pix))
      return PixelMask<PixelGray<double> >();
    else {
//       std::cout << "Vec1 : " << pix.child() << "   " << norm_2(pix.child()) << "\n";
//       std::cout << "Vec2 : " << m_vec << "   " << norm_2(m_vec) << "\n";
//       std::cout << "OVerall: " << dot_prod(pix.child(),m_vec)/(norm_2(pix.child()) * norm_2(m_vec)) << "\n\n";
      return dot_prod(pix.child(),m_vec)/(norm_2(pix.child()) * norm_2(m_vec));
    }
  }
};

template <class ViewT>
UnaryPerPixelView<ViewT, DotProdFunc> dot_prod(ImageViewBase<ViewT> const& view, Vector3 const& vec) {
  return UnaryPerPixelView<ViewT, DotProdFunc>(view.impl(), DotProdFunc(vec));
}

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

template <class PixelT>
void do_hillshade(po::variables_map const& vm) {

  cartography::GeoReference georef;
  cartography::read_georeference(georef, input_file_name);

  // Select the pixel scale.
  double u_scale, v_scale;
  if (scale == 0) {
    if (georef.is_projected()) {
      u_scale = georef.transform()(0,0);
      v_scale = georef.transform()(1,1);
    } else {
      double meters_per_degree = 2*M_PI*georef.datum().semi_major_axis()/360.0;
      u_scale = georef.transform()(0,0) * meters_per_degree;
      v_scale = georef.transform()(1,1) * meters_per_degree;
    }
  } else {
    u_scale = scale;
    v_scale = -scale;
  }
  // For debugging:
  //  std::cout << "\t--> Scale: " << u_scale << "  " << v_scale << "\n";

  // Set the direction of the light source.
  Vector3 light_0(1,0,0);
  Vector3 light = vw::math::euler_to_rotation_matrix(elevation*M_PI/180, azimuth*M_PI/180, 0, "yzx") * light_0;

  // Compute the surface normals
  std::cout << "Loading: " << input_file_name << ".\n";
  DiskImageView<PixelT> disk_dem_file(input_file_name);
  ImageViewRef<PixelGray<double> > input_image = channel_cast<double>(disk_dem_file);

  ImageViewRef<PixelMask<PixelGray<double> > > dem;
  SrcImageResource *disk_dem_rsrc = DiskImageResource::open(input_file_name);
  if (vm.count("nodata-value")) {
    std::cout << "\t--> Masking pixel value: " << nodata_value << ".\n";
    dem = create_mask(input_image, nodata_value);
  } else if ( disk_dem_rsrc->has_nodata_read() ) {
    nodata_value = disk_dem_rsrc->nodata_read();
    std::cout << "\t--> Extracted nodata value from file: " << nodata_value << ".\n";
    dem = create_mask(input_image, nodata_value);
  } else {
    dem = pixel_cast<PixelMask<PixelGray<double> > >(input_image);
  }
  delete disk_dem_rsrc;

  if (vm.count("blur")) {
    std::cout << "\t--> Blurring pixel with gaussian kernal.  Sigma = " << blur_sigma << "\n";
    dem = gaussian_filter(dem, blur_sigma);
  }

  // The final result is the dot product of the light source with the normals
  ImageViewRef<PixelMask<PixelGray<uint8> > > shaded_image = channel_cast_rescale<uint8>(clamp(dot_prod(compute_normals(dem, u_scale, v_scale), light)));

  // Save the result
  std::cout << "Writing shaded relief image: " << output_file_name << "\n";

  DiskImageResourceGDAL rsrc(output_file_name, shaded_image.format());
  rsrc.set_block_write_size(Vector2i(1024,1024));
  write_georeference(rsrc, georef);
  write_image(rsrc, shaded_image,
              TerminalProgressCallback( "tools.hillshade", "Writing:"));
}

int main( int argc, char *argv[] ) {

  po::options_description desc("Description: Outputs image of a DEM lighted as specified\n\nUsage: hillshade [options] <input file> \n\nOptions");
  desc.add_options()
    ("help,h", "Display this help message")
    ("input-file", po::value<std::string>(&input_file_name), "Explicitly specify the input file")
    ("output-file,o", po::value<std::string>(&output_file_name), "Specify the output file")
    ("azimuth,a", po::value<double>(&azimuth)->default_value(0), "Sets the direction tha the light source is coming from (in degrees).  Zero degrees is to the right, with positive degree counter-clockwise.")
    ("elevation,e", po::value<double>(&elevation)->default_value(45), "Set the elevation of the light source (in degrees).")
    ("scale,s", po::value<double>(&scale)->default_value(0), "Set the scale of a pixel (in the same units as the DTM height values.")
    ("nodata-value", po::value<double>(&nodata_value), "Remap the DEM default value to the min altitude value.")
    ("blur", po::value<double>(&blur_sigma), "Pre-blur the DEM with the specified sigma.");
  po::positional_options_description p;
  p.add("input-file", 1);

  po::variables_map vm;
  try {
    po::store( po::command_line_parser( argc, argv ).options(desc).positional(p).run(), vm );
    po::notify( vm );
  } catch (po::error &e) {
    std::cout << "An error occured while parsing command line arguments.\n";
    std::cout << "\t" << e.what() << "\n\n";
    std::cout << desc << std::endl;
    return 1;
  }

  if( vm.count("help") ) {
    std::cout << desc << std::endl;
    return 1;
  }

  if( vm.count("input-file") != 1 ) {
    std::cout << "Error: Must specify exactly one input file!\n" << std::endl;
    std::cout << desc << std::endl;
    return 1;
  }

  if( output_file_name == "" ) {
    output_file_name = fs::path(input_file_name).replace_extension().string() + "_HILLSHADE.tif";
  }

  try {
    // Get the right pixel/channel type.
    ImageFormat fmt = tools::taste_image(input_file_name);

    switch(fmt.pixel_format) {
    case VW_PIXEL_GRAY:
    case VW_PIXEL_GRAYA:
    case VW_PIXEL_RGB:
    case VW_PIXEL_RGBA:
      switch(fmt.channel_type) {
      case VW_CHANNEL_UINT8:  do_hillshade<PixelGray<uint8>   >(vm); break;
      case VW_CHANNEL_INT16:  do_hillshade<PixelGray<int16>   >(vm); break;
      case VW_CHANNEL_UINT16: do_hillshade<PixelGray<uint16>  >(vm); break;
      case VW_CHANNEL_FLOAT64:do_hillshade<PixelGray<float64> >(vm); break;
      default:                do_hillshade<PixelGray<float32> >(vm); break;
      }
      break;
    default:
      std::cout << "Error: Unsupported pixel format.\n";
      exit(0);
    }
  } catch( Exception& e ) {
    std::cout << "Error: " << e.what() << std::endl;
  }

  return 0;
}