/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  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 Autonomous Systems Lab / ETH Zurich 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.
 *
 *  Created on: Feb 3, 2015
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file implementation/EquidistantDistortion.hpp
 * @brief Header implementation file for the EquidistantDistortion class.
 * @author Stefan Leutenegger
 */

#include <Eigen/LU>
#include <iostream>

/// \brief okvis Main namespace of this package.
namespace vio {
/// \brief cameras Namespace for camera-related functionality.
namespace cameras {
    int FTR_UNDIST_DL_MAX_ITERATIONS              = 10;
    float FTR_UNDIST_DL_RADIUS_INITIAL              = 1.0f;      // 1.0^2
    float FTR_UNDIST_DL_RADIUS_MIN                  = 1.0e-10f;  // 0.00001^2
    float FTR_UNDIST_DL_RADIUS_MAX                  = 1.0e4f;    // 100.0^2
    float FTR_UNDIST_DL_RADIUS_FACTOR_INCREASE      = 9.0f;      // 3.0^2
    float FTR_UNDIST_DL_RADIUS_FACTOR_DECREASE      = 0.25f;     // 0.5^2
    float FTR_UNDIST_DL_GAIN_RATIO_MIN              = 0.25f;
    float FTR_UNDIST_DL_GAIN_RATIO_MAX              = 0.75f;
// The default constructor with all zero ki
EquidistantDistortion::EquidistantDistortion()
        : k1_(0.0),
          k2_(0.0),
          k3_(0.0),
          k4_(0.0)
{
    parameters_.setZero();
}

// Constructor initialising ki
EquidistantDistortion::EquidistantDistortion(float k1, float k2, float k3,
                                             float k4)
{
    parameters_[0] = k1;
    parameters_[1] = k2;
    parameters_[2] = k3;
    parameters_[3] = k4;
    k1_ = k1;
    k2_ = k2;
    k3_ = k3;
    k4_ = k4;
}

bool EquidistantDistortion::setParameters(const Eigen::VectorXd & parameters)
{
    if (parameters.cols() != NumDistortionIntrinsics) {
        return false;
    }
    parameters_ = parameters.cast<float>();;
    k1_ = parameters[0];
    k2_ = parameters[1];
    k3_ = parameters[2];
    k4_ = parameters[3];
    return true;
}

bool EquidistantDistortion::distort(const Eigen::Vector2d & pointUndistorted,
                                    Eigen::Vector2d * pointDistorted) const
{
    // distortion only:
    const float u0 = pointUndistorted[0];
    const float u1 = pointUndistorted[1];
    const float r = sqrt(u0 * u0 + u1 * u1);
    const float theta = atan(r);
    const float theta2 = theta * theta;
    const float theta4 = theta2 * theta2;
    const float theta6 = theta4 * theta2;
    const float theta8 = theta4 * theta4;
    const float thetad = theta
                          * (1 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8);

    const float scaling = (r > 1e-8) ? thetad / r : 1.0;
    (*pointDistorted)[0] = scaling * u0;
    (*pointDistorted)[1] = scaling * u1;
    return true;
}

bool EquidistantDistortion::distort(const Eigen::Vector2f & pointUndistorted,
                                    Eigen::Vector2f * pointDistorted) const
{
    // distortion only:
    const float u0 = pointUndistorted[0];
    const float u1 = pointUndistorted[1];
    const float r = sqrt(u0 * u0 + u1 * u1);
    const float theta = atan(r);
    const float theta2 = theta * theta;
    const float theta4 = theta2 * theta2;
    const float theta6 = theta4 * theta2;
    const float theta8 = theta4 * theta4;
    const float thetad = theta
                         * (1 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8);

    const float scaling = (r > 1e-8) ? thetad / r : 1.0;
    (*pointDistorted)[0] = scaling * u0;
    (*pointDistorted)[1] = scaling * u1;
    return true;
}

bool EquidistantDistortion::distort(const Eigen::Vector2d & pointUndistorted,
                                    Eigen::Vector2d * pointDistorted,
                                    Eigen::Matrix2d * pointJacobian,
                                    Eigen::Matrix2Xd * parameterJacobian) const
{
    // distortion first:
    const float u0 = pointUndistorted[0];
    const float u1 = pointUndistorted[1];
    const float r = sqrt(u0 * u0 + u1 * u1);
    const float theta = atan(r);
    const float theta2 = theta * theta;
    const float theta4 = theta2 * theta2;
    const float theta6 = theta4 * theta2;
    const float theta8 = theta4 * theta4;
    const float thetad = theta
                          * (1 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8);

    const float scaling = (r > 1e-8) ? thetad / r : 1.0;
    (*pointDistorted)[0] = scaling * u0;
    (*pointDistorted)[1] = scaling * u1;

    Eigen::Matrix2d & J = *pointJacobian;
    if (r > 1e-8) {
        // mostly matlab generated...
        float t2;
        float t3;
        float t4;
        float t6;
        float t7;
        float t8;
        float t9;
        float t11;
        float t17;
        float t18;
        float t19;
        float t20;
        float t25;

        t2 = u0 * u0;
        t3 = u1 * u1;
        t4 = t2 + t3;
        t6 = atan(sqrt(t4));
        t7 = t6 * t6;
        t8 = 1.0 / sqrt(t4);
        t9 = t7 * t7;
        t11 = 1.0 / ((t2 + t3) + 1.0);
        t17 = (((k1_ * t7 + k2_ * t9) + k3_ * t7 * t9) + k4_ * (t9 * t9)) + 1.0;
        t18 = 1.0 / t4;
        t19 = 1.0 / sqrt(t4 * t4 * t4);
        t20 = t6 * t8 * t17;
        t25 = ((k2_ * t6 * t7 * t8 * t11 * u1 * 4.0
                + k3_ * t6 * t8 * t9 * t11 * u1 * 6.0)
               + k4_ * t6 * t7 * t8 * t9 * t11 * u1 * 8.0)
              + k1_ * t6 * t8 * t11 * u1 * 2.0;
        t4 = ((k2_ * t6 * t7 * t8 * t11 * u0 * 4.0
               + k3_ * t6 * t8 * t9 * t11 * u0 * 6.0)
              + k4_ * t6 * t7 * t8 * t9 * t11 * u0 * 8.0)
             + k1_ * t6 * t8 * t11 * u0 * 2.0;
        t7 = t11 * t17 * t18 * u0 * u1;
        J(0, 1) = (t7 + t6 * t8 * t25 * u0) - t6 * t17 * t19 * u0 * u1;
        J(1, 1) = ((t20 - t3 * t6 * t17 * t19) + t3 * t11 * t17 * t18)
                  + t6 * t8 * t25 * u1;
        J(0, 0) = ((t20 - t2 * t6 * t17 * t19) + t2 * t11 * t17 * t18)
                  + t6 * t8 * t4 * u0;
        J(1, 0) = (t7 + t6 * t8 * t4 * u1) - t6 * t17 * t19 * u0 * u1;

        if (parameterJacobian) {
            Eigen::Matrix2Xd & Ji = *parameterJacobian;
            Ji.resize(2,NumDistortionIntrinsics);
            // mostly matlab generated...
            float t6;
            float t2;
            float t3;
            float t8;
            float t10;

            t6 = u0 * u0 + u1 * u1;
            t2 = atan(sqrt(t6));
            t3 = t2 * t2;
            t8 = t3 * t3;
            t6 = 1.0 / sqrt(t6);
            t10 = t8 * t8;
            Ji(0, 0) = t2 * t3 * t6 * u0;
            Ji(1, 0) = t2 * t3 * t6 * u1;
            Ji(0, 1) = t2 * t8 * t6 * u0;
            Ji(1, 1) = t2 * t8 * t6 * u1;
            Ji(0, 2) = t2 * t3 * t8 * t6 * u0;
            Ji(1, 2) = t2 * t3 * t8 * t6 * u1;
            Ji(0, 3) = t2 * t6 * t10 * u0;
            Ji(1, 3) = t2 * t6 * t10 * u1;

        }
    } else {
        // handle limit case for [u0,u1]->0
        if (parameterJacobian) {
            parameterJacobian->resize(2,NumDistortionIntrinsics);
            parameterJacobian->setZero();
        }
        J.setIdentity();
    }

    return true;
}

bool EquidistantDistortion::distort(const Eigen::Vector2f & pointUndistorted,
                                    Eigen::Vector2f* pointDistorted,
                                    Eigen::Matrix2f * pointJacobian,
                                    Eigen::Matrix2Xf * parameterJacobian) const
{

    ////针对k1,k2,k2,k3的等距投影模型, r^2 = x^2 + y^2
    ////残差2*1 归一化坐标的2维： min F(x,y) = 0.5*r(x,y)^2 约束： x^2+y^2 <=
    ///      r.x = x*(theta*(1 + k1*theta^2 + k2*theta^4 + k3 *theta^6 + k4*theta^8)/r) - x0
    ////     r.x = y*(theta*(1 + k1*theta^2 + k2*theta^4 + k3 *theta^6 + k4*theta^8)/r) - y0
    // distortion first:
    const float u0 = pointUndistorted[0];//x
    const float u1 = pointUndistorted[1];//y
    const float r = sqrt(u0 * u0 + u1 * u1);//r = sqrt(x^2 + y^2)
    const float theta = atan(r);
    const float theta2 = theta * theta;
    const float theta4 = theta2 * theta2;
    const float theta6 = theta4 * theta2;
    const float theta8 = theta4 * theta4;
    const float thetad = theta
                         * (1 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8);

    const float scaling = (r > 1e-8) ? thetad / r : 1.0;
    (*pointDistorted)[0] = scaling * u0;
    (*pointDistorted)[1] = scaling * u1;

    Eigen::Matrix2f & J = *pointJacobian;
    if (r > 1e-8) {
        // mostly matlab generated...
        float t2;//x^2
        float t3;//y^2
        float t4;//r^2 = x^2+y^2
        float t6;//theta
        float t7;//theta^2
        float t8;// 1/r
        float t9;//theta^4
        float t11;// 1 / ((x^2+y^2) + 1.0);
        float t17;// (((k1_ * theta^2 + k2_ * theta^4) + k3_ * t7 * t9) + k4_ * (t9 * t9)) + 1.0
        float t18;
        float t19;
        float t20;
        float t25;

        t2 = u0 * u0;
        t3 = u1 * u1;
        t4 = t2 + t3;
        t6 = atan(sqrt(t4));
        t7 = t6 * t6;
        t8 = 1.0 / sqrt(t4);
        t9 = t7 * t7;
        t11 = 1.0 / ((t2 + t3) + 1.0);
        t17 = (((k1_ * t7 + k2_ * t9) + k3_ * t7 * t9) + k4_ * (t9 * t9)) + 1.0;
        t18 = 1.0 / t4;
        t19 = 1.0 / sqrt(t4 * t4 * t4);
        t20 = t6 * t8 * t17;
        t25 = ((k2_ * t6 * t7 * t8 * t11 * u1 * 4.0
                + k3_ * t6 * t8 * t9 * t11 * u1 * 6.0)
               + k4_ * t6 * t7 * t8 * t9 * t11 * u1 * 8.0)
              + k1_ * t6 * t8 * t11 * u1 * 2.0;
        t4 = ((k2_ * t6 * t7 * t8 * t11 * u0 * 4.0
               + k3_ * t6 * t8 * t9 * t11 * u0 * 6.0)
              + k4_ * t6 * t7 * t8 * t9 * t11 * u0 * 8.0)
             + k1_ * t6 * t8 * t11 * u0 * 2.0;
        t7 = t11 * t17 * t18 * u0 * u1;
        J(0, 1) = (t7 + t6 * t8 * t25 * u0) - t6 * t17 * t19 * u0 * u1;
        J(1, 1) = ((t20 - t3 * t6 * t17 * t19) + t3 * t11 * t17 * t18)
                  + t6 * t8 * t25 * u1;
        J(0, 0) = ((t20 - t2 * t6 * t17 * t19) + t2 * t11 * t17 * t18)
                  + t6 * t8 * t4 * u0;
        J(1, 0) = (t7 + t6 * t8 * t4 * u1) - t6 * t17 * t19 * u0 * u1;

        if (parameterJacobian) {
            Eigen::Matrix2Xf & Ji = *parameterJacobian;
            Ji.resize(2,NumDistortionIntrinsics);
            // mostly matlab generated...
            float t6;
            float t2;
            float t3;
            float t8;
            float t10;

            t6 = u0 * u0 + u1 * u1;
            t2 = atan(sqrt(t6));
            t3 = t2 * t2;
            t8 = t3 * t3;
            t6 = 1.0 / sqrt(t6);
            t10 = t8 * t8;
            Ji(0, 0) = t2 * t3 * t6 * u0;
            Ji(1, 0) = t2 * t3 * t6 * u1;
            Ji(0, 1) = t2 * t8 * t6 * u0;
            Ji(1, 1) = t2 * t8 * t6 * u1;
            Ji(0, 2) = t2 * t3 * t8 * t6 * u0;
            Ji(1, 2) = t2 * t3 * t8 * t6 * u1;
            Ji(0, 3) = t2 * t6 * t10 * u0;
            Ji(1, 3) = t2 * t6 * t10 * u1;

        }
    } else {
        // handle limit case for [u0,u1]->0
        if (parameterJacobian) {
            parameterJacobian->resize(2,NumDistortionIntrinsics);
            parameterJacobian->setZero();
        }
        J.setIdentity();
    }

    return true;
}

bool EquidistantDistortion::distortWithExternalParameters(
        const Eigen::Vector2d & pointUndistorted,
        const Eigen::VectorXd & parameters, Eigen::Vector2d * pointDistorted,
        Eigen::Matrix2d * pointJacobian, Eigen::Matrix2Xd * parameterJacobian) const
{
    // decompose parameters

    const float k1 = parameters[0];
    const float k2 = parameters[1];
    const float k3 = parameters[2];
    const float k4 = parameters[3];
    // distortion first:
    const float u0 = pointUndistorted[0];
    const float u1 = pointUndistorted[1];
    const float r = sqrt(u0 * u0 + u1 * u1);
    const float theta = atan(r);
    const float theta2 = theta * theta;
    const float theta4 = theta2 * theta2;
    const float theta6 = theta4 * theta2;
    const float theta8 = theta4 * theta4;
    const float thetad = theta
                          * (1 + k1 * theta2 + k2 * theta4 + k3 * theta6 + k4 * theta8);

    const float scaling = (r > 1e-8) ? thetad / r : 1.0;
    (*pointDistorted)[0] = scaling * u0;
    (*pointDistorted)[1] = scaling * u1;

    Eigen::Matrix2d & J = *pointJacobian;
    if (r > 1e-8) {
        // mostly matlab generated...
        float t2;
        float t3;
        float t4;
        float t6;
        float t7;
        float t8;
        float t9;
        float t11;
        float t17;
        float t18;
        float t19;
        float t20;
        float t25;

        t2 = u0 * u0;
        t3 = u1 * u1;
        t4 = t2 + t3;
        t6 = atan(sqrt(t4));
        t7 = t6 * t6;
        t8 = 1.0 / sqrt(t4);
        t9 = t7 * t7;
        t11 = 1.0 / ((t2 + t3) + 1.0);
        t17 = (((k1 * t7 + k2 * t9) + k3 * t7 * t9) + k4 * (t9 * t9)) + 1.0;
        t18 = 1.0 / t4;
        t19 = 1.0 / sqrt(t4 * t4 * t4);
        t20 = t6 * t8 * t17;
        t25 = ((k2 * t6 * t7 * t8 * t11 * u1 * 4.0
                + k3 * t6 * t8 * t9 * t11 * u1 * 6.0)
               + k4 * t6 * t7 * t8 * t9 * t11 * u1 * 8.0)
              + k1 * t6 * t8 * t11 * u1 * 2.0;
        t4 = ((k2 * t6 * t7 * t8 * t11 * u0 * 4.0
               + k3 * t6 * t8 * t9 * t11 * u0 * 6.0)
              + k4 * t6 * t7 * t8 * t9 * t11 * u0 * 8.0)
             + k1 * t6 * t8 * t11 * u0 * 2.0;
        t7 = t11 * t17 * t18 * u0 * u1;
        J(0, 0) = (t7 + t6 * t8 * t25 * u0) - t6 * t17 * t19 * u0 * u1;
        J(1, 0) = ((t20 - t3 * t6 * t17 * t19) + t3 * t11 * t17 * t18)
                  + t6 * t8 * t25 * u1;
        J(0, 1) = ((t20 - t2 * t6 * t17 * t19) + t2 * t11 * t17 * t18)
                  + t6 * t8 * t4 * u0;
        J(1, 1) = (t7 + t6 * t8 * t4 * u1) - t6 * t17 * t19 * u0 * u1;
        if (parameterJacobian) {
            Eigen::Matrix2Xd & Ji = *parameterJacobian;
            Ji.resize(2,NumDistortionIntrinsics);
            // mostly matlab generated...
            float t6;
            float t2;
            float t3;
            float t8;
            float t10;

            t6 = u0 * u0 + u1 * u1;
            t2 = atan(sqrt(t6));
            t3 = t2 * t2;
            t8 = t3 * t3;
            t6 = 1.0 / sqrt(t6);
            t10 = t8 * t8;
            Ji(0, 0) = t2 * t3 * t6 * u0;
            Ji(1, 0) = t2 * t3 * t6 * u1;
            Ji(0, 1) = t2 * t8 * t6 * u0;
            Ji(1, 1) = t2 * t8 * t6 * u1;
            Ji(0, 2) = t2 * t3 * t8 * t6 * u0;
            Ji(1, 2) = t2 * t3 * t8 * t6 * u1;
            Ji(0, 3) = t2 * t6 * t10 * u0;
            Ji(1, 3) = t2 * t6 * t10 * u1;

        }
    } else {
        // handle limit case for [u0,u1]->0
        if (parameterJacobian) {
            parameterJacobian->resize(2,NumDistortionIntrinsics);
            parameterJacobian->setZero();
        }
        J.setIdentity();
    }

    return true;
}
bool EquidistantDistortion::undistort(const Eigen::Vector2d & pointDistorted,
                                      Eigen::Vector2d * pointUndistorted) const
{
    // this is expensive: we solve with Gauss-Newton...
    Eigen::Vector2d x_bar = pointDistorted;  // initialise at distorted point
    const int n = 5;  // just 5 iterations max.
    Eigen::Matrix2d E;  // error Jacobian

    bool success = false;
    for (int i = 0; i < n; i++) {

        Eigen::Vector2d x_tmp;

        distort(x_bar, &x_tmp, &E);

        Eigen::Vector2d e(pointDistorted - x_tmp);//Hx = b=>
        Eigen::Vector2d du = (E.transpose() * E).inverse() * E.transpose() * e;

        x_bar += du;

        const double chi2 = e.dot(e);
        if (chi2 < 1e-2) {
            success = true;
        }
        //std::cout<<"chi2"<<chi2<<std::endl;
        if (chi2 < 1e-15) {
            success = true;
            break;
        }

    }
    *pointUndistorted = x_bar;

    return success;
}

bool EquidistantDistortion::undistort(const Eigen::Vector2f & pointDistorted,
                                      Eigen::Vector2f * pointUndistorted) const
{

    float dx2GN/*G-N解出的步长的欧式距离*/, dx2GD/*pU点的欧式距离*/, delta2/*信赖域半径*/, beta;
    Eigen::Vector2f dxGN/*G-N解出的增量*/, dxGD/*pU点*/;
    bool update, converge;
    float dr, j, dx2;

    delta2 = 1.0;
    // this is expensive: we solve with Gauss-Newton...
    Eigen::Vector2f x_bar = pointDistorted;  // initialise at distorted point
    const int n = 30;  // just 5 iterations max.
    const int UNDIST_DL_MAX_ITERATIONS = 4;
    Eigen::Matrix2f E;  // error Jacobian

    bool success = false;
    for (int i = 0; i < n; i++)
    {

        Eigen::Vector2f x_tmp;

        distort(x_bar, &x_tmp, &E);

        Eigen::Vector2f e(x_tmp -pointDistorted);
        Eigen::Vector2f dx = -(E.transpose() * E).inverse() * E.transpose() * e;
        Eigen::Vector2f b = -E.transpose() * e;
        Eigen::Matrix2f A = -E.transpose() * E;

        x_bar += dx;

//
//        dx2 = dx.squaredNorm();//x的欧式距离
//        //by wya
//       dxGN = dx;//G-N法求出的增量
//        dx2GN = dx2;//G-N法求出的增量的欧式距离
//        dx2GD = 0.0f;//dogleg
//        const float F = e.squaredNorm();//dogleg迭代开始前残差的欧式距离
//        const Eigen::Vector2f xnBkp = x_bar;//待优化变量
//        update = true;
//        converge = false;//狗腿迭代次数
//        for (int iIterDL = 0; iIterDL < UNDIST_DL_MAX_ITERATIONS; ++iIterDL)
//        {
//            if (dx2GN > delta2 && dx2GD == 0.0f) {//如果G-N增量在信赖域外且dx2GD还没有初始化
//                const float bl = sqrtf(b.squaredNorm());//模长
//                const Eigen::Vector2f g = b * (1.0f / bl);//梯度方向
//                const Eigen::Vector2f Ag = A * g;
//                const float xl = bl / g.dot(Ag);//计算pU点的步长
//                dxGD = g * -xl;//负梯度*步长,pU点
//                dx2GD = xl * xl;//pU点的半径
//
//            }
//            //三种情况,1：GN极值点在域内直接变成无约束条件 2:GN和pU点都在域外,那么就在给pU点的步长一个比例因子(域半径/自己的步长^2（因为用的是最小2乘）),让它刚好落在域半径上
//            //3:GN在域外,pU点在域内,那么增量就是GN极值点和pU点的连线与信赖域的交点
//            if (dx2GN <= delta2) {//如果G-N的极值在信赖域内,那么就是一个无约束问题,就直接用GN法求出的增量就可以
//                dx = dxGN;
//                dx2 = dx2GN;
//                beta = 1.0f;
//            } else if (dx2GD >= delta2) {//如果G-N和pU点求的最优点都在信赖域外
//                if (delta2 == 0.0f) {//信赖域为0,直接用pU点求得最优值
//                    dx = dxGD;
//                    dx2 = dx2GD;
//                } else {
//                    dx =  dxGD * sqrtf(delta2 / dx2GD);//乘比例因子
//                    dx2 = delta2;
//                }
//                beta = 0.0f;
//            } else {//GN在域外,pU点在域内,那么增量就是GN极值点和pU点的连线与信赖域的交点
//                const Eigen::Vector2f v = dxGN - dxGD;//方向
//                const float d = dxGD.dot(v), v2 = v.squaredNorm();
//                //beta = float((-d + sqrt(double(d) * d + (delta2 - dx2GD) * double(v2))) / v2);
//                beta = (-d + sqrtf(d * d + (delta2 - dx2GD) * v2)) / v2;//算得是在域外那段连线的长度
//                dx = dxGD;
//                dx += v * beta;
//                dx2 = delta2;
//            }
//            x_bar += dx;//加上这一次的增量
//            Eigen::Vector2f x_d_temp;
//            distort(x_bar, &x_d_temp);
//            const float dFa = F - (x_d_temp - pointDistorted).squaredNorm();//实际下降的
//            const float dFp = F - (e + ( E.transpose()) * dx).squaredNorm();//理论下降值,直接用J*dx近似下降值了
//            const float rho = dFa > 0.0f && dFp > 0.0f ? dFa / dFp : -1.0f;//求实际/理论的比值,理论不可能为负,实际为负的时候拒绝这次更新
//            //信赖域： Numerical Optimization 第二版 p69
//
//            //rho < 0.25 如果大于0说明近似的不好,需要减小信赖域,减小近似的范围。如果<0就说明是错误的近似,那么就拒绝这次的增量
//            if (rho < FTR_UNDIST_DL_GAIN_RATIO_MIN) {
//                delta2 *= FTR_UNDIST_DL_RADIUS_FACTOR_DECREASE;
//                if (delta2 < FTR_UNDIST_DL_RADIUS_MIN) {
//                    delta2 = FTR_UNDIST_DL_RADIUS_MIN;
//                }
//                x_bar = xnBkp;//取消这次增量
//                update = false;//不更新
//                converge = false;
//                continue;
//            } else if (rho > FTR_UNDIST_DL_GAIN_RATIO_MAX) //rho > 0.75,可以扩大信赖域半径
//            {
//                delta2 = std::max(delta2, FTR_UNDIST_DL_RADIUS_FACTOR_INCREASE * dx2);
//                if (delta2 > FTR_UNDIST_DL_RADIUS_MAX) {//信赖域半径最大值
//                    delta2 = FTR_UNDIST_DL_RADIUS_MAX;
//                }
//            }
//            update = true;//
//
//            converge = dx2 < 1e-12;//增量小于阈值,认为收敛
//            break;
//        }
//        if (!update || converge) {
//            break;
//        }
//
//        const double chi2 = e.dot(e);
//        if (chi2 < 1e-5) {
//            success = true;
//        }
        const double chi2 = e.dot(e);

        if (chi2 < 1e-8) {
            success = true;
            break;
        }

    }
    *pointUndistorted = x_bar;
    if(converge)
        success = true;

    return success;
}

bool EquidistantDistortion::undistort(const Eigen::Vector2d & pointDistorted,
                                      Eigen::Vector2d * pointUndistorted,
                                      Eigen::Matrix2d * pointJacobian) const
{
    // this is expensive: we solve with Gauss-Newton...
    Eigen::Vector2d x_bar = pointDistorted;  // initialise at distorted point
    const int n = 5;  // just 5 iterations max.
    Eigen::Matrix2d E;  // error Jacobian

    bool success = false;
    for (int i = 0; i < n; i++) {

        Eigen::Vector2d x_tmp;

        distort(x_bar, &x_tmp, &E);

        Eigen::Vector2d e(pointDistorted - x_tmp);
        Eigen::Vector2d dx = (E.transpose() * E).inverse() * E.transpose() * e;

        x_bar += dx;

        const double chi2 = e.dot(e);
        if (chi2 < 1e-2) {
            success = true;
        }
        if (chi2 < 1e-15) {
            success = true;
            break;
        }

    }
    *pointUndistorted = x_bar;

    // the Jacobian of the inverse map is simply the inverse Jacobian.
    *pointJacobian = E.inverse();

    return success;
}

bool EquidistantDistortion::undistort(const Eigen::Vector2f & pointDistorted,
                                      Eigen::Vector2f * pointUndistorted,
                                      Eigen::Matrix2f * pointJacobian) const
{
    // this is expensive: we solve with Gauss-Newton...
    Eigen::Vector2f x_bar = pointDistorted;  // initialise at distorted point
    const int n = 5;  // just 5 iterations max.
    Eigen::Matrix2f E;  // error Jacobian

    bool success = false;
    for (int i = 0; i < n; i++) {

        Eigen::Vector2f x_tmp;

        distort(x_bar, &x_tmp, &E);

        Eigen::Vector2f e(pointDistorted - x_tmp);
        Eigen::Vector2f dx = (E.transpose() * E).inverse() * E.transpose() * e;

        x_bar += dx;

        const double chi2 = e.dot(e);
        if (chi2 < 1e-2) {
            success = true;
        }
        if (chi2 < 1e-15) {
            success = true;
            break;
        }

    }
    *pointUndistorted = x_bar;

    // the Jacobian of the inverse map is simply the inverse Jacobian.
    *pointJacobian = E.inverse();

    return success;
}

}  // namespace cameras
}  // namespace okvis


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Andreas Gaida
 Copyright (C) 2008 Ralph Schreyer
 Copyright (C) 2008 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file ninepointlinearop.hpp
    \brief nine point linear operator
