////////////////////////////////////////////////////////////////////////////////
// Materials.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//      Parametrized materials that can be used with MaterialField for purposes
//      of material optimization. Each material provides getETensorDerivative,
//      which gives the derivative of the elasticity tensor with respect to one
//      material parameter.
//
//      The exception is ConstantMaterial, which is intended to be read from a
//      file and which doesn't support material optimization.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Company:  New York University
//  Created:  07/11/2014 15:48:34
////////////////////////////////////////////////////////////////////////////////
#ifndef MATERIAL_HH
#define MATERIAL_HH

#include <MeshFEM/Types.hh>
#include <MeshFEM/Flattening.hh>
#include <MeshFEM/ElasticityTensor.hh>
#include <nlohmann/json.hpp>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>

#define BOOST_PARSER 1
#if BOOST_PARSER
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#endif

#include <MeshFEM_export.h>

namespace Materials {

// Material parameter bounds
struct Bounds {
    struct Bound {
        Bound(size_t _var, Real _val) : var(_var), value(_val) { }
        size_t var; Real value;
    };

    Bounds() { }
    Bounds(const std::vector<Bound> &l, const std::vector<Bound> &u)
        : m_lower(l), m_upper(u) { }

    void MESHFEM_EXPORT setFromJson(const nlohmann::json &entry);

    const std::vector<Bound> &lower() const { return m_lower; }
    const std::vector<Bound> &upper() const { return m_upper; }

    void setLower(const std::vector<Bound> &l) { m_lower = l; }
    void setUpper(const std::vector<Bound> &u) { m_upper = u; }
private:
    std::vector<Bound> m_lower, m_upper;
};

// Base class for variable materials
template<size_t _N, template<size_t> class _Mat, size_t _NVars>
struct VariableMaterial {
    typedef ElasticityTensor<Real, _N> ETensor;
    static constexpr size_t numVars = _NVars;

    virtual void getETensorDerivative(size_t p, ETensor &d) const = 0;
    virtual void getTensor(ETensor &tensor) const = 0;

    static const std::vector<Bounds::Bound> &upperBounds() { return _Mat<_N>::g_bounds.upper(); }
    static const std::vector<Bounds::Bound> &lowerBounds() { return _Mat<_N>::g_bounds.lower(); }

    static void setUpperBounds(const std::vector<Bounds::Bound> &u) { _Mat<_N>::g_bounds.setUpper(u); }
    static void setLowerBounds(const std::vector<Bounds::Bound> &l) { _Mat<_N>::g_bounds.setLower(l); }

    static void setBoundsFromFile(const std::string &path) {
        std::ifstream is(path);
        if (!is.is_open()) {
            throw std::runtime_error("Couldn't open bounds " + path);
        }
        nlohmann::json config;
        is >> config;
        setBoundsFromJson(config);
    }

    static void setBoundsFromJson(const nlohmann::json &config) {
        _Mat<_N>::g_bounds.setFromJson(config);
        // Validate bounds.
        std::runtime_error indexError("Bounds variable index out-of-bounds.");
        for (const auto &b : _Mat<_N>::g_bounds.lower()) if (b.var >= numVars) throw indexError;
        for (const auto &b : _Mat<_N>::g_bounds.upper()) if (b.var >= numVars) throw indexError;
    }

    virtual ~VariableMaterial() { }

    Real vars[numVars];
};

// Var 0: Young's modulus, var 1: Poisson ratio
template<size_t _N>
struct Isotropic : public VariableMaterial<_N, Isotropic, 2> {
    static constexpr size_t N = _N;
    typedef ElasticityTensor<Real, _N> ETensor;
    typedef Eigen::Matrix<Real, flatLen(_N), 1> FlattenedSymmetricMatrix;
    typedef VariableMaterial<N, Materials::Isotropic, 2> Base;
    using Base::vars;

    // WARNING: bounds are shared by all isotropic materials! (static)
    struct IsotropicBounds : Bounds {
        IsotropicBounds() {
            // Default Bounds
            // Upper: Upper bounds should be based on base material's moduli.
            //        Poisson ratio can't be greater than or equal 0.5
            //        (at 0.5, 3D lambda becomes Inf)
            // Lower: Young's modulus must be positive and is hard to make
            //        small--this minimum should be set based on homogenization results
            //        Poisson ratio can't be less than -1, and for robustness we
            //        limit it to -0.75
            Bounds::setUpper({ Bounds::Bound(0, 292), Bounds::Bound(1, 0.6) });
            Bounds::setLower({ Bounds::Bound(0, 25),  Bounds::Bound(1, 0.1) });
        }
    };

    Isotropic() {
        // Default Parameters: midway between bounds (if they exist)
        const auto &lb = this->lowerBounds();
        const auto &ub = this->upperBounds();
        if ((lb.size() == 2) && (ub.size() == 2)) {
            for (auto &bd : lb) vars[bd.var]  = 0.5 * bd.value;
            for (auto &bd : ub) vars[bd.var] += 0.5 * bd.value;
        }
        else {
            vars[0] = 50.0;
            vars[1] = 0.3;
        }
    }

    static const std::string &variableName(size_t i) {
        static const std::vector<std::string> names = { "E", "nu" };
        return names.at(i);
    }

    // Used for adjoint method gradient-based optimization
    void getETensorDerivative(size_t p, ETensor &d) const {
        assert(p == 0 || p == 1);
        d.clear();
        Real E = vars[0], nu = vars[1];
        Real dL, dmu;
        if (_N == 2) {
            // 2D Lambda = (nu * E) / (1.0 - nu * nu);
            //    mu = E / (2.0 + 2.0 * nu);
            dL = (p == 0) ? nu / (1 - nu * nu)
                          : E * (1 + nu * nu) / ((1 - nu * nu) * (1 - nu * nu));
        }
        if (_N == 3) {
            // 3D Lambda = (nu * E) / ((1.0 + nu) * (1.0 - 2.0 * nu));
            Real denSqrt = 1 - nu - 2 * nu * nu;
            dL = (p == 0) ? nu / ((1.0 + nu) * (1.0 - 2 * nu)) : E * (1 + 2 * nu * nu) / (denSqrt * denSqrt);
        }

        // 2D and 3D mu: E / (2 (1 + nu))
        dmu = (p == 0) ? 1 / (2 * (1 + nu))
                       : -E / (2 * (1 + nu) * (1 + nu));
        for (size_t i = 0; i < flatLen(_N); ++i) {
            for (size_t j = i; j < _N; ++j)
                d.D(i, j) = dL;
            d.D(i, i) += (i < _N) ? 2 * dmu : dmu;
        }
    }

    void getTensor(ETensor &tensor) const {
        tensor.setIsotropic(vars[0], vars[1]);
    }

    // Ceres-compatible cost function to fit Young's modulus, Y, and Poisson
    // ratio, nu, to best achieve:
    //      s ~= E(Y, nu) : e
    // Where Y and nu are Young's modulus and Poisson ratio. In 2D, the
    // condition s = E(Y, nu) : e can be written in a linear form:
    //  [s_00]   [e_00,  s_11][Y ]
    //  [s_11] = [e_11,  s_00][nu]
    //  [s_01]   [e_01, -s_01]
    // We solve this in a least squares sense to get ``optimal'' Y and nu. This
    // is a slightly strange formulation in which the residual is harder to
    // interpret, but it's nice because the optimization variables are Y and nu
    // directly.
    //
    // Other options are to use Lame coefficients, which appear linearly in the
    // stress-strain relationship, or variables 1/Y and nu/Y (which also appear
    // linearly), but then E and nu are nonlinear functions of the variables
    // (and bounds/penalties/etc. on them must be transformed accordingly).
    // In 3D the corresponding condition is:
    //  [s_00]   [e_00,  s_11 + s_22]
    //  [s_11]   [e_11,  s_00 + s_22]
    //  [s_22] = [e_22,  s_00 + s_11][Y ]
    //  [s_12]   [e_12,        -s_12][nu]
    //  [s_02]   [e_02,        -s_02]
    //  [s_01]   [e_01,        -s_01]
    template<class SMatrix>
    struct StressStrainFitCostFunction {
        StressStrainFitCostFunction(const SMatrix &e, const SMatrix &s, Real vol)
            : strain(e), stress(s) {
            if (vol <= 0) throw std::runtime_error("Volume must be positive");
            volSqrt = sqrt(vol);
        }

        template<typename T>
        bool operator()(const T *x, T *e) const {
            // Nonlinear version
            if (_N == 3) {
                e[0] = T(stress[0]) - x[1] * T(stress[1] + stress[2]);
                e[1] = T(stress[1]) - x[1] * T(stress[0] + stress[2]);
                e[2] = T(stress[2]) - x[1] * T(stress[0] + stress[1]);
                e[3] = (T(1) + x[1]) * T(stress[3]);
                e[4] = (T(1) + x[1]) * T(stress[4]);
                e[5] = (T(1) + x[1]) * T(stress[5]);
            }
            else {
                e[0] = T(stress[0]) - x[1] * T(stress[1]);
                e[1] = T(stress[1]) - x[1] * T(stress[0]);
                e[2] = (T(1) + x[1]) * T(stress[2]);
            }
            for (size_t i = 0; i < flatLen(_N); ++i) {
                e[i] /= x[0];
                e[i] -= T(strain[i]);
                if (i >= _N) e[i] *= T(sqrt(2.0));
                e[i] *= T(volSqrt);
            }

            // // Linear version
            // if (_N == 3) {
            //     e[0] = T(strain[0]) * x[0] + T(stress[1] + stress[2]) * x[1];
            //     e[1] = T(strain[1]) * x[0] + T(stress[0] + stress[2]) * x[1];
            //     e[2] = T(strain[2]) * x[0] + T(stress[0] + stress[1]) * x[1];
            //     e[3] = T(strain[3]) * x[0] - T(stress[3]) * x[1];
            //     e[4] = T(strain[4]) * x[0] - T(stress[4]) * x[1];
            //     e[5] = T(strain[5]) * x[0] - T(stress[5]) * x[1];
            // }
            // else {
            //     e[0] = T(strain[0]) * x[0] + T(stress[1]) * x[1];
            //     e[1] = T(strain[1]) * x[0] + T(stress[0]) * x[1];
            //     e[2] = T(strain[2]) * x[0] - T(stress[2]) * x[1];
            // }
            // for (size_t i = 0; i < flatLen(_N); ++i) {
            //     e[i] -= T(stress[i]);
            // }

            return true;
        }