*/

#ifndef quantlib_nine_point_linear_op_hpp
#define quantlib_nine_point_linear_op_hpp

#include <ql/math/matrixutilities/sparsematrix.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearop.hpp>
#if !defined(QL_USE_STD_UNIQUE_PTR)
#include <boost/shared_array.hpp>
#endif
#include <memory>

namespace QuantLib {

    class FdmMesher;

    class NinePointLinearOp : public FdmLinearOp {
      public:
        NinePointLinearOp(Size d0, Size d1,
                const ext::shared_ptr<FdmMesher>& mesher);
        NinePointLinearOp(const NinePointLinearOp& m);
        NinePointLinearOp(NinePointLinearOp&& m) QL_NOEXCEPT;
        #ifdef QL_USE_DISPOSABLE
        NinePointLinearOp(const Disposable<NinePointLinearOp>& m);
        #endif
        NinePointLinearOp& operator=(const NinePointLinearOp& m);
        NinePointLinearOp& operator=(NinePointLinearOp&& m) QL_NOEXCEPT;
        #ifdef QL_USE_DISPOSABLE
        NinePointLinearOp& operator=(const Disposable<NinePointLinearOp>& m);
        #endif

        Disposable<Array> apply(const Array& r) const override;
        Disposable<NinePointLinearOp> mult(const Array& u) const;