        SMatrix strain, stress;
        Real volSqrt;
    };
private:
    friend Base;
    static IsotropicBounds g_bounds;
};

// Static variable needs to be explicitly defined...
template<size_t _N>
typename Isotropic<_N>::IsotropicBounds Isotropic<_N>::g_bounds;

// Axis-aligned orthotropic material.
// 2D: 4 variables
// Vars 0..1: Young's moduli,
// Var     2: Poisson ratio (YX)
// Var     3: Shear modulus
// 3D: 9 variables
// Vars 0..2: Young's moduli,
// Vars 3..5: Poisson ratios (YX, ZX, ZY)
// Vars 6..8: Shear ratios   (YZ, ZX, XY)
size_t constexpr nOrthotropicVars(size_t n) { return (n == 3) ? 9 : 4; }
template<size_t _N>
struct Orthotropic : public VariableMaterial<_N, Orthotropic, nOrthotropicVars(_N)> {
    static constexpr size_t N = _N;
    typedef ElasticityTensor<Real, _N> ETensor;
    typedef Eigen::Matrix<Real, flatLen(_N), 1> FlattenedSymmetricMatrix;

    typedef VariableMaterial<N, Materials::Orthotropic, nOrthotropicVars(N)> Base;
    using Base::vars;

    // WARNING: bounds are shared by all orthotropic materials! (static)
    struct OrthotropicBounds : Bounds {
        OrthotropicBounds() {
            // Default Bounds
            // Upper: Upper bounds should be based on base material's moduli.
            //        Poisson ratios can't be greater than 0.5
            //        (at 0.5, 3D isotropic lambda becomes Inf, so we avoid it
            //        here too)
            // Lower: Young's and shear moduli must be positive and are hard to make
            //        small--this minimum should be set based on homogenization results
            //        Poisson ratios can't be less than -1, and for robustness we
            //        limit them to -0.75
            if (_N == 3) Base::setUpperBounds({ Bound(3, 0.45), Bound(4, 0.45), Bound(5, 0.45) });
            else         Base::setUpperBounds({ Bound(0,  384), Bound(1,  384), Bound(2, 0.45), Bound(3, 102) });
            if (_N == 3) {
                Base::setLowerBounds({ Bound(0,  0.01), Bound(1,  0.01), Bound(2,  0.01),
                                       Bound(3, -0.75), Bound(4, -0.75), Bound(5, -0.75),
                                       Bound(6,  0.01), Bound(7,  0.01), Bound(8,  0.01) });
            }
            else Base::setLowerBounds({ Bound(0,  18), Bound(1,  18),
                                        Bound(2, 0.0), Bound(3,  2) });
        }
    };

    Orthotropic() {
        // Default Parameters
        if (_N == 3) {
            vars[0] = vars[1] = vars[2] = 1.0;
            vars[3] = vars[4] = vars[5] = 0.3;
            vars[6] = vars[7] = vars[8] = 1 / (2.0 * (1 + 0.3));
        }
        else {
            vars[0] = vars[1] = 200.0;
            vars[2] = 0.3;
            vars[3] = 200.0 / (2.0 * (1 + 0.3));
        }
    }

    static const std::string &variableName(size_t i) {
        if (_N == 3) {
            static const std::vector<std::string> names3D = {
                "E_x", "E_y", "E_z",
                "nu_yx", "nu_zx", "nu_zy",
                "mu_yz", "mu_zx", "mu_xy" };
            return names3D.at(i);
        }
        else {
            static const std::vector<std::string> names2D = {
                "E_x", "E_y", "nu_yx", "mu" };
            return names2D.at(i);
        }
    }

    // Used for adjoint method gradient-based optimization
    void getETensorDerivative(size_t p, ETensor &d) const;

    void getTensor(ETensor &tensor) const {
        if (_N == 3) {
            tensor.setOrthotropic3D(vars[0], vars[1], vars[2],
                                    vars[3], vars[4], vars[5],
                                    vars[6], vars[7], vars[8]);
        }
        else {
            tensor.setOrthotropic2D(vars[0], vars[1], vars[2], vars[3]);
        }
    }