        void swap(NinePointLinearOp& m);

        Disposable<SparseMatrix> toMatrix() const override;

      protected:
        NinePointLinearOp() = default;

        Size d0_, d1_;
        #if !defined(QL_USE_STD_UNIQUE_PTR)
        boost::shared_array<Size> i00_, i10_, i20_;
        boost::shared_array<Size> i01_, i21_;
        boost::shared_array<Size> i02_, i12_, i22_;
        boost::shared_array<Real> a00_, a10_, a20_;
        boost::shared_array<Real> a01_, a11_, a21_;
        boost::shared_array<Real> a02_, a12_, a22_;
        #else
        std::unique_ptr<Size[]> i00_, i10_, i20_;
        std::unique_ptr<Size[]> i01_, i21_;
        std::unique_ptr<Size[]> i02_, i12_, i22_;
        std::unique_ptr<Real[]> a00_, a10_, a20_;
        std::unique_ptr<Real[]> a01_, a11_, a21_;
        std::unique_ptr<Real[]> a02_, a12_, a22_;
        #endif

        ext::shared_ptr<FdmMesher> mesher_;
    };

    inline NinePointLinearOp::NinePointLinearOp(NinePointLinearOp&& m) QL_NOEXCEPT {
        swap(m);
    }

    inline NinePointLinearOp& NinePointLinearOp::operator=(const NinePointLinearOp& m) {
        NinePointLinearOp temp(m);
        swap(temp);
        return *this;
    }

    inline NinePointLinearOp& NinePointLinearOp::operator=(NinePointLinearOp&& m) QL_NOEXCEPT {
        swap(m);
        return *this;
    }

}

#endif


#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/physics.hh>

/* A simple drag plugin. Usage:
 *
 *    <gazebo>
 *      <plugin name="OrcaDragPlugin" filename="libOrcaDragPlugin.so">
 *        <link name="base_link">
 *          <center_of_mass>0 0 -0.2</center_of_mass>
 *          <tether_attach>-0.5, -0.4, 0</tether_attach>
 *          <linear_drag>10 20 30</linear_drag>
 *          <angular_drag>5 10 15</angular_drag>
 *          <tether_drag>4</tether_drag>
 *          <surface>10</surface>
 *        </link>
 *      </plugin>
 *    </gazebo>
 *
 *    <center_of_mass> Drag force is applied to the center of mass.
 *    <tether_attach> Relative position of tether attachment.
 *    <linear_drag> Linear drag constants. See default calculation.
 *    <angular_drag> Angular drag constants. See defaut calculation.
 *    <tether_drag> Tether drag constant. See default calculation.
 *    <surface> How far above z=0 the surface of the water is; used to calculate depth.
 *
 * Limitations:
 *    Tether drag is modeled only in x
 */

namespace gazebo {

/* drag = 0.5 * density * area * velocity^2 * coefficient
 *
 * The drag coefficient for a box is 1.0, so we'll use 0.9 for the ROV.
 * The drag coefficient for an unfaired tether is 1.2.
 *
 * The ROV constants below capture all but velocity:
 * constant = 0.5 * density * area * coefficient
 *
 * The tether constant below captures all but depth and velocity:
 * constant = 0.5 * density * width * coefficient
 */

constexpr double FLUID_DENSITY = 1029;    // Fluid density of seawater
constexpr double ROV_DIM_X = 0.457;       // Length
constexpr double ROV_DIM_Y = 0.338;       // Width
constexpr double ROV_DIM_Z = 0.254;       // Height
constexpr double ROV_AREA_X = ROV_DIM_Y * ROV_DIM_Z;  // Fore, aft area
constexpr double ROV_AREA_Y = ROV_DIM_X * ROV_DIM_Z;  // Top, bottom area
constexpr double ROV_AREA_Z = ROV_DIM_X * ROV_DIM_Y;  // Port, starboard area
constexpr double ROV_DRAG_COEFFICIENT_X = 0.8;
constexpr double ROV_DRAG_COEFFICIENT_Y = 0.95;
constexpr double ROV_DRAG_COEFFICIENT_Z = 0.95;
constexpr double ROV_LINEAR_DRAG_X = 0.5 * FLUID_DENSITY * ROV_AREA_X * ROV_DRAG_COEFFICIENT_X;
constexpr double ROV_LINEAR_DRAG_Y = 0.5 * FLUID_DENSITY * ROV_AREA_Y * ROV_DRAG_COEFFICIENT_Y;
constexpr double ROV_LINEAR_DRAG_Z = 0.5 * FLUID_DENSITY * ROV_AREA_Z * ROV_DRAG_COEFFICIENT_Z;
constexpr double ANGULAR_DRAG_X = ROV_LINEAR_DRAG_X / 2; // A hack
constexpr double ANGULAR_DRAG_Y = ROV_LINEAR_DRAG_Y / 2;
constexpr double ANGULAR_DRAG_Z = ROV_LINEAR_DRAG_Z / 2; // TODO these are wrong, see revised cals in orca_mission.cpp
constexpr double TETHER_DIAM = 0.008;
constexpr double TETHER_DRAG_COEFFICIENT = 1.1;
constexpr double TETHER_DRAG = 0.5 * FLUID_DENSITY * TETHER_DIAM * TETHER_DRAG_COEFFICIENT;

class OrcaDragPlugin : public ModelPlugin
{
private:

  physics::LinkPtr base_link_;

  // Drag force will be applied to the center_of_mass_ (body frame)
  ignition::math::Vector3d center_of_mass_ {0, 0, 0};

  // Tether drag will be applied to the tether attachment point (body frame)
  ignition::math::Vector3d tether_attach_ {0, 0, 0};

  // Drag constants (body frame)
  ignition::math::Vector3d linear_drag_ {ROV_LINEAR_DRAG_X, ROV_LINEAR_DRAG_Y, ROV_LINEAR_DRAG_Z};
  ignition::math::Vector3d angular_drag_ {ANGULAR_DRAG_X, ANGULAR_DRAG_Y, ANGULAR_DRAG_Z};
  double tether_drag_ {TETHER_DRAG};

  // Distance to surface
  double surface_ {10};

  event::ConnectionPtr update_connection_;

public:

  // Called once when the plugin is loaded.
  void Load(physics::ModelPtr model, sdf::ElementPtr sdf)
  {
    std::string link_name {"base_link"};

    std::cout << std::endl;
    std::cout << "ORCA DRAG PLUGIN PARAMETERS" << std::endl;
    std::cout << "-----------------------------------------" << std::endl;
    std::cout << "Default link name: " << link_name << std::endl;
    std::cout << "Default center of mass: " << center_of_mass_ << std::endl;
    std::cout << "Default tether attachment point: " << tether_attach_ << std::endl;
    std::cout << "Default linear drag: " << linear_drag_ << std::endl;
    std::cout << "Default angular drag: " << angular_drag_ << std::endl;
    std::cout << "Default tether drag: " << tether_drag_ << std::endl;
    std::cout << "Default surface: " << surface_ << std::endl;

    GZ_ASSERT(model != nullptr, "Model is null");
    GZ_ASSERT(sdf != nullptr, "SDF is null");

    if (sdf->HasElement("link"))
    {
      sdf::ElementPtr linkElem = sdf->GetElement("link"); // Only one link is supported

      if (linkElem->HasAttribute("name"))
      {
        linkElem->GetAttribute("name")->Get(link_name);
        std::cout << "Link name: " << link_name << std::endl;
      }

      if (linkElem->HasElement("center_of_mass"))
      {
        center_of_mass_ = linkElem->GetElement("center_of_mass")->Get<ignition::math::Vector3d>();
        std::cout << "Center of mass: " << center_of_mass_ << std::endl;
      }

      if (linkElem->HasElement("tether_attach"))
      {
        tether_attach_ = linkElem->GetElement("tether_attach")->Get<ignition::math::Vector3d>();
        std::cout << "Tether attachment point: " << tether_attach_ << std::endl;
      }

      if (linkElem->HasElement("linear_drag"))
      {
        linear_drag_ = linkElem->GetElement("linear_drag")->Get<ignition::math::Vector3d>();
        std::cout << "Linear drag: " << linear_drag_ << std::endl;
      }

      if (linkElem->HasElement("angular_drag"))
      {
        angular_drag_ = linkElem->GetElement("angular_drag")->Get<ignition::math::Vector3d>();
        std::cout << "Angular drag: " << angular_drag_ << std::endl;
      }

      if (linkElem->HasElement("tether_drag")) // TODO should be child of gazebo element, not link element
      {
        tether_drag_ = linkElem->GetElement("tether_drag")->Get<double>();
        std::cout << "Tether drag: " << tether_drag_ << std::endl;
      }

      if (linkElem->HasElement("surface")) // TODO should be child of gazebo element, not link element
      {
        surface_ = linkElem->GetElement("surface")->Get<double>();
        std::cout << "Surface: " << surface_ << std::endl;
      }
    }

    base_link_ = model->GetLink(link_name);
    GZ_ASSERT(base_link_ != nullptr, "Missing link");

    // Listen for the update event. This event is broadcast every simulation iteration.
    update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&OrcaDragPlugin::OnUpdate, this, _1));

    std::cout << "-----------------------------------------" << std::endl;
    std::cout << std::endl;
  }

  // Called by the world update start event, up to 1000 times per second.
  void OnUpdate(const common::UpdateInfo& /*info*/)
  {
    ignition::math::Vector3d linear_velocity = base_link_->RelativeLinearVel();
    ignition::math::Vector3d angular_velocity = base_link_->RelativeAngularVel();

    ignition::math::Vector3d drag_force;
    drag_force.X() = linear_velocity.X() * fabs(linear_velocity.X()) * -linear_drag_.X();
    drag_force.Y() = linear_velocity.Y() * fabs(linear_velocity.Y()) * -linear_drag_.Y();
    drag_force.Z() = linear_velocity.Z() * fabs(linear_velocity.Z()) * -linear_drag_.Z();
    base_link_->AddLinkForce(drag_force, center_of_mass_);

    ignition::math::Vector3d drag_torque;
    drag_torque.X() = angular_velocity.X() * fabs(angular_velocity.X()) * -angular_drag_.X();
    drag_torque.Y() = angular_velocity.Y() * fabs(angular_velocity.Y()) * -angular_drag_.Y();
    drag_torque.Z() = angular_velocity.Z() * fabs(angular_velocity.Z()) * -angular_drag_.Z();
    base_link_->AddRelativeTorque(drag_torque); // ODE adds torque at the center of mass

    // Tether drag only accounts for motion in x (forward/reverse)
    ignition::math::Vector3d tether_force;
    double depth = surface_ - base_link_->WorldPose().Pos().Z();
    tether_force.X() = linear_velocity.X() * fabs(linear_velocity.X()) * depth * -tether_drag_;
    tether_force.Y() = 0;
    tether_force.Z() = 0;
    base_link_->AddLinkForce(tether_force, tether_attach_);
  }
};

GZ_REGISTER_MODEL_PLUGIN(OrcaDragPlugin)

}


#ifndef CANNON_PHYSICS_LAGRANGIAN_MECHANICS
#define CANNON_PHYSICS_LAGRANGIAN_MECHANICS

/*!
 * \file cannon/physics/lagrangian_mechanics.hpp
 * \brief File containing class and free function definitions for working with
 * Lagrangian dynamics, as in Structure and Interpretation of Classical
 * Mechanics.
 */

#include <functional>

#include <Eigen/Dense>

#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::log;

namespace cannon {
  namespace physics {

    /*!
     * \brief Struct representing a local tuple consisting of a coordinate
     * vector, velocity vector, and time.
     */
    struct LocalTuple {

      LocalTuple() = delete;

      /*!
       * \brief Constructor taking generalized coordinate dimension and
       * initializing coordinates and time to zero.
       */
      LocalTuple(unsigned int dim) : dim(dim), time(0.0) {
        coordinates = VectorXd::Zero(dim);
        velocities = VectorXd::Zero(dim);
      }

      /*!
       * \brief Copy constructor.
       */
      LocalTuple(const LocalTuple &l) : dim(l.dim), time(l.time),
      coordinates(l.coordinates), velocities(l.velocities) {}

      /*!
       * \brief Move constructor.
       */
      LocalTuple(LocalTuple &&l) : dim(l.dim), time(l.time),
      coordinates(std::move(l.coordinates)),
      velocities(std::move(l.velocities)) {}

      /*!
       * \brief Copy assignment operator.
       */
      LocalTuple& operator=(const LocalTuple& o) {
        dim = o.dim;
        time = o.time;
        coordinates = o.coordinates;
        velocities = o.velocities;

        return *this;
      }

      /*!
       * \brief Get a vector representation of this local tuple.
       *
       * \returns A vector containing the time, generalized coordinates, and
       * generalized velocity for this local tuple, in that order.
       */
      VectorXd to_vec();

      /*!
       * \brief Initialize this local tuple with the input vector, which should
       * have the time, generalized coordinates, and generalized velocities, in
       * that order.
       *
       * \param vec The vector to use for initialization.
       */
      void from_vec(const VectorXd& vec);

      unsigned int dim; //!< Dimension of generalized coordinates
      double time; //!< Time for this local tuple
      VectorXd coordinates; //!< Generalized coordinates for this local tuple.
      VectorXd velocities; //!< Generalized velocities for this local tuple

    };

    /*!
     * \brief Struct representing a path as a series of waypoints in
     * generalized coordinates for doing Lagrangian mechanics.
     */
    struct Path {
      Path() = delete;

      /*!
       * \brief Straight-line constructor taking a start, and end time and
       * start and end coordinates, as well as the number of waypoints to
       * construct.
       */
      Path(double t0, double t1, VectorXd start, VectorXd end,
          unsigned int length) : t0(t0), t1(t1) {
        assert(0 <= t0);
        assert(t0 < t1);
        assert(start.size() == end.size());
        assert(length >= 2);

        VectorXd delta = (end - start) / (double)(length-1);

        for (unsigned int i = 0; i < length; i++) {
          waypoints.push_back(start + i * delta);
        }
      }

      /*!
       * \brief Constructor taking generalized coordinate dimension and a
       * vector of appended waypoints.
       */
      Path(unsigned int dim, const VectorXd& vec) {
        from_vec(dim, vec);
      }

      /*!
       * \brief Get a vector representation of this path by appending all
       * waypoints.
       *
       * \returns The vector representation.
       */
      VectorXd to_vec();

      /*!
       * \brief Load a path with waypoints of the input dimension from the
       * input vector.
       *
       * \param vec The vector to load.
       */
      void from_vec(unsigned int dim, const VectorXd& vec);

      /*!
       * \brief Get the local tuple at time t from this path.
       *
       * Just doing linear interpolation for now.
       *
       * \param t The time to get local tuple for.
       *
       * \returns Local tuple at the input time along this path.
       */
      LocalTuple get_local_tuple(double t) const;

      double t0; //!< Start time for this path
      double t1; //!< End time for this path
      std::vector<VectorXd> waypoints; //!< Waypoints making up this path
    };

    using Lagrangian = std::function<double(const LocalTuple&)>;

    /*!
     * \brief Compute path action of the input Lagrangian on the input path.
     *
     * \param L The Lagrangian to use
     * \param q The path to evaluate path action on
     *
     * \returns The path action
     */
    double compute_path_action(Lagrangian L, const Path& q);

  } // namespace physics
} // namespace cannon

#endif /* ifndef CANNON_PHYSICS_LAGRANGIAN_MECHANICS */


/*
 * H.cpp
 *
 *  Created on: Apr 11, 2014
 *      Author: wilfeli
 *
 *
 *
 *
 *
 *
 *
 *
 *
 */

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

#include "H.h"
#include "Asset.h"
#include "W.h"
#include "Contract.h"
#include "Market.h"
#include "Tools.h"

using Eigen::MatrixXd;

H::H(W &w_, int id_):w(w_),
l_ask(0.0, 0.0),
c_bid(0.0, 0.0),
id(id_),
theta_x(1,3),
s0(1,18){
	// create reference to the world
	w.AddH(this);

	//create storage for goods
	ass_asGC.push_back(new GoodC(this, 0.0));

	//create HK
	ass_asHK.push_back(new HK(1.0));

	type = "H";

	c_bid.issuer = this;
	l_ask.issuer = this;

	//create wm
	ExpectationBackward* exp = new ExpectationBackward(1.0, 10, 0.5);
	wm["MasCHK"] = exp;

	//create wm
	exp = new ExpectationBackward(1.0, 10, 0.5);
	wm["MasGC"] = exp;

	//create wm
	exp = new ExpectationBackward(1.0, 0, 0.0);
	wm["api"] = exp;

	//create wm
	exp = new ExpectationBackward(0.0, 0, 0.01);
	wm["fi_ti"] = exp;

	//parameters
	param = new Parameters();
	param->init();

	//accounting
	account = new AccountH();

};

void
H::init(){
    //ADP setup
    theta_V = Eigen::MatrixXd::Constant(1, 1, param->ADP_BF_N);
    B_n_1_RLS = Eigen::MatrixXd::Zero(param->ADP_BF_N, param->ADP_BF_N);

    //QL setup
    _set_QL();

    //mRe setup
    _set_mRE();

};

void
H::ac_W(MessageMakeDec* mes){

	//calls opt choice
	_s0();
	//calls opt choice
    if (param->opt_TYPE == "EO_CS"){
        opt_CS(&s0,&theta_x, param->opt_CS_N);
    };

    if (param->opt_TYPE == "EO_ADP"){
        theta_V = _API_LM(&s0, param->ADP_API_LM_N, param->ADP_API_LM_M);

        Eigen::Block<Eigen::MatrixXd> s = s0.block(0,0,s0.rows(),s0.cols());
        _opt_CS_s_V(s,theta_x, theta_V, param->opt_CS_N);
    };

    if (param->opt_TYPE == "QL"){
        opt_QL();
    };

    if (param->opt_TYPE == "mRE"){
        opt_mRE();

    };

	//transforms it into ask and bid
	l_ask.q = theta_x(0,0) * s0(0,1);
	l_ask.p = theta_x(0,1) * s0(0,4);

	c_bid.pq_share = theta_x(0,2);

};

Ask*
H::ac_L(){
	//update numbers in bid
	l_ask.supply_curve = Eigen::MatrixXd::Zero(1,2);
	l_ask.supply_curve << l_ask.p , l_ask.q;

	l_ask.p_t_0 = l_ask.p;
	l_ask.q_t_0 = l_ask.q;

	//return reference to the ask
	return &l_ask;
};

Bid*
H::ac_C(){
	//update numbers in bid
	c_bid.pq = _s0()(0,0) * c_bid.pq_share;

	c_bid.pq_t_0 = c_bid.pq;
	c_bid.pq_share_t_0 = c_bid.pq_share;

	//update numbers in bid

//    if (param->GRID == "test"){
//        c_bid.demand_curve = Eigen::MatrixXd::Zero(1,2);
//        c_bid.demand_curve.row(0) << wm["MasGC"]->mu , c_bid.pq/wm["MasGC"]->mu ;
//        return &c_bid;
//    };

	//number of steps
	int N_STEPS_dc = 30;
	double P_MAX_dc = wm["MasGC"]->mu * 10;
	double P_MIN_dc = wm["MasGC"]->mu * 0.01;
	double STEP_SIZE = (P_MAX_dc - P_MIN_dc)/N_STEPS_dc;

	c_bid.demand_curve = Eigen::MatrixXd::Zero(N_STEPS_dc,2);
	for (int i = 0; i < N_STEPS_dc; i++){

		c_bid.demand_curve.row(i) << P_MIN_dc + i * STEP_SIZE, c_bid.pq/(P_MIN_dc + i * STEP_SIZE);

	};

	//return reference to the bid
	return &c_bid;
};

double
H::_mas_q_sell(MessageMarketLCheckAsk* inf){
	//checks how much to sell
	//return q from ask

	double q_sell;

	if (inf->p_eq >= l_ask.p){
		q_sell = l_ask.q;
	};

	return q_sell;
};

double
H::_mas_q_buy(MessageMarketCCheckBid* inf){
	//checks how much to sell
	//return q from ask

	double q_buy;

	int i=0;

	while ((c_bid.demand_curve(i,0) < inf->p_eq) && (i<c_bid.demand_curve.rows())){
		i++;
	};

	if (i<c_bid.demand_curve.rows()){
		q_buy = c_bid.demand_curve(i,1);
	}else{
		//buys zero
		q_buy = 0.0;
	};

	return q_buy;
};

void
H::_sell_asCHK(MessageMarketLSellC* inf){
	//new contract
	ContractHK* cHK = new ContractHK(this,
                                     inf->buyer,
                                     inf->p,
                                     inf->q,
                                     w.t,
                                     w.t);

	type = "H_EO";

	ass_asCHK.push_back(cHK);

	//?update ask
	l_ask.q -= cHK->q;

	//tell market about the contract
	inf->sender->_clear_C(cHK);

	//call accounting
	//create accounting message
    //	MessageSellC* mes = new MessageSellC(inf, cHK);
	account->ac_get_inf(inf, cHK);

    //	delete mes;

};

double
H::_goal_t(Eigen::MatrixXd* s){

	double l;
	double c;

	if ((*s)(0,3) <= 0.0){
		c = -0.5;
//        c = 0.0;
	}else{
        c = (*s)(0,3);
    };

	l = (*s)(0,2);

	return (param->GOAL_T_theta(0,0) * log(1+c) + param->GOAL_T_theta(0,1) * (1-l));
//    return (param->GOAL_T_theta(0,0) * log(0.5+c) + param->GOAL_T_theta(0,1) * (1-l));
};

void
H::_buy_asGC(MessageGoodC* mes){

	ass_asGC.back()->q += mes->q;
	account->ac_get_inf(mes);

};

void
H::_PS_receive_payment(MessagePSSendPayment* mes){

	account->ac_get_inf(mes);

};

bool
H::_PS_send_payment(MessagePSSendPayment* inf){

	inf->sender = this; //static_cast<IHolderPS*>(const_cast<H*>(this));

	bool FLAG_CLEARED = ass_asCBDt0.front()->issuer->_PS_accept_PO(inf);

	if(FLAG_CLEARED){
		account->ac_get_inf(inf);

	};

	return FLAG_CLEARED;

};

void
H::ac_W_begin_step(){

	//update accounting
	account->ac_W_initialize_step();

	//
	for (auto c:ass_asCHK){
		if (c->t_end <= w.t){
			delete c;
			c = NULL;
		};
	};

	//clear contracts
	ass_asCHK.erase(std::remove_if(ass_asCHK.begin(), ass_asCHK.end(),
                                   [&](ContractHK* x) -> bool { return (x); }),
                    ass_asCHK.end());

};

void
H::ac_W_begin_step(MessageStatus* mes){
	if (mes->status <= 0.0){
		account->ac_W_initialize_step(mes);
	};
};

void
H::ac_W_end_step(){
	//updates state
	_s0();
	double ut = _goal_t(&s0);
	account->ac_W_end_step(&s0, ut);

	//eats good
	ass_asGC.front()->q = 0.0;

};

void
H::ac_W_end_step(MessageStatus* mes){
    if (mes->status <= 0.0){
        ac_W_end_step();
    };
};

void
H::wm_update_k_s(){
	if (w.t > 0.0){

		MatrixXd w0 = wm_w0_tbeg();
		ExpectationBackward* exp_i;
		std::vector<double> s_t_i;

		//update k_s if conditions are met
//		if ((w0(0,1) > 0.0) || ((w0(0,1) == 0.0) && (w0(0,0) != 0.0)) || ((w0(0,4) > 0.0) && (w0(0,0) == 0.0))){
			s_t_i.push_back(w0(0,0));
			s_t_i.push_back((w0(0,1) > 0.0)? w0(0,1):w0(0,7));

			exp_i = wm["MasCHK"];

			exp_i->s_t.push_back(s_t_i);

			wm_update_expectation_backward(exp_i, 1, true);
//		};

		s_t_i.clear();

		s_t_i.push_back(w0(0,2));
		s_t_i.push_back((w0(0,2) > 0.0)? w0(0,3)/w0(0,2):w0(0,8));

		exp_i = wm["MasGC"];

		exp_i->s_t.push_back(s_t_i);

		wm_update_expectation_backward(exp_i, 1, true);

		s_t_i.clear();

		if (w.t > 0.0){
			exp_i = wm["api"];

			s_t_i.push_back(w0(0,5));
			exp_i->s_t.push_back(s_t_i);

			wm_update_expectation_backward(exp_i, 0, false);

			s_t_i.clear();

			exp_i = wm["fi_ti"];

			s_t_i.push_back(w0(0,6));
			exp_i->s_t.push_back(s_t_i);

//			wm_update_expectation_backward(exp_i, 0, false);
            wm_update_expectation_backward(exp_i, 0, true);

		};

        if (param->opt_TYPE == "QL"){
            wm_update_Q();

        };

        if (param->opt_TYPE == "mRE"){
            wm_update_mRE();

        };

	};

};