    // Ceres-compatible cost function to fit orthotropic material parameters to
    // best achieve:
    //      e ~= E^(-1)(Y_x, Y_y, ...) : s
    template<class SMatrix>
    struct StressStrainFitCostFunction {
        StressStrainFitCostFunction(const SMatrix &e, const SMatrix &s, Real vol)
            : strain(e), stress(s) {
            if (vol <= 0) throw std::runtime_error("Volume must be positive");
            volSqrt = sqrt(vol);
        }

        template<typename T>
        bool operator()(const T *x, T *e) const {
            if (_N == 3) {
                T D01 =  -x[3] / x[1], // -nu_yx / E_y
                  D02 =  -x[4] / x[2], // -nu_zx / E_z
                  D12 =  -x[5] / x[2]; // -nu_zy / E_z
                e[0] = T(stress[0]) / x[0] + T(stress[1]) *  D01 + T(stress[2]) *  D02;
                e[1] = T(stress[0]) *  D01 + T(stress[1]) / x[1] + T(stress[2]) *  D12;
                e[2] = T(stress[0]) *  D02 + T(stress[1]) *  D12 + T(stress[2]) / x[2];
                e[3] = T(0.5 * stress[3]) / x[6];
                e[4] = T(0.5 * stress[4]) / x[7];
                e[5] = T(0.5 * stress[5]) / x[8];
            }
            else {
                T D01 = -x[2] / x[1]; // -nu_yx / E_y
                e[0] = T(stress[0]) / x[0] + T(stress[1]) *  D01;
                e[1] = T(stress[0]) *  D01 + T(stress[1]) / x[1];
                e[2] = T(0.5 * stress[2]) / x[3];
            }

            for (size_t i = 0; i < flatLen(_N); ++i) {
                e[i] -= T(strain[i]);
                if (i >= _N) e[i] *= T(sqrt(2.0));
                e[i] *= T(volSqrt);
            }

            return true;
        }

        Real volSqrt;
        SMatrix strain, stress;
    };
private:
    friend Base;
    static OrthotropicBounds g_bounds;
};

// Static variable needs to be explicitly defined...
template<size_t _N>
typename Orthotropic<_N>::OrthotropicBounds Orthotropic<_N>::g_bounds;

template<size_t _N>
struct Constant {
    static constexpr size_t N = _N;
    static constexpr size_t numVars = 0;
    typedef ElasticityTensor<Real, _N> ETensor;

    Constant() { m_E.setIsotropic(1.0, 0.3); }
    Constant(const std::string &materialFile) { setFromFile(materialFile); }

    void setFromFile(const std::string &materialFile);
    void setFromJson(const nlohmann::json &config);
#if BOOST_PARSER
    void setFromPTree(const boost::property_tree::ptree &pt);
#endif

    // Used for adjoint method gradient-based optimization
    void getETensorDerivative(size_t /* p */, ETensor &/* d */) const {
        throw std::runtime_error("Constant material can't be optimized\n");
    }

    const ETensor &getTensor()      const { return m_E; }
    void getTensor(ETensor &tensor) const { tensor = m_E; }
    void setTensor(const ETensor &tensor) { m_E = tensor; }

    void setIsotropic(Real E, Real nu) { m_E.setIsotropic(E, nu); }

    nlohmann::json getJson() const;

    // "type": "anisotropic",
    // "dim": 3,
    // "material_matrix": [[C_00, C_01, C02, C03, C04, C05],
    //                     [C_10, C_11, C12, C13, C14, C15],
    //                     [C_20, C_21, C22, C23, C24, C25],
    //                     [C_30, C_31, C32, C33, C34, C35],
    //                     [C_40, C_41, C42, C43, C44, C45],
    //                     [C_50, C_51, C52, C53, C54, C55]]
    friend std::ostream &operator<<(std::ostream &os, const Constant &cmat) {
        os << cmat.getJson().dump(4);
        return os;
        // os << "{ \"type\": \"anisotropic\"," << std::endl;
        // os << "\"material_matrix\": [";
        // for (size_t i = 0; i < flatLen(N); ++i) {
        //     for (size_t j = 0; j < flatLen(N); ++j) {
        //         os << (j ? ", " : "[") << cmat.m_E.D(i, j);
        //     }
        //     os << (i == flatLen(N) - 1 ? "]]" : "],") << std::endl;
        // }
        // os << "}";
        // return os;
    }

private:
    ETensor m_E;
};

} // Materials

#endif /* end of include guard: MATERIAL_HH */


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

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

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

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

#include <boost/make_shared.hpp>

#include <ql/errors.hpp>
#include <qle/instruments/commodityforward.hpp>

#include <ored/portfolio/builders/commodityforward.hpp>
#include <ored/portfolio/commodityforward.hpp>
#include <ored/portfolio/enginefactory.hpp>

using namespace QuantLib;
using namespace std;

namespace ore {
namespace data {

CommodityForward::CommodityForward() : Trade("CommodityForward") {}

CommodityForward::CommodityForward(const Envelope& envelope, const string& position, const string& commodityName,
                                   const string& currency, Real quantity, const string& maturityDate, Real strike)
    : Trade("CommodityForward", envelope), position_(position), commodityName_(commodityName), currency_(currency),
      quantity_(quantity), maturityDate_(maturityDate), strike_(strike) {}

void CommodityForward::build(const boost::shared_ptr<EngineFactory>& engineFactory) {

    // Create the commodity forward instrument
    Currency currency = parseCurrency(currency_);
    Position::Type position = parsePositionType(position_);
    Date maturity = parseDate(maturityDate_);
    boost::shared_ptr<Instrument> commodityForward = boost::make_shared<QuantExt::CommodityForward>(
        commodityName_, currency, position, quantity_, maturity, strike_);

    // Pricing engine
    boost::shared_ptr<EngineBuilder> builder = engineFactory->builder(tradeType_);
    QL_REQUIRE(builder, "No builder found for " << tradeType_);
    boost::shared_ptr<CommodityForwardEngineBuilder> commodityForwardEngineBuilder =
        boost::dynamic_pointer_cast<CommodityForwardEngineBuilder>(builder);
    commodityForward->setPricingEngine(commodityForwardEngineBuilder->engine(commodityName_, currency));

    // set up other Trade details
    instrument_ = boost::make_shared<VanillaInstrument>(commodityForward);
    npvCurrency_ = currency_;
    maturity_ = maturity;

    // We really need today's spot to get the correct notional.
    // But rather than having it move around we use strike * quantity
    notional_ = strike_ * quantity_;
}

void CommodityForward::fromXML(XMLNode* node) {

    Trade::fromXML(node);
    XMLNode* commodityDataNode = XMLUtils::getChildNode(node, "CommodityForwardData");

    position_ = XMLUtils::getChildValue(commodityDataNode, "Position", true);
    commodityName_ = XMLUtils::getChildValue(commodityDataNode, "Name", true);
    currency_ = XMLUtils::getChildValue(commodityDataNode, "Currency", true);
    quantity_ = XMLUtils::getChildValueAsDouble(commodityDataNode, "Quantity", true);
    maturityDate_ = XMLUtils::getChildValue(commodityDataNode, "Maturity", true);
    strike_ = XMLUtils::getChildValueAsDouble(commodityDataNode, "Strike", true);
}

XMLNode* CommodityForward::toXML(XMLDocument& doc) {

    XMLNode* node = Trade::toXML(doc);
    XMLNode* commodityDataNode = doc.allocNode("CommodityForwardData");
    XMLUtils::appendNode(node, commodityDataNode);

    XMLUtils::addChild(doc, commodityDataNode, "Position", position_);
    XMLUtils::addChild(doc, commodityDataNode, "Name", commodityName_);
    XMLUtils::addChild(doc, commodityDataNode, "Currency", currency_);
    XMLUtils::addChild(doc, commodityDataNode, "Quantity", quantity_);
    XMLUtils::addChild(doc, commodityDataNode, "Maturity", maturityDate_);
    XMLUtils::addChild(doc, commodityDataNode, "Strike", strike_);

    return node;
}

} // namespace data
} // namespace ore


//
// Created by christoph on 10.02.19.
//

#include <boost/algorithm/string/predicate.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <Graphics/Shader/ShaderManager.hpp>
#include <Math/Geometry/MatrixUtil.hpp>
#include "ShadowTechnique.hpp"

void ShadowTechnique::setLightDirection(const glm::vec3 &lightDirection, const sgl::AABB3 &sceneBoundingBox)
{
    glm::vec3 cameraFront = lightDirection; // Gram-Schmidt
    glm::vec3 cameraUp = glm::vec3(0.0f, 1.0f, 0.0f); // Gram-Schmidt
    cameraUp = glm::normalize(cameraUp - cameraFront * glm::dot(cameraUp, cameraFront)); // Gram-Schmidt
    glm::mat4 boundingBoxRotationMatrix = glm::lookAt(glm::vec3(0.0f), cameraFront, cameraUp);

    glm::vec3 sceneCenter = sceneBoundingBox.getCenter();
    sgl::AABB3 lightFacingBoundingBox = sceneBoundingBox.transformed(boundingBoxRotationMatrix);
    float screenHalfWidth = std::max(lightFacingBoundingBox.getExtent().x, lightFacingBoundingBox.getExtent().y)*1.0f;
    //std::cout << "sceneCenter: (" << sceneCenter.x << ", " << sceneCenter.y << ", " << sceneCenter.z << ")" << std::endl;
    lightProjectionMatrix = glm::ortho(-screenHalfWidth, screenHalfWidth, -screenHalfWidth, screenHalfWidth,
            LIGHT_NEAR_CLIP_DISTANCE, LIGHT_FAR_CLIP_DISTANCE);
    lightViewMatrix = glm::lookAt(lightDirection+sceneCenter, sceneCenter, glm::vec3(0.0f, 1.0f, 0.0f))
            *sgl::matrixTranslation(-2.0f*lightDirection);
    lightSpaceMatrix = lightProjectionMatrix * lightViewMatrix;
    this->lightDirection = lightDirection;
}