void
H::wm_update_expectation_backward(ExpectationBackward* exp_i, int p_position, bool FLAG_UPDATE_V){

	std::vector<double> p;

	int i_s_t_1_max = 0;
	long i_s_t_max = 0;

	//depending on the length of the wm and accumulated number of prices
	if (std::isinf(param->WM_LENGTH)){
		i_s_t_1_max = exp_i->s_t_1["n"];
		i_s_t_max = exp_i->s_t.size();
	}else{
		i_s_t_1_max = std::min(std::max(param->WM_LENGTH - exp_i->s_t.size(), 0.0), exp_i->s_t_1["n"]);
		i_s_t_max = std::min(param->WM_LENGTH, (double)exp_i->s_t.size());
	};

	for (int i = 0; i < i_s_t_1_max; i++){
		p.push_back(exp_i->s_t_1["mu"]);
	};

	//push other prices
	for (std::size_t i = (exp_i->s_t.size() - i_s_t_max);i < exp_i->s_t.size();i++){
		p.push_back(exp_i->s_t[i][p_position]);
	};

	MatrixXd mean_variance = wm_mean_variance(p);

	exp_i->mu = mean_variance(0,0);
	exp_i->n += 1.0;

    if (FLAG_UPDATE_V){
        exp_i->v = mean_variance(1,0);

        if (exp_i->v <= 0.0){
            exp_i->v = exp_i->mu * 0.01;
        };

    };

};

Eigen::MatrixXd
H::wm_mean_variance(std::vector<double> &x){
	MatrixXd mean_variance(2,1);

	//gets mean and variance
	double sum = std::accumulate(x.begin(), x.end(), 0.0);
	double mean = sum / x.size();
	std::vector<double> diff(x.size());
	std::transform(x.begin(), x.end(), diff.begin(),
	               std::bind2nd(std::minus<double>(), mean));
	double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
    //	double stdev = std::sqrt(sq_sum / x.size());
	double variance = sq_sum/x.size();

	mean_variance(0,0) = mean;
	mean_variance(1,0) = variance;

	return mean_variance;
};

Eigen::MatrixXd
H::wm_w0_tbeg(){
	MatrixXd w0(1,9);

	long life_length = account->asCHK_q.size();

	w0(0,0) = account->asCHK_q.at(life_length - 2);
	w0(0,1) = account->asCHK_p.at(life_length - 2);
	w0(0,2) = account->asGC_q.at(life_length - 2);
	w0(0,3) = account->asGC_pq.at(life_length - 2);

	w0(0,4) = l_ask.q_t_0;
	w0(0,5) = account->FI_div_t.at(life_length - 2) + account->asCHK_p_t.at(life_length - 2);
	w0(0,6) = account->FI_div_t.at(life_length - 2);

	w0(0,7) = w.ml->market_price.at(w.t - 1)->p;
	w0(0,8) = w.mc->market_price.at(w.t - 1)->p;

    //	std::cout << w0 << std::endl;

	return w0;
};

Eigen::MatrixXd
H::_s0(){
	//money holdings
	s0(0,0) = ass_asCBDt0.front()->q;

	s0(0,1) = ass_asHK.front()->q;

	s0(0,2) = 0.0;
	//labor to be supplied
	for (auto c:ass_asCHK){
		s0(0,2) += c->q;
	};

	//amount of good c
	s0(0,3) = ass_asGC.front()->q;

	//expected price on labor market
	s0(0,4) = wm["MasCHK"]->mu;

	//number of observations
	s0(0,5) = wm["MasCHK"]->n;

	//expected price on goods market
	s0(0,6) = wm["MasGC"]->mu;

	//number of observations
	s0(0,7) = wm["MasGC"]->n;

	//average past income
	s0(0,8) = wm["api"]->mu;

	s0(0,9) = wm["api"]->n;

	//current income
	s0(0,10) = account->FI_div_t.back() + account->asCHK_p_t.back();

	//to be used in max routinue
	//current signed labor contracts - q
	s0(0,11) = 0.0;

	//current signed labor contracts - p
	s0(0,12) = 0.0;

	//financial income
	s0(0,13) = wm["fi_ti"]->mu;

	s0(0,14) = wm["fi_ti"]->n;

	//variance of labor market
	s0(0,15) = wm["MasCHK"]->v;

	//variance of goods market
	s0(0,16) = wm["MasGC"]->v;

	//variance of financial income
	s0(0,17) = wm["fi_ti"]->v;

	//

	return s0;
};

void
H::opt_CS(MatrixXd* s0,  MatrixXd* theta_x, int N){

//	N = 1;

	//gets matrix of thetas
	//prepare initial matrix
	MatrixXd m_s0 = MatrixXd::Zero(N, s0->cols());

	m_s0.rowwise() += s0->row(0);

	//allocate space to the matrix
	MatrixXd n_w;
	MatrixXd n_x;
	MatrixXd n_s = MatrixXd::Zero(N, s0->cols());
	MatrixXd n_c;

	//create matrix for the grid - small size here
	MatrixXd grid(3,3);

    create_decision_grid(grid,0);

    if (w.param->SIMULATION_MODE == "test"){
        Tools::mgrid_test(grid, &theta_x_all);

    } else{

        Tools::mgrid(grid, &theta_x_all);
    };

    //	std::cout << theta_x_all.transpose() << "\n";

	//create random number generators for the implementation

	boost::normal_distribution<> nd_w1((*s0)(0,4), pow((*s0)(0,15),0.5));
	boost::normal_distribution<> nd_w2((*s0)(0,6), pow((*s0)(0,16),0.5));
	boost::normal_distribution<> nd_w3((*s0)(0,13), pow((*s0)(0,17),0.5));

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w1(w.rng, nd_w1);

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w2(w.rng, nd_w2);

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w3(w.rng, nd_w3);

    if (w.param->SIMULATION_MODE != "test"){

        //draw random variables
        //fast realization
        n_w = MatrixXd::Zero(N, 3 * param->T_MAX);
        for (int i = 0; i < N; i++){
            for(int j=0; j< n_w.cols(); j++){
                if (j%3 == 0){
                    n_w(i,j) = rng_w1();
                } else {
                    if (j%3 == 1){
                        n_w(i,j) = rng_w2();
                    }else{
                        n_w(i,j) = rng_w3();
                    };

                };
                n_w(i,j) = std::max(0.0, (double)n_w(i,j));
            };
        };
    };

	//matrix of all
	MatrixXd c_all(theta_x_all.cols(),1);

	for (int i = 0 ; i < theta_x_all.cols(); i++){
        if (w.param->SIMULATION_MODE == "test"){
            n_w = MatrixXd::Zero(N, 3 * param->T_MAX);
            for (int i = 0; i < N; i++){
                for(int j=0; j< n_w.cols(); j++){
                    if (j%3 == 0){
                        n_w(i,j) = Tools::get_normal((*s0)(0,4), pow((*s0)(0,15),0.5), w.myrng);
                    } else {
                        if (j%3 == 1){
                            n_w(i,j) = Tools::get_normal((*s0)(0,6), pow((*s0)(0,16),0.5), w.myrng);
                        }else{
                            n_w(i,j) = Tools::get_normal((*s0)(0,13), pow((*s0)(0,17),0.5), w.myrng);
                        };

                    };
                    n_w(i,j) = std::max(0.0, (double)n_w(i,j));
                };
            };
        };

		//call estimator of results of steps, given theta_x
        theta_x->row(0) = theta_x_all.col(i).transpose();
		n_x = MatrixXd::Zero(N, 3);
		n_s = m_s0;
		n_c = MatrixXd::Zero(N, param->T_MAX);
		_step_opt_CS(theta_x, &n_w, &n_x, &n_s, &n_c, param->T_MAX);

		//after n_c
		//average over n runs
//        std::cout << n_w << std::endl;
//        std::cout << n_c << std::endl;
		c_all(i,0) = (n_c * param->BETA_T).mean();

	};

    //	std::cout << c_all << "\n";

	//find all max elements
	double max = c_all(0,0);
	std::list<int> i_max;
	for (int i=0; i<c_all.rows(); i++){
		if (c_all(i,0) == max){
			i_max.push_back(i);
		}else{
			if (c_all(i,0) > max){
				max = c_all(i,0);
				i_max.clear();
				i_max.push_back(i);
			};
		}
	};

	//if more than 1 max - randomly pick
	long theta_max_i;
	if (i_max.size()>1){
		boost::random::uniform_int_distribution<> i_max_dist(0, i_max.size()-1);
		theta_max_i = i_max_dist(w.rng);
	}else{
		theta_max_i = i_max.front();
	};

	theta_x->row(0) = theta_x_all.col(theta_max_i).transpose();

    //	std::cout << "theta" << *theta_x << "\n";
};

double
H::get_status(){
	return status;
};

void
H::_step_opt_CS(MatrixXd *theta_x,
                MatrixXd *w,
                MatrixXd *x,
                MatrixXd *s,
                MatrixXd *c,
                int T){

	double l_t;
	double c_t;

    //	std::cout <<"w" << *w << "\n";

	for (int i=0; i< T; i++){

		x->col(0) = (*theta_x)(0,0) * s->col(1).array();
		x->col(1) = (*theta_x)(0,1) * s->col(4).array();

		//labor market results
		//if wage is less than realized wage - get hired, otherwise zero
		s->col(11) = (w->col(i*3).array() >= x->col(1).array()).cast<double>().array() *x->col(0).array();
		s->col(12) = w->col(i*3);

        //		auto FLAG_UPDATE = ((s->col(12) > 0.0).array() ||
        //							((s->col(12) == 0.0).array() &&
        //							(s->col(11) != 0.0).array()) ||
        //							((x->col(0) > 0.0).array() &&
        //							(s->col(11) == 0.0).array()));

		//TODO update inside , not coded for now
        //		if (param->wm_INSIDE_UPDATE){
        //		};

		//made consumption good decision
		x->col(2) = (*theta_x)(0,2) * s->col(0).array();

		//buy consumption goods , assume all goods are consumed in the previous
		//period, so no addition here
		for (int j=0; j<s->rows(); j++){
			if ((*w)(j,i*3 + 1) > 0.0){
				(*s)(j,3) = (*x)(j,2) / (*w)(j,i*3 + 1);

				//for speed calculate goal in this cycle
				//stores realization of a goal
				//as couldn't pass block need to calculate in place?

                //				(*c)(j,i) = _goal_t(&(s->block(1,s->cols(),j,0)));
			}else{
				(*s)(j,3) = 0.0;
			};

			if ((*s)(j,3) <= 0.0){
				c_t = -0.5;
//                c_t = 0.0;
			} else {
				c_t = (*s)(j,3);
			};

			l_t = (*s)(j,11);

			(*c)(j,i) = param->GOAL_T_theta(0,0) * log(1+c_t) + param->GOAL_T_theta(0,1) * (1-l_t);
//            (*c)(j,i) = param->GOAL_T_theta(0,0) * log(0.5+c_t) + param->GOAL_T_theta(0,1) * (1-l_t);

		};

		//account for money being spent
		s->col(0) = s->col(0).array() - w->col(i*3 + 1).array() * s->col(3).array();

		//labor payments
		s->col(0) = s->col(0).array() + s->col(11).array() * s->col(12).array();

		//dividend payment
		s->col(0) += w->col(i*3 + 2);

        //		std::cout << s->col(0) << "\n";
//        std::cout << *s << "\n";
        //		std::cout << *c << "\n";
        //		std::cout << *x << "\n";

		//skipped, because they are assigned, not added in a cycle
		//zero labor contracts
		//zero consumption good

	};

};

void
H::_bf(Eigen::MatrixXd* s, Eigen::MatrixXd* ret){
    (*ret)(0,0) = (*s)(0,0);
};

void
H::_bf(Eigen::Block<Eigen::MatrixXd>& s, Eigen::MatrixXd* ret){
    (*ret)(0,0) = s(0,0);
};

double
H::_V(Eigen::MatrixXd* s, Eigen::MatrixXd* phi_f, Eigen::MatrixXd& theta_V){
    _bf(s, phi_f);

    return (theta_V*(*phi_f).transpose()).sum();

};

double
H::_V(Eigen::Block<Eigen::MatrixXd>& s, Eigen::MatrixXd* phi_f, Eigen::MatrixXd& theta_V){
    _bf(s, phi_f);

    return (theta_V*(*phi_f).transpose()).sum();

};

Eigen::MatrixXd
H::_API_LM(Eigen::MatrixXd* s0, int N, int M){
    //Approximate policy iteration using linear models.
    //
    //p.407 ADP
    //

    //fix basis functions
    long n_theta_t = param->ADP_BF_N;

    //inner theta
    //theta for now and future
    Eigen::MatrixXd theta_V_n = Eigen::MatrixXd::Constant(1, n_theta_t, 1);

    //policy theta
    Eigen::MatrixXd theta_V_pi = theta_V;

    Eigen::MatrixXd s_n_m;
    Eigen::MatrixXd v_n_m;
    Eigen::MatrixXd x_n_m(1,3);
    Eigen::MatrixXd phi(1, param->ADP_BF_N);
    Eigen::MatrixXd phi1(1, param->ADP_BF_N);

    for (int n=0; n<N; n++){
        theta_V_n = theta_V_pi;

        s_n_m = MatrixXd::Zero(M+1, s0->cols());
        s_n_m.row(0) += s0->row(0);
        //v_n_m_T1 = 0
        v_n_m = Eigen::MatrixXd::Constant(M, n_theta_t, 0.0);

        for (int m=0; m<M; m++){

            Eigen::Block<Eigen::MatrixXd> s = s_n_m.block(m,0,1,(*s0).cols());

            //choose action, takes reference to s
            _opt_CS_s_V(s, x_n_m, theta_V_pi, param->opt_CS_N);

            s_n_m.block(m+1,0,1,(*s0).cols()) = s;

            Eigen::Block<Eigen::MatrixXd> s1 = s_n_m.block(m+1,0,1,(*s0).cols());
            s = s_n_m.block(m,0,1,(*s0).cols());

            //make step given action, to update state and return value function
            double _c = _c_theta_x(x_n_m, s1, theta_V_pi);

            double c_n_m = _c - _V(s1,&phi,theta_V_pi);

            _bf(s,&phi);
            _bf(s1,&phi1);
            Eigen::MatrixXd v_n_m = phi - param->BETA*phi1;

            //            std::cout << s_n_m.row(m) << std::endl;
            //            std::cout << s << std::endl;
            //
            //            std::cout << s_n_m.row(m+1) << std::endl;
            //            std::cout << theta_V_pi << std::endl;
            //            std::cout << theta_V_n << std::endl;

            //update theta_V_n
            theta_V_n = _RLS(theta_V_n,
                             v_n_m,
                             s,
                             m,
                             c_n_m);
            //assume that only 1 element in v and restrict it
            if (theta_V_n(0,0) > 1000){
                theta_V_n(0,0) = 1000;
            };
            if (theta_V_n(0,0) < 0.0){
                theta_V_n(0,0) = 0.01;
            };

        };

        theta_V_pi = theta_V_n;
    };

    //std::cout << theta_V_pi << std::endl;
    return theta_V_pi;
};