void ShadowTechnique::newModelLoaded(const std::string &filename, bool modelContainsTrajectories)
{
    if (modelContainsTrajectories) {
        sgl::ShaderManager->addPreprocessorDefine("MODEL_WITH_VORTICITY", "");
    } else {
        sgl::ShaderManager->removePreprocessorDefine("MODEL_WITH_VORTICITY");
    }
    sgl::ShaderManager->invalidateShaderCache();
    loadShaders();
}

void NoShadowMapping::setShaderDefines()
{
    sgl::ShaderManager->removePreprocessorDefine("SHADOW_MAPPING_STANDARD");
    sgl::ShaderManager->removePreprocessorDefine("SHADOW_MAPPING_MOMENTS");
    sgl::ShaderManager->invalidateShaderCache();
}


#include "../include/franka_emulator/model.h"
#include <Eigen/Geometry>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#ifndef _GNU_SOURCE
    #define _GNU_SOURCE
#endif
#include <link.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>

FRANKA_EMULATOR::Frame FRANKA_EMULATOR::operator++(Frame& frame, int /* dummy */) noexcept
{
    Frame result = frame;
    frame = static_cast<Frame>(static_cast<unsigned int>(result) + 1);
    return result;
}

FRANKA_EMULATOR::Model::Model(FRANKA_EMULATOR::Network&)
{
    //Searching for urdf file
    struct LibrarySearchCallbackData
    {
        bool found              = false;
        std::string directory   = "";
    } library_search_callback_data;
    auto library_search_callback = [](struct dl_phdr_info *info, size_t size, void *data) -> int
    {
        const char *library_name = "libfranka_emulator_model.so";
        if (strstr(info->dlpi_name, library_name) != nullptr)
        {
            LibrarySearchCallbackData *library_search_callback_data = (LibrarySearchCallbackData*)data;
            library_search_callback_data->found = true;
            library_search_callback_data->directory.assign(info->dlpi_name);
            library_search_callback_data->directory.erase(library_search_callback_data->directory.find(library_name), strlen(library_name));
        }
        return 0;
    };
    dl_iterate_phdr(library_search_callback, &library_search_callback_data);
    if (!library_search_callback_data.found) throw std::runtime_error("franka_emulator::Model::Model: Could not find library path");
    struct stat model_stat;
    if (stat((library_search_callback_data.directory + "model/franka_emulator.urdf").c_str(), &model_stat) == 0)
        pinocchio::urdf::buildModel(library_search_callback_data.directory + "model/franka_emulator.urdf", _model);
    if (stat((library_search_callback_data.directory + "../model/franka_emulator.urdf").c_str(), &model_stat) == 0)
        pinocchio::urdf::buildModel(library_search_callback_data.directory + "../model/franka_emulator.urdf", _model);
    else if (stat((library_search_callback_data.directory + "../share/franka_emulator/model/franka_emulator.urdf").c_str(), &model_stat) == 0)
        pinocchio::urdf::buildModel(library_search_callback_data.directory + "../share/franka_emulator/model/franka_emulator.urdf", _model);
    else throw std::runtime_error("franka_emulator::Model::Model: Could not find model file");

    //Other initialization
    _data = pinocchio::Data(_model);
    for (size_t joint = 0; joint < 8; joint++)
    {
        std::string name = "panda_joint" + std::to_string(joint + 1);
        if (!_model.existFrame(name)) throw std::runtime_error("Joint frame not found");
        _joint_frame_id[joint] = _model.getFrameId(name);
    }
    for (size_t link = 0; link < 8; link++)
    {
        std::string name = "panda_link" + std::to_string(link + 1);
        if (!_model.existFrame(name)) throw std::runtime_error("Link frame not found");
        _link_frame_id[link] = _model.getFrameId(name);
    }
}

FRANKA_EMULATOR::Model::Model(Model &&other) noexcept
{
    _model = other._model;
    _data = other._data;
    for (size_t i = 0; i < 8; i++) _joint_frame_id[i] = other._joint_frame_id[i];
    for (size_t i = 0; i < 8; i++) _link_frame_id[i] = other._link_frame_id[i];
}

FRANKA_EMULATOR::Model& FRANKA_EMULATOR::Model::operator=(Model&&) noexcept
{
    return *this;
}

FRANKA_EMULATOR::Model::~Model() noexcept
{}

std::array<double, 16> FRANKA_EMULATOR::Model::pose(Frame frame, const FRANKA_EMULATOR::RobotState& robot_state) const
{
    return pose(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
}

std::array<double, 16> FRANKA_EMULATOR::Model::pose(
    Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,
    const std::array<double, 16>& EE_T_K) const
{

    pinocchio::forwardKinematics(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]));
    pinocchio::SE3 placement;
    if (static_cast<size_t>(frame) >= static_cast<size_t>(Frame::kJoint1) && static_cast<size_t>(frame) <= static_cast<size_t>(Frame::kJoint7))
    {
        placement = _data.oMi[static_cast<size_t>(frame) + 1];
    }
    else if (frame == Frame::kFlange || frame == Frame::kEndEffector || frame == Frame::kStiffness)
    {
        placement = pinocchio::updateFramePlacement(_model, _data, _joint_frame_id[7]);
        if (frame == Frame::kEndEffector || frame == Frame::kStiffness) placement = placement.act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&F_T_EE[0])));
        if (frame == Frame::kStiffness) placement = placement.act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&EE_T_K[0])));
    }
    else throw std::runtime_error("franka_emulator::Model::pose: Invalid frame");

    Eigen::Affine3d transform;
    transform.linear() = placement.rotation_impl();
    transform.translation() = placement.translation_impl();
    std::array<double, 16> result;
    Eigen::Matrix4d::Map(&result[0]) = transform.matrix();
    return result;
}

std::array<double, 42> FRANKA_EMULATOR::Model::bodyJacobian(Frame frame, const FRANKA_EMULATOR::RobotState& robot_state) const
{
    return bodyJacobian(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
}

std::array<double, 42> FRANKA_EMULATOR::Model::bodyJacobian(
    Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,
    const std::array<double, 16>& EE_T_K) const
{
    Eigen::Matrix<double, 6, 7> result_matrix = Eigen::Matrix<double, 6, 7>::Zero();
    if (static_cast<size_t>(frame) >= static_cast<size_t>(Frame::kJoint1) && static_cast<size_t>(frame) <= static_cast<size_t>(Frame::kJoint7))
    {
        pinocchio::computeJointJacobian(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]), static_cast<size_t>(frame) + 1, result_matrix);
    }
    else if (frame == Frame::kFlange)
    {
        pinocchio::computeFrameJacobian(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]), _joint_frame_id[7], result_matrix);
    }
    else if (frame == Frame::kEndEffector || frame == Frame::kStiffness)
    {
        pinocchio::SE3 placement = pinocchio::updateFramePlacement(_model, _data, _joint_frame_id[7]).act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&F_T_EE[0])));
        if (frame == Frame::kStiffness) placement = placement.act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&EE_T_K[0])));
        for (size_t i = 0; i < 7; i++)
        {
            result_matrix.block<3, 1>(0, i) = placement.rotation_impl().transpose() * (_data.oMi[i+1].rotation_impl().col(2).cross(placement.translation_impl() - _data.oMi[1+i].translation_impl()));
            result_matrix.block<3, 1>(3, i) = placement.rotation_impl().transpose() * _data.oMi[i+1].rotation_impl().col(2);
        }
    }
    else throw std::runtime_error("franka_emulator::Model::bodyJacobian: Invalid frame");

    std::array<double, 42> result;
    Eigen::Matrix<double, 6, 7>::Map(&result[0]) = result_matrix;
    return result;
}

std::array<double, 42> FRANKA_EMULATOR::Model::zeroJacobian(Frame frame, const FRANKA_EMULATOR::RobotState& robot_state) const
{
    return zeroJacobian(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
}

std::array<double, 42> FRANKA_EMULATOR::Model::zeroJacobian(
    Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,
    const std::array<double, 16>& EE_T_K) const
{
    Eigen::Matrix<double, 6, 7> result_matrix = Eigen::Matrix<double, 6, 7>::Zero();
    if (static_cast<size_t>(frame) >= static_cast<size_t>(Frame::kJoint1) && static_cast<size_t>(frame) <= static_cast<size_t>(Frame::kJoint7))
    {
        pinocchio::forwardKinematics(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]));
        pinocchio::computeJointJacobian(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]), static_cast<size_t>(frame) + 1, result_matrix);
        result_matrix.block<3, 7>(0,0) = _data.oMi[static_cast<size_t>(frame) + 1].rotation_impl() * result_matrix.block<3, 7>(0,0);
        result_matrix.block<3, 7>(3,0) = _data.oMi[static_cast<size_t>(frame) + 1].rotation_impl() * result_matrix.block<3, 7>(3,0);
    }
    else if (frame == Frame::kFlange)
    {
        pinocchio::SE3 placement = pinocchio::updateFramePlacement(_model, _data, _joint_frame_id[7]);
        pinocchio::computeFrameJacobian(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]), _joint_frame_id[7], result_matrix);
        result_matrix.block<3, 7>(0,0) = placement.rotation_impl() * result_matrix.block<3, 7>(0,0);
        result_matrix.block<3, 7>(3,0) = placement.rotation_impl() * result_matrix.block<3, 7>(3,0);
    }
    else if (frame == Frame::kEndEffector || frame == Frame::kStiffness)
    {
        pinocchio::SE3 placement = pinocchio::updateFramePlacement(_model, _data, _joint_frame_id[7]).act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&F_T_EE[0])));
        if (frame == Frame::kStiffness) placement = placement.act_impl(pinocchio::SE3(Eigen::Matrix4d::Map(&EE_T_K[0])));
        for (size_t i = 0; i < 7; i++)
        {
            result_matrix.block<3, 1>(0, i) = _data.oMi[i+1].rotation_impl().col(2).cross(placement.translation_impl() - _data.oMi[i+1].translation_impl());
            result_matrix.block<3, 1>(3, i) = _data.oMi[i+1].rotation_impl().col(2);
        }
    }
    else throw std::runtime_error("franka_emulator::Model::zeroJacobian: Invalid frame");

    std::array<double, 42> result;
    Eigen::Matrix<double, 6, 7>::Map(&result[0]) = result_matrix;
    return result;
}