Eigen::MatrixXd
H::_RLS(Eigen::MatrixXd& theta, Eigen::MatrixXd& v, Eigen::Block<Eigen::MatrixXd>& s, int i, double c){

    //i - iteration if i = 0 - initialize B
    //for each theta_t:
    Eigen::MatrixXd B_n_1;

    if (i == 0){
        //identity matrix
        double e_B_0 = 0.0005;

        //B(n-1)
        B_n_1 = e_B_0 * Eigen::MatrixXd::Identity(param->ADP_BF_N, param->ADP_BF_N);

    }else{
        B_n_1 = B_n_1_RLS;
    };

    //container for basis functions
    Eigen::MatrixXd phi(1, param->ADP_BF_N);
    _bf(s,&phi);

    double e_n = c - (v.transpose()*theta)(0,0);

    double gamma_n = 1 + ((v.transpose()*B_n_1)*phi)(0,0);

    Eigen::MatrixXd B_n = B_n_1 - 1/gamma_n *((B_n_1*(phi*v.transpose()))*B_n_1);

    Eigen::MatrixXd theta_n = theta + 1/gamma_n * ((e_n*B_n_1)*phi);

    //    std::cout << 1/gamma_n * ((e_n*B_n_1)*phi) << std::endl;

    //    std::cout << theta_n << std::endl;

    B_n_1_RLS = B_n;

    return theta_n;

};

void
H::_opt_CS_s_V(Eigen::Block<Eigen::MatrixXd>& s0,  Eigen::MatrixXd& theta_x, Eigen::MatrixXd& theta_V, int N){

    //	N = 1;

	//gets matrix of thetas
	//prepare initial matrix
	MatrixXd m_s0 = MatrixXd::Zero(N, s0.cols());

	m_s0.rowwise() += s0.row(0);

	//allocate space to the matrix
	MatrixXd n_w;
	MatrixXd n_x;
	MatrixXd n_s = MatrixXd::Zero(N, s0.cols());
	MatrixXd n_c;
    MatrixXd v_bf_ret = MatrixXd::Zero(1, param->ADP_BF_N);
    MatrixXd n_v;

	//create matrix for the grid - small size here
	MatrixXd grid(3,3);

    create_decision_grid(grid,0);

    if (w.param->SIMULATION_MODE == "test"){
        Tools::mgrid_test(grid, &theta_x_all);

    } else{

        Tools::mgrid(grid, &theta_x_all);
    };

	//create random number generators for the implementation

	boost::normal_distribution<> nd_w1(s0(0,4), pow(s0(0,15),0.5));
	boost::normal_distribution<> nd_w2(s0(0,6), pow(s0(0,16),0.5));
	boost::normal_distribution<> nd_w3(s0(0,13), pow(s0(0,17),0.5));

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w1(w.rng, nd_w1);

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w2(w.rng, nd_w2);

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w3(w.rng, nd_w3);

    if (w.param->SIMULATION_MODE != "test"){

        //draw random variables
        //fast realization
        n_w = MatrixXd::Zero(N, 3 * param->T_MAX);
        for (int i = 0; i < N; i++){
            for(int j=0; j< n_w.cols(); j++){
                if (j%3 == 0){
                    n_w(i,j) = rng_w1();
                } else {
                    if (j%3 == 1){
                        n_w(i,j) = rng_w2();
                    }else{
                        n_w(i,j) = rng_w3();
                    };

                };
                n_w(i,j) = std::max(0.0, (double)n_w(i,j));
            };
        };
    };

	//matrix of all
	MatrixXd c_all(theta_x_all.cols(),1);

	for (int i = 0 ; i < theta_x_all.cols(); i++){
        if (w.param->SIMULATION_MODE == "test"){
            n_w = MatrixXd::Zero(N, 3 * param->T_MAX);
            for (int i = 0; i < N; i++){
                for(int j=0; j< n_w.cols(); j++){
                    if (j%3 == 0){
                        n_w(i,j) = Tools::get_normal(s0(0,4), pow(s0(0,15),0.5), w.myrng);
                    } else {
                        if (j%3 == 1){
                            n_w(i,j) = Tools::get_normal(s0(0,6), pow(s0(0,16),0.5), w.myrng);
                        }else{
                            n_w(i,j) = Tools::get_normal(s0(0,13), pow(s0(0,17),0.5), w.myrng);
                        };

                    };
                    n_w(i,j) = std::max(0.0, (double)n_w(i,j));
                };
            };
        };

		//call estimator of results of steps, given theta_x
		theta_x.row(0) = theta_x_all.col(i).transpose();
		n_x = MatrixXd::Zero(N, 3);
		n_s = m_s0;
		n_c = MatrixXd::Zero(N, 1);
        n_v = MatrixXd::Zero(N, 1);
		_step_opt_CS(theta_x, &n_w, &n_x, &n_s, &n_c, &n_v, theta_V, &v_bf_ret, 1);

		//after n_c
		//average over n runs
        c_all(i,0) = (n_c + param->BETA*n_v).mean();

	};

    //    std::cout << (n_c + param->BETA*n_v).mean() << std::endl;

	//find all max elements
	double max = c_all(0,0);
	std::list<int> i_max;
	for (int i=0; i<c_all.rows(); i++){
		if (c_all(i,0) == max){
			i_max.push_back(i);
		}else{
			if (c_all(i,0) > max){
				max = c_all(i,0);
				i_max.clear();
				i_max.push_back(i);
			};
		}
	};

	//if more than 1 max - randomly pick
	long theta_max_i;
	if (i_max.size()>1){
		boost::random::uniform_int_distribution<> i_max_dist(0, i_max.size()-1);
		theta_max_i = i_max_dist(w.rng);
	}else{
		theta_max_i = i_max.front();
	};

	theta_x.row(0) = theta_x_all.col(theta_max_i).transpose();

    //    std::cout << theta_x << std::endl;
};

void
H::_step_opt_CS(MatrixXd& theta_x,
                MatrixXd *w,
                MatrixXd *x,
                MatrixXd *s,
                MatrixXd *c,
                MatrixXd* v,
                MatrixXd& theta_v,
                MatrixXd* ret,
                int T){

	double l_t;
	double c_t;

	//zero vector
	auto s_zero = MatrixXd::Zero(s->rows(),1);
    Eigen::Block<Eigen::MatrixXd> s_block = s->block(0,0,1,s->cols());;

    //	std::cout <<"w" << *w << "\n";

	for (int i=0; i< T; i++){

		x->col(0) = theta_x(0,0) * s->col(1).array();
		x->col(1) = theta_x(0,1) * s->col(4).array();

		//labor market results
		//if wage is less than realized wage - get hired, otherwise zero
		s->col(11) = (w->col(i*3).array() >= x->col(1).array()).cast<double>().array() *x->col(0).array();
		s->col(12) = w->col(i*3);

		//made consumption good decision
		x->col(2) = theta_x(0,2) * s->col(0).array();

		//buy consumption goods , assume all goods are consumed in the previous
		//period, so no addition here
		for (int j=0; j<s->rows(); j++){
			if ((*w)(j,i*3 + 1) > 0.0){
				(*s)(j,3) = (*x)(j,2) / (*w)(j,i*3 + 1);

				//for speed calculate goal in this cycle
				//stores realization of a goal
				//as couldn't pass block need to calculate in place?

                //				(*c)(j,i) = _goal_t(&(s->block(1,s->cols(),j,0)));
			}else{
				(*s)(j,3) = 0.0;
			};

			if ((*s)(j,3) <= 0.0){
				c_t = -0.5;
//                c_t = 0.0;
			} else {
				c_t = (*s)(j,3);
			};

			l_t = (*s)(j,11);

			(*c)(j,i) = param->GOAL_T_theta(0,0) * log(1+c_t) + param->GOAL_T_theta(0,1) * (1-l_t);
//            (*c)(j,i) = param->GOAL_T_theta(0,0) * log(0.5+c_t) + param->GOAL_T_theta(0,1) * (1-l_t);

		};

		//account for money being spent
		s->col(0) = s->col(0).array() - w->col(i*3 + 1).array() * s->col(3).array();

		//labor payments
		s->col(0) = s->col(0).array() + s->col(11).array() * s->col(12).array();

		//dividend payment
		s->col(0) += w->col(i*3 + 2);

        //zero out labor contracts
        s->col(11) = s_zero;
        s->col(12) = s_zero;
        //zero out consumption goods
        s->col(3) = s_zero;

        //add value function estimation
        for (int j=0; j < s->rows(); j++){
            s_block  = s->block(j,0,1,s->cols());

            (*v)(j,i) = _V(s_block,ret, theta_V);
        };

	};

};

double
H::_c_theta_x(Eigen::MatrixXd& theta_x, Eigen::Block<Eigen::MatrixXd>& s0, Eigen::MatrixXd& theta_V){

    int N = 1;
    double _c = 0.0;

    //    std::cout << s0 << std::endl;

	//gets matrix of thetas
	//prepare initial matrix
	MatrixXd m_s0 = MatrixXd::Zero(N, s0.cols());

	m_s0.rowwise() += s0.row(0);

	//allocate space to the matrix
	MatrixXd n_w;
	MatrixXd n_x = MatrixXd::Zero(N, 3);
	MatrixXd n_s = MatrixXd::Zero(N, s0.cols());
    MatrixXd v_bf_ret = MatrixXd::Zero(1, param->ADP_BF_N);
	MatrixXd n_c = MatrixXd::Zero(N, 1);;
    MatrixXd n_v = MatrixXd::Zero(N, 1);;

	//create random number generators for the implementation

	boost::normal_distribution<> nd_w1(s0(0,4), pow(s0(0,15),0.5));
	boost::normal_distribution<> nd_w2(s0(0,6), pow(s0(0,16),0.5));
	boost::normal_distribution<> nd_w3(s0(0,13), pow(s0(0,17),0.5));

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w1(w.rng, nd_w1);

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w2(w.rng, nd_w2);

	boost::variate_generator<boost::mt19937&,
    boost::normal_distribution<>> rng_w3(w.rng, nd_w3);

    if (w.param->SIMULATION_MODE != "test"){

        //draw random variables
        //fast realization
        n_w = MatrixXd::Zero(N, 3 * param->T_MAX);
        for (int i = 0; i < N; i++){
            for(int j=0; j< n_w.cols(); j++){
                if (j%3 == 0){
                    n_w(i,j) = rng_w1();
                } else {
                    if (j%3 == 1){
                        n_w(i,j) = rng_w2();
                    }else{
                        n_w(i,j) = rng_w3();
                    };

                };
                n_w(i,j) = std::max(0.0, (double)n_w(i,j));
            };
        };
    };

    //redraw random in comparative with Python mode
    if (w.param->SIMULATION_MODE == "test"){
        n_w = MatrixXd::Zero(N, 3 * param->T_MAX);
        for (int i = 0; i < N; i++){
            for(int j=0; j< n_w.cols(); j++){
                if (j%3 == 0){
                    n_w(i,j) = Tools::get_normal(s0(0,4), pow(s0(0,15),0.5), w.myrng);
                } else {
                    if (j%3 == 1){
                        n_w(i,j) = Tools::get_normal(s0(0,6), pow(s0(0,16),0.5), w.myrng);
                    }else{
                        n_w(i,j) = Tools::get_normal(s0(0,13), pow(s0(0,17),0.5), w.myrng);
                    };

                };
                n_w(i,j) = std::max(0.0, (double)n_w(i,j));
            };
        };
    };

    //call estimator of results of steps, given theta_x
    n_s = m_s0;
    _step_opt_CS(theta_x, &n_w, &n_x, &n_s, &n_c, &n_v, theta_V, &v_bf_ret, 1);
    _c = (n_c + param->BETA*n_v).mean();

    s0.row(0) = n_s.row(0);

    //    std::cout << s0 << std::endl;

    return _c;

};