std::array<double, 49> FRANKA_EMULATOR::Model::mass(const FRANKA_EMULATOR::RobotState& robot_state) const noexcept
{
    return mass(robot_state.q, robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
}

std::array<double, 49> FRANKA_EMULATOR::Model::mass(
    const std::array<double, 7>& q,
    const std::array<double, 9>& I_total,
    double m_total,
    const std::array<double, 3>& F_x_Ctotal) const noexcept
{
    pinocchio::Inertia initial_end_inertia = _model.inertias[7];
    _model.inertias[7].mass() += m_total;
    _model.inertias[7].inertia().data()[0] += I_total[0];                    //xx
    _model.inertias[7].inertia().data()[1] += (I_total[1] + I_total[3]) / 2; //xy
    _model.inertias[7].inertia().data()[2] += I_total[4];                    //yy
    _model.inertias[7].inertia().data()[3] += (I_total[2] + I_total[6]) / 2; //xz
    _model.inertias[7].inertia().data()[4] += (I_total[5] + I_total[7]) / 2; //yz
    _model.inertias[7].inertia().data()[5] += I_total[8];                    //zz
    Eigen::Matrix<double, 7, 7> result_matrix = pinocchio::crba(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]));
    _model.inertias[7] = initial_end_inertia;
    result_matrix.triangularView<Eigen::StrictlyLower>() = result_matrix.transpose().triangularView<Eigen::StrictlyLower>();
    std::array<double, 49> result;
    Eigen::Matrix<double, 7, 7>::Map(&result[0]) = result_matrix;
    return result;
}

std::array<double, 7> FRANKA_EMULATOR::Model::coriolis(const FRANKA_EMULATOR::RobotState& robot_state) const noexcept
{
    return coriolis(robot_state.q, robot_state.dq, robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
}

std::array<double, 7> FRANKA_EMULATOR::Model::coriolis(
    const std::array<double, 7>& q,
    const std::array<double, 7>& dq,
    const std::array<double, 9>& I_total,
    double m_total,
    const std::array<double, 3>& F_x_Ctotal) const noexcept
{
    pinocchio::Inertia initial_end_inertia = _model.inertias[7];
    _model.inertias[7].mass() += m_total;
    _model.inertias[7].inertia().data()[0] += I_total[0];                    //xx
    _model.inertias[7].inertia().data()[1] += (I_total[1] + I_total[3]) / 2; //xy
    _model.inertias[7].inertia().data()[2] += I_total[4];                    //yy
    _model.inertias[7].inertia().data()[3] += (I_total[2] + I_total[6]) / 2; //xz
    _model.inertias[7].inertia().data()[4] += (I_total[5] + I_total[7]) / 2; //yz
    _model.inertias[7].inertia().data()[5] += I_total[8];                    //zz
    Eigen::Matrix<double, 7, 7> result_matrix = pinocchio::computeCoriolisMatrix(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]), Eigen::Matrix<double, 7, 1>::Map(&dq[0]));
    _model.inertias[7] = initial_end_inertia;
    std::array<double, 7> result;
    Eigen::Matrix<double, 7, 1>::Map(&result[0]) = result_matrix * Eigen::Matrix<double, 7, 1>::Map(&dq[0]);
    return result;
}

std::array<double, 7> FRANKA_EMULATOR::Model::gravity(
    const std::array<double, 7>& q,
    double m_total,
    const std::array<double, 3>& F_x_Ctotal,
    const std::array<double, 3>& gravity_earth) const noexcept
{
    _model.gravity.linear_impl() = Eigen::Matrix<double, 3, 1>::Map(&gravity_earth[0]);
    double initial_end_mass = _model.inertias[7].mass();
    _model.inertias[7].mass() += m_total;
    std::array<double, 7> result;
    Eigen::Matrix<double, 7, 1>::Map(&result[0]) = pinocchio::computeGeneralizedGravity(_model, _data, Eigen::Matrix<double, 7, 1>::Map(&q[0]));
    _model.inertias[7].mass() = initial_end_mass;
    return result;
}

std::array<double, 7> FRANKA_EMULATOR::Model::gravity(
    const FRANKA_EMULATOR::RobotState& robot_state,
    const std::array<double, 3>& gravity_earth) const noexcept
{
    return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, gravity_earth);
}