void
H::opt_QL(){
    //find best action for the state

    //find current bin for the state
    _s0();
    std::vector<double> s_bin = _place_state_QL(s0);
    s_t_bin = s_bin;
    std::vector<int> Q_s;

    //create list of all indexes that correspond to current state
    for (int i = 0; i < s_bin.size(); i++){
        for (int j = 0; j< Q.cols(); j++){
            if (_is_bin(j, i, s_bin)){
                Q_s.push_back(j);
            };

        };
    };

    //find index with max Q
    std::vector<int> Q_max;
    double max = Q(4,Q_s.at(0));

    for (auto j:Q_s){
        if (Q(4,j) > max){
            max = Q(4,j);
            Q_max.clear();
            Q_max.push_back(j);

        }else{
            if (Q(4,j) == max){
                Q_max.push_back(j);
            };
        };

    };

    boost::random::uniform_01<> _exp_dist;
    double exp_rate = _exp_dist(w.rng);

    boost::random::uniform_int_distribution<> Q_dist(0, Q.cols()-1);
    long theta_max_i;

    if (exp_rate < param->QL_experimental_rate){
        theta_max_i = Q_dist(w.rng);
    }else{
        //pick randomly best action
        if (Q_max.size()>1){
            boost::random::uniform_int_distribution<> Q_max_dist(0, Q_max.size()-1);
            theta_max_i = Q_max.at(Q_max_dist(w.rng));

        }else{
            theta_max_i = Q_max.front();
        };
    };

    theta_x_t_bin.push_back(theta_max_i);

    //transform from index to decision
    theta_x.row(0) = Q.block(s_bin.size(),theta_max_i,3,1).transpose();

};

bool
H::_is_bin(int j, int i, std::vector<double>& s_bin){
    bool is_bin = false;

    if (i < s_bin.size()){
        is_bin = (Q(i,j) == Q_bin.at(i).at(s_bin.at(i))) && _is_bin(j, i+1, s_bin);
        //        std::cout << (Q(i,j) == s_bin.at(i)) << std::endl;
        //        std::cout << _is_bin(j, i+1, s_bin) << std::endl;
    }else{
        is_bin = true;
    };

    return is_bin;
};

void
H::wm_update_Q(){
    //updates Q matrix
    //reward
    double R_t1 = account->g_t.at(account->g_t.size()-2);

    //new state
    _s0();
    std::vector<double> s_bin = _place_state_QL(s0);
    std::vector<int> Q_s;

    //create list of all indexes that correspond to current state
    for (int i = 0; i < s_bin.size(); i++){
        for (int j = 0; j< Q.cols(); j++){
            if (_is_bin(j, i+1, s_bin)){
                Q_s.push_back(j);
            };

        };
    };

    //find index with max Q
    std::vector<int> Q_max;
    double max = Q(4,Q_s.at(0));

    for (auto j:Q_s){
        if (Q(4,j) > max){
            max = Q(4,j);
            Q_max.clear();
            Q_max.push_back(j);

        }else{
            if (Q(4,j) == max){
                Q_max.push_back(j);
            };
        };

    };

    //pick best action
    long theta_max_i;
    long max_i;
    if (Q_max.size()>1){
        boost::random::uniform_int_distribution<> Q_max_dist(0, Q_max.size()-1);
        max_i = Q_max_dist(w.rng);
        theta_max_i = Q_max.at(max_i);
    }else{
        theta_max_i = Q_max.front();
    };

    //update Q value
    Q(4,theta_x_t_bin.back()) = Q(4,theta_x_t_bin.back())
    + Q(5,theta_x_t_bin.back())
    * (R_t1 + param->BETA*Q(4,theta_max_i) - Q(4,theta_x_t_bin.back()));
    //    std::cout << Q << std::endl;

};

void
H::_set_QL(){
    //setup QL matrix
    //create matrix for the grid - small size here
	Q_grid = Eigen::MatrixXd::Zero(6,3);

    //money and stock of goods on hand
    Q_grid(0,0) = 0.0;
    Q_grid(0,1) = 10.0;
    Q_grid(0,2) = ((Q_grid(0,1) - Q_grid(0,0))/2);

    create_decision_grid(Q_grid, 1);

    //Q factors
    Q_grid(4,0) = 0.0;
    Q_grid(4,1) = 0.1;
    Q_grid(4,2) = 1.0;

    Q_grid(5,0) = param->QL_L; //speed of learning
    Q_grid(5,1) = Q_grid(5,0) + 0.1;
    Q_grid(5,2) = Q_grid(5,0) + 1.0;

    if (w.param->SIMULATION_MODE == "test"){
        Tools::mgrid_test(Q_grid, &Q);

    } else{

        Tools::mgrid(Q_grid, &Q);
    };

    create_bin_values(Q_grid, Q_bin);

    //update Q to push to hiring all of them
    for (int j = 0; j < Q.cols(); j++){
        if (Q(1,j) == Q_bin.at(1).back()){
            Q(4,j) = 0.05;
        };
    };

    //    std::cout << Q << std::endl;

    //    Tools::print_vector(Q_bin);

};

void
H::create_bin_values(Eigen::MatrixXd& _grid, std::vector<std::vector<double>>& _bin){
    long N_rows = _grid.rows();
	int n_steps_i;
	double n_steps_i_int;

	double val;
    std::vector<double> values_temp;

    //create matrix of bin values
    //create list of values
	for (int i=0; i < N_rows; i++){
		//n_steps
		n_steps_i_int = (_grid(i,1) - _grid(i,0))/_grid(i,2);

		n_steps_i = static_cast<int>(floor(n_steps_i_int)) + 1;

		for (int j=0; j<n_steps_i; j++){
			val = _grid(i,0) + _grid(i,2)*j;
			values_temp.push_back(val);
		};

		_bin.push_back(values_temp);
		values_temp.clear();
	};

};

std::vector<double>
H::_place_state_QL(Eigen::MatrixXd& s){
    std::vector<double> s_bin;

    //pick first and compare
    //push values for bins ina  vector

    bool FLAG_PLACED = false;

    int j = Q_bin.at(0).size()-1;
    while ((j>=0) && !FLAG_PLACED){

        if (s(0,0) >= Q_bin.at(0).at(j)){
            s_bin.push_back(j);
            FLAG_PLACED = true;
        };
        j -= 1;
    };

    return s_bin;
};

void
H::create_decision_grid(Eigen::MatrixXd& grid, int begin_index){
    int i = begin_index;

    if (param->GRID == "small"){
		grid(i,0) = 0.0;
		grid(i,1) = 1.1;
		grid(i,2) = 1.0;
		grid(i+1,0) = 0.8;
		grid(i+1,1) = 1.25;
		grid(i+1,2) = 0.2;
		grid(i+2,0) = 0.0;
		grid(i+2,1) = 1.0;
		grid(i+2,2) = 0.5;
	};

    if (param->GRID == "big"){
		grid(i,0) = 0.0;
		grid(i,1) = 1.1;
		grid(i,2) = 1.0;
		grid(i+1,0) = 0.1;
		grid(i+1,1) = 2.1;
		grid(i+1,2) = 0.45;
		grid(i+2,0) = 0.0;
		grid(i+2,1) = 1.0;
		grid(i+2,2) = 0.5;

	};
    if (param->GRID == "test"){

        grid(i,0) = 1.0;
        grid(i,1) = 1.1;
        grid(i,2) = 1.0;

        grid(i+1,0) = 0.8;
        grid(i+1,1) = 1.2;
        grid(i+1,2) = 1.0;

        grid(i+2,0) = 0.5;
        grid(i+2,1) = 1.1;
        grid(i+2,2) = 1.0;

    };

};

void
H::_set_mRE(){
    //
    mRE_grid = Eigen::MatrixXd::Zero(4,3);
    create_decision_grid(mRE_grid, 0);
    mRE_grid(3,0) = 1.0;
    mRE_grid(3,1) = 1.1;
    mRE_grid(3,2) = 1.0;

    if (w.param->SIMULATION_MODE == "test"){
        Tools::mgrid_test(mRE_grid, &mRE);

    } else{

        Tools::mgrid(mRE_grid, &mRE);
    };

    create_bin_values(mRE_grid, mRE_bin);

    //update mRE to push to hiring all of them
    for (int j = 0; j < mRE.cols(); j++){
        if (mRE(0,j) == mRE_bin.at(0).back()){
            mRE(3,j) = 1.05;
        };
    };

};

void
H::opt_mRE(){
    //form matrix of probabilities
    double sum_prob = 0.0;

    for (int j=0; j<mRE.cols();j++){
        sum_prob += exp(mRE(3,j)/param->mRE_T);
    };

    std::vector<double> probs;

    for (int j=0; j<mRE.cols();j++){
        probs.push_back(exp(mRE(3,j)/param->mRE_T)/sum_prob);
    };

    //roll weighted dice
    boost::random::discrete_distribution<> dist(probs.begin(), probs.end());

    int theta_i = dist(w.rng);

    theta_x_t_bin.push_back(theta_i);

    //transform from index to decision
    theta_x.row(0) = mRE.block(0,theta_i,3,1).transpose();

};

void
H::wm_update_mRE(){
    //adjustment matrix
    Eigen::MatrixXd _E_re  = Eigen::MatrixXd::Zero(1, mRE.cols());

    //reward
    double R_t1 = account->g_t.at(account->g_t.size()-2);

    //adjustment
    _E_re = mRE.row(3) * (param->mRE_EPSILON/(mRE.cols()-1));

    //include reward
    _E_re(0,theta_x_t_bin.back()) = R_t1 * (1 - param->mRE_EPSILON);

    //update whole matrix
    mRE.row(3) = mRE.row(3) * (1 - param->mRE_PHI) + _E_re;

};

bool
H::_PS_accept_PO(MessagePSSendPayment* mes){
	return true;
};

ContractBDt0*
H::_PS_contract(){
	return ass_asCBDt0.front();
};


// (C) Copyright Renaud Detry   2007-2015.
// Distributed under the GNU General Public License and under the
// BSD 3-Clause License (See accompanying file LICENSE.txt).

/** @file */

#ifdef NUKLEI_HAS_PARTIAL_VIEW

#define CGAL_EIGEN3_ENABLED

#include <CGAL/Simple_cartesian.h>
#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/AABB_polyhedron_triangle_primitive.h>
#include <CGAL/property_map.h>
#include <CGAL/IO/read_off_points.h>
#include <CGAL/IO/read_xyz_points.h>
#include <CGAL/IO/write_xyz_points.h>
#include <CGAL/IO/Polyhedron_iostream.h>

#include <CGAL/squared_distance_3.h>
#include <CGAL/Point_with_normal_3.h>

#endif

#include <nuklei/KernelCollection.h>
#include <nuklei/ObservationIO.h>

#include <boost/shared_ptr.hpp>

#include <nuklei/KernelCollection.h>

#ifdef NUKLEI_HAS_PARTIAL_VIEW

typedef CGAL::Simple_cartesian<double> K;
typedef K::Point_3 Point;
typedef K::Plane_3 Plane;
typedef K::Vector_3 Vector;
typedef K::Segment_3 Segment;
typedef K::Line_3 Line;
typedef CGAL::Polyhedron_3<K> Polyhedron;
typedef CGAL::AABB_polyhedron_triangle_primitive<K,Polyhedron> Primitive;
typedef CGAL::AABB_traits<K, Primitive> Traits;
typedef CGAL::AABB_tree<Traits> Tree;
typedef Tree::Object_and_primitive_id Object_and_primitive_id;
typedef Tree::Primitive_id Primitive_id;

typedef CGAL::Point_with_normal_3<K> Point_with_normal;
typedef std::vector<Point_with_normal> PointList;

#endif

namespace nuklei {

  typedef std::vector< std::pair< Vector3, std::vector<int> > > viewcache_t;

#ifdef NUKLEI_HAS_PARTIAL_VIEW

  inline bool isVisible(const Vector3& wtarget,
                        const Vector3& wnormal,
                        const Vector3& wcamera,
                        const Polyhedron& poly,
                        const Tree& tree,
                        const coord_t& tolerance)
  {
    if (std::fabs(wnormal.SquaredLength()-1) < FLOATTOL)
    {
      Vector3 ctot = la::normalized(wtarget-wcamera);
      double dot = wnormal.Dot(ctot);
      if (std::acos(std::fabs(dot)) > (80./180*M_PI))
      {
        return false;
      }
    }

    Point camera(wcamera.X(), wcamera.Y(), wcamera.Z());
    Point target(wtarget.X(), wtarget.Y(), wtarget.Z());

    //double d = dot();
    Segment segment_query(camera,target);
    bool visible = true;

    std::list<Object_and_primitive_id> intersections;
    tree.all_intersections(segment_query, std::back_inserter(intersections));

    int self_intersect = 0;
    for (std::list<Object_and_primitive_id>::const_iterator intersection = intersections.begin();
         intersection != intersections.end(); ++intersection)
    {
      Object_and_primitive_id op = *intersection;
      CGAL::Object object = op.first;
      Point intersectionPoint;
      if(CGAL::assign(intersectionPoint,object))
      {
        double sq2 = squared_distance(intersectionPoint, target);
        if (sq2 < tolerance*tolerance) self_intersect++;
      }
      else
      {
        // This is very unlikely (p<<0.001)
        //NUKLEI_THROW("intersection is not a point!");
      }
    }

    int num_inter = intersections.size();
    if (self_intersect > 1)
      NUKLEI_LOG("Number of self-intersections (" << self_intersect << ") greater than 1");

    if (num_inter == 0)
      visible = true;
    else
    {
      if (num_inter - self_intersect > 0)
        visible = false;
      else
        visible = true;
    }
    if (num_inter > 1 && visible)
      NUKLEI_INFO("Strange visibility at" << target);

    return visible;

  }

  inline bool isVisible(const Vector3& wtarget,
                        const Vector3& wcamera,
                        const Polyhedron& poly,
                        const Tree& tree,
                        const coord_t& tolerance)
  {
    return isVisible(wtarget,
                     Vector3::ZERO,
                     wcamera,
                     poly,
                     tree,
                     tolerance);
  }

#endif

  bool KernelCollection::isVisibleFrom(const Vector3& p, const Vector3& viewpoint,
                                       const coord_t& tolerance) const
  {
    NUKLEI_TRACE_BEGIN();
#ifdef NUKLEI_HAS_PARTIAL_VIEW
    if (!deco_.has_key(MESH_KEY))
      NUKLEI_THROW("Undefined mesh. Call buildMesh() first.");
    if (!deco_.has_key(AABBTREE_KEY))
      NUKLEI_THROW("Undefined AABB tree. Call buildMesh() first.");

    return isVisible(p, viewpoint,
                     *deco_.get< boost::shared_ptr<Polyhedron> >(MESH_KEY),
                     *deco_.get< boost::shared_ptr<Tree> >(AABBTREE_KEY),
                     tolerance);
#else
    NUKLEI_THROW("This function requires the partial view build of Nuklei. See http://nuklei.sourceforge.net/doxygen/group__install.html");
#endif
    NUKLEI_TRACE_END();
  }

  bool KernelCollection::isVisibleFrom(const kernel::r3xs2p& p, const Vector3& viewpoint,
                                       const coord_t& tolerance) const
  {
    NUKLEI_TRACE_BEGIN();
#ifdef NUKLEI_HAS_PARTIAL_VIEW
    if (!deco_.has_key(MESH_KEY))
      NUKLEI_THROW("Undefined mesh. Call buildMesh() first.");
    if (!deco_.has_key(AABBTREE_KEY))
      NUKLEI_THROW("Undefined AABB tree. Call buildMesh() first.");

    return isVisible(p.loc_, p.dir_, viewpoint,
                     *deco_.get< boost::shared_ptr<Polyhedron> >(MESH_KEY),
                     *deco_.get< boost::shared_ptr<Tree> >(AABBTREE_KEY),
                     tolerance);
#else
    NUKLEI_THROW("This function requires the partial view build of Nuklei. See http://nuklei.sourceforge.net/doxygen/group__install.html");
#endif
    NUKLEI_TRACE_END();
  }

  template<typename C>
  C KernelCollection::partialView(const Vector3& viewpoint,
                                  const coord_t& tolerance,
                                  const bool useViewcache,
                                  const bool useRayToSurfacenormalAngle) const
  {
    NUKLEI_TRACE_BEGIN();

    C index_collection;

    if (!useViewcache)
    {
#ifdef NUKLEI_HAS_PARTIAL_VIEW
      if (!deco_.has_key(MESH_KEY))
        NUKLEI_THROW("Undefined mesh. Call buildMesh() first.");
      if (!deco_.has_key(AABBTREE_KEY))
        NUKLEI_THROW("Undefined AABB tree. Call buildMesh() first.");

      const Polyhedron& poly = *deco_.get< boost::shared_ptr<Polyhedron> >(MESH_KEY);
      const Tree& tree = *deco_.get< boost::shared_ptr<Tree> >(AABBTREE_KEY);

      for (const_iterator v = begin(); v != end(); ++v)
      {
        Vector3 p = v->getLoc();
        Vector3 normal = Vector3::ZERO;
        if (useRayToSurfacenormalAngle)
          normal = kernel::r3xs2p(*v).dir_;
        if (isVisible(p, normal, viewpoint, poly, tree, tolerance))
          index_collection.push_back(std::distance(begin(), v));
      }
#else
      NUKLEI_THROW("This function requires the partial view build of Nuklei. See http://nuklei.sourceforge.net/doxygen/group__install.html");
#endif
    }
    else
    {
      if (!deco_.has_key(VIEWCACHE_KEY))
        NUKLEI_THROW("Undefined view cache. Call buildPartialViewCache() first.");

      const viewcache_t &viewIndex = *deco_.get< boost::shared_ptr<viewcache_t> >(VIEWCACHE_KEY);

      double minDist = 1e6;
      viewcache_t::const_iterator closest = as_const(viewIndex).begin();
      for (viewcache_t::const_iterator oo = as_const(viewIndex).begin(); oo != viewIndex.end(); ++oo)
      {
        double tmp = (viewpoint-oo->first).SquaredLength();
        if (tmp < minDist*minDist)
        {
          minDist = tmp;
          closest = oo;
        }
      }
      for (std::vector<int>::const_iterator i = closest->second.begin(); i != closest->second.end(); ++i)
        index_collection.push_back(*i);
    }
    return index_collection;
    NUKLEI_TRACE_END();
  }

  std::vector<int> KernelCollection::partialView(const Vector3& viewpoint,
                                                 const coord_t& tolerance,
                                                 const bool useViewcache,
                                                 const bool useRayToSurfacenormalAngle) const
  {
    NUKLEI_TRACE_BEGIN();
#ifdef NUKLEI_HAS_PARTIAL_VIEW
    return partialView< std::vector<int> >(viewpoint, tolerance, useViewcache, useRayToSurfacenormalAngle);
#else
    NUKLEI_THROW("This function requires the partial view build of Nuklei. See http://nuklei.sourceforge.net/doxygen/group__install.html");
#endif
    NUKLEI_TRACE_END();
  }

  KernelCollection::const_partialview_iterator KernelCollection::partialViewBegin(const Vector3& viewpoint,
                                                                                const coord_t& tolerance,
                                                                                const bool useViewcache,
                                                                                const bool useRayToSurfacenormalAngle) const
  {
    NUKLEI_TRACE_BEGIN();
#ifdef NUKLEI_HAS_PARTIAL_VIEW

    typedef const_partialview_iterator::index_container
    index_container;
    typedef const_partialview_iterator::index_container_ptr
    index_container_ptr;
    typedef const_partialview_iterator::index_t
    index_t;

    index_container_ptr index_collection(new index_container);
    *index_collection = partialView< index_container >(viewpoint, tolerance, useViewcache, useRayToSurfacenormalAngle);

    return const_partialview_iterator(begin(), index_collection);
#else
    NUKLEI_THROW("This function requires the partial view build of Nuklei. See http://nuklei.sourceforge.net/doxygen/group__install.html");
#endif
    NUKLEI_TRACE_END();
  }

  void KernelCollection::buildPartialViewCache(const double meshTol, const bool useRayToSurfacenormalAngle)
  {
    NUKLEI_TRACE_BEGIN();
#ifdef NUKLEI_HAS_PARTIAL_VIEW
    Vector3 mean = as_const(*this).moments()->getLoc();
    double stdev = as_const(*this).moments()->getLocH();

    std::vector< Vector3 > keys;
    for (int o = 0; o < 10000; ++o)
    {
      Vector3 key = Random::uniformDirection3d();
      keys.push_back(key);
    }

    {
      double minDist2 = .15;
      std::vector< Vector3 > tmp = keys;
      keys.clear();
      for (std::vector<Vector3>::const_iterator
           i = tmp.begin();
           i != tmp.end(); ++i)
      {
        bool cnt = false;
        Vector3 iLoc = (*i);
        for (std::vector<Vector3>::const_iterator
             j = keys.begin();
             j != keys.end(); ++j)
        {
          if ( (iLoc - *j).SquaredLength() < minDist2*minDist2 )
          {
            cnt = true;
            break;
          }
        }
        if (!cnt)
        {
          keys.push_back(iLoc);
        }
      }
    }

    boost::shared_ptr<viewcache_t> viewIndex(new viewcache_t());

    for (unsigned int o = 0; o < keys.size(); ++o)
    {
      Vector3 key = keys.at(o);
      Vector3 vp = mean + key*stdev*20;
      std::vector<int> vi = as_const(*this).partialView(vp, meshTol, false, useRayToSurfacenormalAngle);
#if 0
      // debug - delete when code is considered stable
      KernelCollection v;
      for (std::vector<int>::iterator i = vi.begin(); i != vi.end(); ++i)
        v.add(as_const(*this).at(*i));
      v.computeKernelStatistics();
      kernel::base::ptr k = v.randomKernel().create();
      k->setLoc(vp);
      v.add(*k);
      v.computeKernelStatistics();
#endif
      viewIndex->push_back(std::make_pair(key, vi));
    }

    if (deco_.has_key(VIEWCACHE_KEY)) deco_.erase(VIEWCACHE_KEY);
    deco_.insert(VIEWCACHE_KEY, viewIndex);

#if 0
    // debug - delete when code is considered stable

      for (viewcache_t::iterator o = viewIndex->begin(); o != viewIndex->end(); ++o)
      {
        double d = 1e6;
        KernelCollection near;

        for (viewcache_t::iterator oo = viewIndex->begin(); oo != viewIndex->end(); ++oo)
        {
          if (oo == o) continue;
          double dd = (o->first-oo->first).Length();
          if (dd < d) {
            d = dd;
            near.clear();
            for (std::vector<int>::iterator j = oo->second.begin(); j != oo->second.end(); ++j)
              near.add(as_const(*this).at(*j));
            near.computeKernelStatistics();
            if (near.size() == 0) continue;
            kernel::base::ptr k = near.randomKernel().create();
            k->setLoc(mean + oo->first*stdev*20);
            near.add(*k);
            near.computeKernelStatistics();
          }
        }
        for (KernelCollection::iterator i = near.begin(); i != near.end(); ++i)
        {
          ColorDescriptor cd;
          cd.setColor(RGBColor(1, 0, 0));
          i->setDescriptor(cd);
        }
        for (std::vector<int>::iterator j = o->second.begin(); j != o->second.end(); ++j)
        {
          near.add(as_const(*this).at(*j));
          near.back().setLoc(near.back().getLoc()+Vector3(0.001, 0, 0));
        }
        near.computeKernelStatistics();
        if (near.size() == 0) continue;
        kernel::base::ptr k = near.randomKernel().create();
        k->setLoc(mean + o->first*stdev*20);
        near.add(*k);
        near.computeKernelStatistics();
        writeObservations("/tmp/v/" + stringify(std::distance(viewIndex->begin(), o)), near, Observation::SERIAL);
      }
#endif

#else
    NUKLEI_THROW("This function requires the partial view build of Nuklei. See http://nuklei.sourceforge.net/doxygen/group__install.html");
#endif
    NUKLEI_TRACE_END();

  }

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2019 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_DISTANCE_MEASURE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_DISTANCE_MEASURE_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

#include <cmath>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename T>
struct distance_measure
{
    T measure;

    distance_measure()
        : measure(T())
    {}

    bool is_small() const { return true; }
    bool is_zero() const { return true; }
    bool is_positive() const { return false; }
    bool is_negative() const { return false; }
};

template <typename T>
struct distance_measure_floating
{
    T measure;

    distance_measure_floating()
        : measure(T())
    {}

    // Returns true if the distance measure is small.
    // This is an arbitrary boundary, to enable some behaviour
    // (for example include or exclude turns), which are checked later
    // with other conditions.
    bool is_small() const { return std::abs(measure) < 1.0e-3; }

    // Returns true if the distance measure is absolutely zero
    bool is_zero() const { return measure == 0.0; }

    // Returns true if the distance measure is positive. Distance measure
    // algorithm returns positive value if it is located on the left side.
    bool is_positive() const { return measure > 0.0; }

    // Returns true if the distance measure is negative. Distance measure
    // algorithm returns negative value if it is located on the right side.
    bool is_negative() const { return measure < 0.0; }
};

template <>
struct distance_measure<long double>
    : public distance_measure_floating<long double> {};

template <>
struct distance_measure<double>
    : public distance_measure_floating<double> {};

template <>
struct distance_measure<float>
    : public distance_measure_floating<float> {};

} // detail

namespace detail_dispatch
{

// TODO: this is effectively a strategy, but for internal usage.
// It might be moved to the strategies folder.

template <typename CalculationType, typename CsTag>
struct get_distance_measure
        : not_implemented<CsTag>
{};

template <typename CalculationType>
struct get_distance_measure<CalculationType, cartesian_tag>
{
    typedef detail::distance_measure<CalculationType> result_type;

    template <typename SegmentPoint, typename Point>
    static result_type apply(SegmentPoint const& p1, SegmentPoint const& p2,
                             Point const& p)
    {
        typedef CalculationType ct;

        // Construct a line in general form (ax + by + c = 0),
        // (will be replaced by a general_form structure in next PR)
        ct const x1 = geometry::get<0>(p1);
        ct const y1 = geometry::get<1>(p1);
        ct const x2 = geometry::get<0>(p2);
        ct const y2 = geometry::get<1>(p2);
        ct const a = y1 - y2;
        ct const b = x2 - x1;
        ct const c = -a * x1 - b * y1;

        // Returns a distance measure
        // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_an_equation
        // dividing by sqrt(a*a+b*b) is not necessary for this distance measure,
        // it is not a real distance and purpose is to detect small differences
        // in collinearity
        result_type result;
        result.measure = a * geometry::get<0>(p) + b * geometry::get<1>(p) + c;

        return result;
    }
};

template <typename CalculationType>
struct get_distance_measure<CalculationType, spherical_tag>
{
    typedef detail::distance_measure<CalculationType> result_type;

    template <typename SegmentPoint, typename Point>
    static result_type apply(SegmentPoint const& , SegmentPoint const& ,
                             Point const& )
    {
        // TODO, optional
        result_type result;
        return result;
    }
};

template <typename CalculationType>
struct get_distance_measure<CalculationType, geographic_tag>
        : get_distance_measure<CalculationType, spherical_tag> {};

} // namespace detail_dispatch

namespace detail
{

// Returns a (often very tiny) value to indicate its side, and distance,
// 0 (absolutely 0, not even an epsilon) means collinear. Like side,
// a negative means that p is to the right of p1-p2. And a positive value
// means that p is to the left of p1-p2.

template <typename cs_tag, typename SegmentPoint, typename Point>
static distance_measure<typename select_coordinate_type<SegmentPoint, Point>::type>
get_distance_measure(SegmentPoint const& p1, SegmentPoint const& p2, Point const& p)
{
    return detail_dispatch::get_distance_measure
            <
                typename select_coordinate_type<SegmentPoint, Point>::type,
                cs_tag
            >::apply(p1, p2, p);

}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_DISTANCE_MEASURE_HPP