/*
 [auto_generated]
 boost/numeric/odeint/stepper/runge_kutta4_classic.hpp

 [begin_description]
 Implementation for the classical Runge Kutta stepper.
 [end_description]

 Copyright 2010-2013 Karsten Ahnert
 Copyright 2010-2013 Mario Mulansky
 Copyright 2012 Christoph Koke

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA4_CLASSIC_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA4_CLASSIC_HPP_INCLUDED

#include <boost/numeric/odeint/stepper/base/explicit_stepper_base.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class runge_kutta4_classic
: public explicit_stepper_base<
  runge_kutta4_classic< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
  4 , State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
class runge_kutta4_classic : public explicit_stepper_base
#endif
{

public :

    #ifndef DOXYGEN_SKIP
    typedef explicit_stepper_base<
    runge_kutta4_classic< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
    4 , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_base_type;
    #else
    typedef explicit_stepper_base< runge_kutta4_classic< ... > , ... > stepper_base_type;
    #endif

    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;

    #ifndef DOXYGEN_SKIP
    typedef typename stepper_base_type::stepper_type stepper_type;
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    #endif // DOXYGEN_SKIP

    runge_kutta4_classic( const algebra_type &algebra = algebra_type() ) : stepper_base_type( algebra )
    { }

    template< class System , class StateIn , class DerivIn , class StateOut >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
    {
        // ToDo : check if size of in,dxdt,out are equal?

        static const value_type val1 = static_cast< value_type >( 1 );

        m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );

        typename odeint::unwrap_reference< System >::type &sys = system;

        const time_type dh = dt / static_cast< value_type >( 2 );
        const time_type th = t + dh;

        // dt * dxdt = k1
        // m_x_tmp = x + dh*dxdt
        stepper_base_type::m_algebra.for_each3( m_x_tmp.m_v , in , dxdt ,
                typename operations_type::template scale_sum2< value_type , time_type >( val1 , dh ) );

        // dt * m_dxt = k2
        sys( m_x_tmp.m_v , m_dxt.m_v , th );

        // m_x_tmp = x + dh*m_dxt
        stepper_base_type::m_algebra.for_each3( m_x_tmp.m_v , in , m_dxt.m_v ,
                typename operations_type::template scale_sum2< value_type , time_type >( val1 , dh ) );

        // dt * m_dxm = k3
        sys( m_x_tmp.m_v , m_dxm.m_v , th );
        //m_x_tmp = x + dt*m_dxm
        stepper_base_type::m_algebra.for_each3( m_x_tmp.m_v , in , m_dxm.m_v ,
                typename operations_type::template scale_sum2< value_type , time_type >( val1 , dt ) );

        // dt * m_dxh = k4
        sys( m_x_tmp.m_v , m_dxh.m_v , t + dt );

        //x += dt/6 * ( m_dxdt + m_dxt + val2*m_dxm )
        time_type dt6 = dt / static_cast< value_type >( 6 );
        time_type dt3 = dt / static_cast< value_type >( 3 );
        stepper_base_type::m_algebra.for_each6( out , in , dxdt , m_dxt.m_v , m_dxm.m_v , m_dxh.m_v ,
                                             typename operations_type::template scale_sum5< value_type , time_type , time_type , time_type , time_type >( 1.0 , dt6 , dt3 , dt3 , dt6 ) );

        // x += dt/6 * m_dxdt + dt/3 * m_dxt )
        // stepper_base_type::m_algebra.for_each4( out , in , dxdt , m_dxt.m_v ,
        //                                         typename operations_type::template scale_sum3< value_type , time_type , time_type >( 1.0 , dt6 , dt3 ) );
        // // x += dt/3 * m_dxm + dt/6 * m_dxh )
        // stepper_base_type::m_algebra.for_each4( out , out , m_dxm.m_v , m_dxh.m_v ,
        //                                         typename operations_type::template scale_sum3< value_type , time_type , time_type >( 1.0 , dt3 , dt6 ) );

    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_impl( x );
        stepper_base_type::adjust_size( x );
    }

private:

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_x_tmp , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_dxm , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_dxt , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_dxh , x , typename is_resizeable<deriv_type>::type() );
        return resized;
    }

    resizer_type m_resizer;

    wrapped_deriv_type m_dxt;
    wrapped_deriv_type m_dxm;
    wrapped_deriv_type m_dxh;
    wrapped_state_type m_x_tmp;

};

/********* DOXYGEN *********/

/**
 * \class runge_kutta4_classic
 * \brief The classical Runge-Kutta stepper of fourth order.
 *
 * The Runge-Kutta method of fourth order is one standard method for
 * solving ordinary differential equations and is widely used, see also
 * en.wikipedia.org/wiki/Runge-Kutta_methods
 * The method is explicit and fulfills the Stepper concept. Step size control
 * or continuous output are not provided.  This class implements the method directly, hence the
 * generic Runge-Kutta algorithm is not used.
 *
 * This class derives from explicit_stepper_base and inherits its interface via
 * CRTP (current recurring template pattern). For more details see
 * explicit_stepper_base.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn runge_kutta4_classic::runge_kutta4_classic( const algebra_type &algebra )
     * \brief Constructs the runge_kutta4_classic class. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

    /**
     * \fn runge_kutta4_classic::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
     * \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
     * The result is updated out of place, hence the input is in `in` and the output in `out`.
     * Access to this step functionality is provided by explicit_stepper_base and
     * `do_step_impl` should not be called directly.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt The derivative of x at t.
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dt The step size.
     */

    /**
     * \fn runge_kutta4_classic::adjust_size( const StateType &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA4_CLASSIC_HPP_INCLUDED


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * Redistributions in binary form must reproduce the above copyright        *
 *   notice, this list of conditions and the following disclaimer in the      *
 *   documentation and/or other materials provided with the distribution.     *
 * * Neither the name of ANU 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 ANU OR THE CONTRIBUTORS BE LIABLE        *
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
 * SUCH DAMAGE.                                                               *
 ******************************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <iomanip>
#include <limits.h>
#include <Eigen/Eigen>
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/absolute_pose/NoncentralAbsoluteMultiAdapter.hpp>
#include <opengv/sac/MultiRansac.hpp>
#include <opengv/sac_problems/absolute_pose/MultiNoncentralAbsolutePoseSacProblem.hpp>
#include <sstream>
#include <fstream>

#include "random_generators.hpp"
#include "experiment_helpers.hpp"
#include "time_measurement.hpp"

using namespace std;
using namespace Eigen;
using namespace opengv;

int main( int argc, char** argv )
{
  //initialize random seed
  initializeRandomSeed();

  //set experiment parameters
  double noise = 0.5;
  double outlierFraction = 0.1;
  size_t pointsPerCam = 25;
  int numberCameras = 4;

  //create a random viewpoint pose
  translation_t position = generateRandomTranslation(2.0);
  rotation_t rotation = generateRandomRotation(0.5);

  //create a random camera-system
  translations_t camOffsets;
  rotations_t camRotations;
  generateRandomCameraSystem( numberCameras, camOffsets, camRotations );

  //derive correspondences based on random point-cloud
  std::vector<std::shared_ptr<points_t> > multiPoints;
  std::vector<std::shared_ptr<bearingVectors_t> > multiBearingVectors;
  std::vector< std::shared_ptr<Eigen::MatrixXd> > gt;
  generateMulti2D3DCorrespondences(
      position, rotation, camOffsets, camRotations,
      pointsPerCam, noise, outlierFraction,
      multiPoints, multiBearingVectors, gt );

  //print the experiment characteristics
  printExperimentCharacteristics(
      position, rotation, noise, outlierFraction );

  //create a non-central absolute adapter
  absolute_pose::NoncentralAbsoluteMultiAdapter adapter(
      multiBearingVectors,
      multiPoints,
      camOffsets,
      camRotations );

  //Create a AbsolutePoseSacProblem and Ransac
  //The method is set to GP3P
  sac::MultiRansac<
      sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> ransac;
  std::shared_ptr<
      sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> absposeproblem_ptr(
      new sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(
      adapter ));

  ransac.sac_model_ = absposeproblem_ptr;
  ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
  ransac.max_iterations_ = 50;

  //Run the experiment
  struct timeval tic;
  struct timeval toc;
  gettimeofday( &tic, 0 );
  ransac.computeModel();
  gettimeofday( &toc, 0 );
  double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));

  //print the results
  std::cout << "the ransac results is: " << std::endl;
  std::cout << ransac.model_coefficients_ << std::endl << std::endl;
  std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
  std::cout << ransac_time << " seconds" << std::endl << std::endl;
  size_t numberInliers = 0;
  for(size_t i = 0; i < ransac.inliers_.size(); i++)
    numberInliers += ransac.inliers_[i].size();
  std::cout << "the number of inliers is: " << numberInliers;
  std::cout << std::endl << std::endl;
  std::cout << "the found inliers are: " << std::endl;
  for(size_t i = 0; i < ransac.inliers_.size(); i++)
  {
    for(size_t j = 0; j < ransac.inliers_[i].size(); j++)
      std::cout << ransac.inliers_[i][j] << " ";
  }
  std::cout << std::endl << std::endl;
}


/*
 * function.cpp
 *
 */

#include <list>
#include <map>
#include <random>
#include <iostream>
#include <chrono>
#include <math.h>
#include <boost/pool/object_pool.hpp>
#include <sstream>

#include "function.h"

using namespace std;

int func_id = 0;

PVariable variable_construct_for_function(Function *f, int rows, int cols) {
    PVariable r = PVariable(variable_construct(rows, cols), variable_destroy);
    r->creator = f;

    return r;
}

// Function class //////////////////////////////////////////////////////////////
Function::Function(){
    name = "Function";
    this->id = func_id;
    func_id += 1;
    count_function += 1;
}

Function::~Function(){
    init();
    count_function--;

}

void Function::init() {
    inputs.clear();
    outputs.clear();
}

PVariable Function::forward(vector<PVariable> &inputs, vector<PVariable > &outputs) {
    return NULL;
}

void Function::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs) {
    //TODO
}

PVariable Function::forward(PVariable v){

    v->forward_count++;

    inputs.push_back(v);
    PVariable r = forward(inputs, outputs);

    return r;
}

PVariable Function::forward(PVariable v1, PVariable v2){

    v1->forward_count++;
    v2->forward_count++;

    inputs.push_back(v1);
    inputs.push_back(v2);
    PVariable r = forward(inputs, outputs);

    return r;
}

PVariable Function::forward(PVariable v1, PVariable v2, PVariable v3){

    v1->forward_count++;
    v2->forward_count++;
    v3->forward_count++;

    inputs.push_back(v1);
    inputs.push_back(v2);
    inputs.push_back(v3);
    PVariable r = forward(inputs, outputs);

    return r;
}

PVariable Function::forward(PVariable v1, PVariable v2, PVariable v3, PVariable v4){
    v1->forward_count++;
    v2->forward_count++;

    inputs.push_back(v1);
    inputs.push_back(v2);
    inputs.push_back(v3);
    inputs.push_back(v4);
    PVariable r = forward(inputs, outputs);

    return r;
}

PVariable Function::forward(PVariable v1, PVariable v2, PVariable v3, PVariable v4,
                            PVariable v5, PVariable v6, PVariable v7, PVariable v8,
                            PVariable v9, PVariable v10, PVariable v11, PVariable v12
){
    v1->forward_count++;
    v2->forward_count++;

    inputs.push_back(v1);
    inputs.push_back(v2);
    inputs.push_back(v3);
    inputs.push_back(v4);
    inputs.push_back(v5);
    inputs.push_back(v6);
    inputs.push_back(v7);
    inputs.push_back(v8);
    inputs.push_back(v9);
    inputs.push_back(v10);
    inputs.push_back(v11);
    inputs.push_back(v12);
    PVariable r = forward(inputs, outputs);

    return r;
}

void Function::backward(cuMat &p_grad){
    backward(p_grad, inputs, outputs);
}

void Function::clip_grad(Variable *v){
    float clip_grad_threshold = 5.0;
    float sq = v->grad.l2();
    float rate = clip_grad_threshold/sq;
    if (rate < 1.){
        v->grad.mul(rate, v->grad);
    }
}

void Function::reset_state(){}

FunctionPlus::FunctionPlus() : Function() {
    name = "FunctionPlus";
}

PVariable FunctionPlus::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v1 = inputs.at(0);
    PVariable v2 = inputs.at(1);

    PVariable r = variable_construct_for_function(this, v1->data.rows, v1->data.cols);

    v1->data.plus(v2->data, r->data);

    return r;
}
void FunctionPlus::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v1 = inputs.at(0);
    PVariable v2 = inputs.at(1);
    //v1->grad += p_grad*1.0;
    //v2->grad += p_grad*1.0;
    if (v1->isGetGrad) p_grad.mul_plus(1.0, v1->grad);
    if (v2->isGetGrad) p_grad.mul_plus(1.0, v2->grad);
}

FunctionMinus::FunctionMinus() : Function() {
    name = "FunctionMinus";
}
PVariable FunctionMinus::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v1 = inputs.at(0);
    PVariable v2 = inputs.at(1);

    PVariable r = variable_construct_for_function(this, v1->data.rows, v1->data.cols);

    outputs.push_back(r);

    v1->data.minus(v2->data, r->data);

    return r;

}
void FunctionMinus::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){
    PVariable v1 = inputs.at(0);
    PVariable v2 = inputs.at(1);
    //v1->grad += p_grad*1.0;
    //v2->grad += p_grad*(-1.0);
    if (v1->isGetGrad) p_grad.mul_plus(1.0, v1->grad);
    if (v2->isGetGrad) p_grad.mul_plus(-1.0, v2->grad);
}

FunctionMul::FunctionMul() : Function() {
    name = "FunctionMul";
}
PVariable FunctionMul::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v1 = inputs.at(0);
    PVariable v2 = inputs.at(1);

    PVariable r = variable_construct_for_function(this, v1->data.rows, v1->data.cols);

    outputs.push_back(r);
    v1->data.mul(v2->data, r->data);

    return r;

}
void FunctionMul::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v1 = inputs.at(0);
    PVariable v2 = inputs.at(1);

    //v1->grad += p_grad * v2->data;
    //v2->grad += p_grad * v1->data;
    if (v1->isGetGrad) p_grad.mul_plus(v2->data, v1->grad, 1.0, 1.0);
    if (v2->isGetGrad) p_grad.mul_plus(v1->data, v2->grad, 1.0, 1.0);
}

FunctionInverse::FunctionInverse() : Function() {
    name = "FunctionInverse";
}
PVariable FunctionInverse::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v = inputs.at(0);

    PVariable r = variable_construct_for_function(this, v->data.rows, v->data.cols);

    outputs.push_back(r);
    v->data.inverse(r->data);

    return r;

}
void FunctionInverse::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v = inputs.at(0);

    if (v->isGetGrad) v->grad += p_grad * v->data.inverse_d();
}

FunctionSqrt::FunctionSqrt() : Function() {
    name = "FunctionSqrt";
}
PVariable FunctionSqrt::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v = inputs.at(0);

    PVariable r = variable_construct_for_function(this, v->data.rows, v->data.cols);

    outputs.push_back(r);
    v->data.sqrt(r->data, 1e-8);

    return r;

}
void FunctionSqrt::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v = inputs.at(0);

    if (v->isGetGrad) v->grad += p_grad * v->data.sqrt_d();
}

FunctionSin::FunctionSin() : Function() { }
PVariable FunctionSin::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v1 = inputs.at(0);

    PVariable r;
    r = PVariable(new Variable(this, v1->data.rows, v1->data.cols));
    outputs.push_back(r);

    v1->data.sin(r->data);
    return r;
}
void FunctionSin::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){
    PVariable v1 = inputs.at(0);

    if (rr.get() == NULL) rr = PVariable(new Variable(this, v1->data.rows, v1->data.cols));
    v1->data.cos(rr->data);
    if (v1->isGetGrad) v1->grad += p_grad * rr->data;
}

FunctionCos::FunctionCos() : Function() { }
PVariable FunctionCos::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v1 = inputs.at(0);

    PVariable r;
    r = PVariable(new Variable(this, v1->data.rows, v1->data.cols));
    outputs.push_back(r);
    v1->data.cos(r->data);
    return r;

}
void FunctionCos::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){
    PVariable v1 = inputs.at(0);

    if (rr.get() == NULL) rr = PVariable(new Variable(this, v1->data.rows, v1->data.cols));
    v1->data.sin(rr->data);
    if (v1->isGetGrad) v1->grad += p_grad * rr->data * (-1.0);
}

FunctionLog::FunctionLog() : Function() {}
PVariable FunctionLog::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable v1 = inputs.at(0);

    PVariable r;
    r = PVariable(new Variable(this, v1->data.rows, v1->data.cols));
    outputs.push_back(r);
    v1->data.log(r->data, 0);
    return r;

}
void FunctionLog::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){
    PVariable v1 = inputs.at(0);

    if (v1->isGetGrad) v1->grad += p_grad * 1.0/v1->data;
}

FunctionLinear::FunctionLinear() : Function() {
    name = "FunctionLinear";
}
FunctionLinear::FunctionLinear(Variable *w, Variable *b,  bool isTranspose) : Function() {
    name = "FunctionLinear";
    this->w  = w;
    this->b = b;
    this->isTranspose = isTranspose;
}
FunctionLinear::FunctionLinear(Variable *w,  bool isTranspose) : Function() {
    name = "FunctionLinear";
    noBias = true;
    this->w  = w;
    this->isTranspose = isTranspose;

}
FunctionLinear::FunctionLinear(int output_size, int input_size) : Function() {
    name = "FunctionLinear";

    this->w = new Variable(output_size, input_size);
    this->b = new Variable(output_size, 1);
    this->w->randoms(0., sqrt((1./(float)input_size)));

}

FunctionLinear::FunctionLinear(int output_size, int input_size, bool no_bias) : Function() {
    name = "FunctionLinear";

    noBias = no_bias;

    this->w = new Variable(output_size, input_size);
    this->w->randoms(0., sqrt((1./(float)input_size)));

    if (!noBias){
        this->b = new Variable(output_size, 1);
    }
}

void FunctionLinear::toHostArray(){
    i1.toHostArray();
    w->data.toHostArray();
    w->grad.toHostArray();
    if (!noBias){
        b->data.toHostArray();
        b->grad.toHostArray();
    }
}
void FunctionLinear::fromHostArray(){
    i1.fromHostArray();
    w->data.fromHostArray();
    w->grad.fromHostArray();
    if (!noBias){
        b->data.fromHostArray();
        b->grad.fromHostArray();
    }
}

PVariable FunctionLinear::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    int w_size = w->data.rows;
    if (isTranspose) w_size = w->data.cols;

    PVariable r = PVariable(new Variable(this, w_size, x->data.cols));

    if (i1.cols == 0 || i1.cols != x->data.cols){
        i1 = cuMat(1, x->data.cols);
        i1.ones();
    }

    if (!noBias) b->data.dot(i1, r->data);
    if (!isTranspose) w->data.dot_plus(x->data, r->data);
    else w->data.transpose().dot_plus(x->data, r->data);
    //r->data = w->data.dot(x->data) + b->data.dot(i1);

    return r;
}
void FunctionLinear::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    if (x->isGetGrad){
        if (!isTranspose) w->data.transpose_dot_plus(p_grad, x->grad);
        else w->data.transpose().transpose_dot_plus(p_grad, x->grad);
    }
    //x->grad += w->data.transpose().dot(p_grad);

    if (!isTranspose) p_grad.dot_transpose_plus(x->data, w->grad);
    else {
        cuMat tmpwg = w->grad.transpose();
        tmpwg *= 0;
        p_grad.dot_transpose_plus(x->data, tmpwg);
        w->grad += tmpwg.transpose();
    }
    //w->grad += p_grad.dot(x->data.transpose());

    if (!noBias){
        p_grad.dot_transpose_plus(i1, b->grad);
    }
    //b->grad += p_grad.dot(i1.transpose());
}

FunctionSparseLinear::FunctionSparseLinear() : Function() {
    name = "FunctionSparseLinear";
}

FunctionSparseLinear::FunctionSparseLinear(Variable *w, Variable *b, float beta, float p, Variable *ph) : Function() {
    name = "FunctionSparseLinear";
    this->w  = w;
    this->b = b;

    this->beta = beta;
    this->p = p;
    this->ph = ph;
}

FunctionSparseLinear::FunctionSparseLinear(Variable *w, float beta, float p, Variable *ph) : Function() {
    name = "FunctionSparseLinear";
    noBias = true;
    this->w  = w;

    this->beta = beta;
    this->p = p;
    this->ph = ph;
}

void FunctionSparseLinear::toHostArray(){
    i1.toHostArray();
    w->data.toHostArray();
    w->grad.toHostArray();
    if (!noBias){
        b->data.toHostArray();
        b->grad.toHostArray();
    }
}

void FunctionSparseLinear::fromHostArray(){
    i1.fromHostArray();
    w->data.fromHostArray();
    w->grad.fromHostArray();
    if (!noBias){
        b->data.fromHostArray();
        b->grad.fromHostArray();
    }
}

PVariable FunctionSparseLinear::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);
    PVariable r = PVariable(new Variable(this, w->data.rows, x->data.cols));

    if (i1.cols == 0 || i1.cols != x->data.cols){
        i1 = cuMat(1, x->data.cols);
        i1.ones();
    }

    if (!noBias) b->data.dot(i1, r->data);
    w->data.dot_plus(x->data, r->data);

    outputs.push_back(r);

    PVariable r2 = PVariable(new Variable(this, w->data.rows, x->data.cols));
    r->data.relu(r2->data);
    //r->data.sigmoid(r2->data);

    return r2;
}

void FunctionSparseLinear::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable o = outputs.at(0);
    cuMat sg = o->data.relu_d();
    //cuMat sg = o->data.sigmoid_d();

    PVariable x = inputs.at(0);

    if (x->isGetGrad){
        w->data.transpose_dot_plus(p_grad, x->grad);
    }

    // KL
    cuMat ones(ph->data.rows, ph->data.cols);
    ones.ones();
    cuMat p_tmp(ph->data.rows, ph->data.cols);
    p_tmp.ones();
    p_tmp *= p;

    cuMat kl = beta * (-1.0 * p_tmp / ph->data + (ones - p_tmp) / (ones - ph->data));

    p_grad += kl;
    p_grad *= sg;

    p_grad.dot_transpose_plus(x->data, w->grad);

    if (!noBias){
        p_grad.dot_transpose_plus(i1, b->grad);
    }

}

FunctionEmbed::FunctionEmbed() : Function() {
    name = "FunctionEmbed";
}
FunctionEmbed::FunctionEmbed(int output_size, int input_size, bool no_bias){
    name = "FunctionEmbed";

    noBias = no_bias;

    Variable w(output_size, input_size);
    this->w = w;
    this->w.randoms(0., sqrt((1./(float)input_size)));

    if (!noBias){
        Variable b(output_size, 1);
        this->b = b;
    }

}
PVariable FunctionEmbed::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    int x_cols = 0;
    if (!x->isSparse) x_cols = x->data.cols;
    else x_cols = x->data_sparse.rows;

    PVariable r = variable_construct_for_function(this, w.data.rows, x_cols);

    outputs.push_back(r);

    if (i1.cols == 0 || i1.cols != x_cols){
        i1 = cuMat(1, x_cols);
        i1.ones();
    }

    if (!noBias) b.data.dot(i1, r->data);

    if (!x->isSparse){
        w.data.dot_plus(x->data, r->data);
    }
    else{
        if (wt.rows == 0) wt = cuMat(w.data.cols, w.data.rows);
        w.data.transpose(wt);
        if (rt.rows == 0) rt = cuMat(r->data.cols, r->data.rows);
        x->data_sparse.s_d_dot(wt, rt);
        if (rtmp.rows == 0) rtmp = cuMat(r->data.rows, r->data.cols);
        rt.transpose(rtmp);
        r->data.plus(rtmp, r->data);
    }

    return r;
}
void FunctionEmbed::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    float batchSizeNorm = 1.0/((float)x->data.cols);

    if (x->isGetGrad) w.data.transpose_dot_plus(p_grad, x->grad);

    if (!x->isSparse){
        p_grad.dot_transpose_plus(x->data, w.grad);
    }
    else{
        cuMat xd = x->data_sparse.toDense();
        p_grad.dot_plus(xd, w.grad);
    }
    w.grad.mul(batchSizeNorm, w.grad);

    if (!noBias){
        p_grad.dot_transpose_plus(i1, b.grad);
        b.grad.mul(batchSizeNorm, b.grad);
    }

}
void FunctionEmbed::toHostArray(){
    i1.toHostArray();
    w.data.toHostArray();
    w.grad.toHostArray();
    if (!noBias){
        b.data.toHostArray();
        b.grad.toHostArray();
    }
}
void FunctionEmbed::fromHostArray(){
    i1.fromHostArray();
    w.data.fromHostArray();
    w.grad.fromHostArray();
    if (!noBias){
        b.data.fromHostArray();
        b.grad.fromHostArray();
    }
}

FunctionReLU::FunctionReLU() : Function() {
    name = "FunctionReLU";
}

PVariable FunctionReLU::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    PVariable r = PVariable(new Variable(this, x->data.rows, x->data.cols));

    x->data.relu(r->data);

    return r;
}

void FunctionReLU::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    if (rr.get() == NULL || rr->data.cols != x->data.cols){
        rr = PVariable(new Variable(x->data.rows, x->data.cols));
    }

    x->data.relu_d(rr->data);

    if (x->isGetGrad) rr->data.mul_plus(p_grad, x->grad, 1.0, 1.0);
}

FunctionPReLU::FunctionPReLU() : Function() {
    name = "FunctionPReLU";
}
FunctionPReLU::FunctionPReLU(Variable *a) {
    name = "FunctionPReLU";
    this->a = a;
}

PVariable FunctionPReLU::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    PVariable r = variable_construct_for_function(this, x->data.rows, x->data.cols);

    outputs.push_back(r);

    x->data.prelu(a->data, r->data);

    return r;
}
void FunctionPReLU::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){
    PVariable x = inputs.at(0);

    if (xd.get() == NULL || xd->data.cols != x->data.cols){
        xd = PVariable(new Variable(x->data.rows, x->data.cols));
        ad = PVariable(new Variable(x->data.rows, x->data.cols));
    }

    x->data.prelu_d(a->data, xd->data, ad->data);

    ad->data.mul_plus(p_grad, a->grad, 1.0, 1.0);
    if (x->isGetGrad) xd->data.mul_plus(p_grad, x->grad, 1.0, 1.0);

}

FunctionSigmoid::FunctionSigmoid() : Function() {
    name = "FunctionSigmoid";
}
PVariable FunctionSigmoid::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    PVariable r = variable_construct_for_function(this, x->data.rows, x->data.cols);

    outputs.push_back(r);
    x->data.sigmoid(r->data);

    return r;
}
void FunctionSigmoid::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){
    PVariable x = inputs.at(0);

    if (rr.get() == NULL || rr->data.cols != x->data.cols){
        rr = PVariable(new Variable(x->data.rows, x->data.cols));
    }
    x->data.sigmoid_d(rr->data);

    if (x->isGetGrad) rr->data.mul_plus(p_grad, x->grad, 1.0, 1.0);
}

FunctionTanh::FunctionTanh() : Function() {
    name = "FunctionTanh";
}
PVariable FunctionTanh::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    PVariable r = variable_construct_for_function(this, x->data.rows, x->data.cols);

    outputs.push_back(r);

    x->data.tanh(r->data);

    return r;
}
void FunctionTanh::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){
    PVariable x = inputs.at(0);

    if (rr.get() == NULL || rr->data.cols != x->data.cols){
        rr = PVariable(new Variable(x->data.rows, x->data.cols));
    }
    x->data.tanh_d(rr->data);

    if (x->isGetGrad) rr->data.mul_plus(p_grad, x->grad, 1.0, 1.0);
}

FunctionSoftmax::FunctionSoftmax() : Function() {
    name = "FunctionSoftmax";
}
PVariable FunctionSoftmax::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    PVariable r = PVariable(new Variable(x->data.rows, x->data.cols));

    outputs.push_back(r);

    x->data.softmax(r->data);

    return r;
}

FunctionSoftmaxCrossEntropy::FunctionSoftmaxCrossEntropy() : Function() {
    name = "FunctionSoftmaxCrossEntropy";
    loss = cuMat(1,1);
    loss.ones();

}
PVariable FunctionSoftmaxCrossEntropy::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);
    PVariable t = inputs.at(1);

    if (rr3.get() == NULL || rr3->data.cols != x->data.cols){
        rr3 = PVariable(new Variable(x->data.rows, x->data.cols));
    }

    x->data.softmax(rr3->data);

    if (rr.get() == NULL || rr->data.cols != rr3->data.cols){
        rr = PVariable(new Variable(rr3->data));
    }
    else rr->data = rr3->data;

    rr3->data.softmax_cross_entropy(t->data, rr3->data);
    float sum = rr3->data.sum();

    sum /= rr3->data.cols;

    PVariable r = PVariable(new Variable(this, loss.rows, loss.cols));

    outputs.push_back(r);

    r->data = loss * sum;

    return r;
}
void FunctionSoftmaxCrossEntropy::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);
    PVariable t = inputs.at(1);
    PVariable y = rr;

    if (x->isGetGrad) x->grad += y->data - t->data;
}

FunctionMeanSquaredError::FunctionMeanSquaredError() : Function() {
    name = "FunctionMeanSquaredError";
    loss = cuMat(1,1);
    loss.ones();
}

PVariable FunctionMeanSquaredError::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);
    PVariable t = inputs.at(1);

    if (rr.get() == NULL || rr->data.cols != x->data.cols){
        rr = PVariable(new Variable(x->data));
    }

    x->data.minus(t->data, rr->data);

    rr->data.mul(rr->data, rr->data);

    float sum = rr->data.sum();
    sum /= (2*rr->data.cols);

    PVariable r = variable_construct_for_function(this, loss.rows, loss.cols);

    outputs.push_back(r);

    loss.mul(sum, r->data);

    return r;
}
void FunctionMeanSquaredError::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){
    PVariable x = inputs.at(0);
    PVariable t = inputs.at(1);

    if (rr.get() == NULL || rr->data.cols != x->data.cols){

        rr = PVariable(new Variable(x->data));
    }

    x->data.minus(t->data, rr->data);
    float batch_size = rr->data.cols;
    if (x->isGetGrad) x->grad.plus(rr->data, x->grad);
}

FunctionDropout::FunctionDropout(float p) : Function() {
    name = "FunctionDropout";
    this->p = p;
}
PVariable FunctionDropout::forward(vector<PVariable > &inputs, vector<PVariable > &outputs){

    PVariable x = inputs.at(0);

    PVariable r = PVariable(new Variable(this, x->data.rows, x->data.cols));

    outputs.push_back(r);

    if (rr.get() == NULL || rr->data.cols != x->data.cols){
        rr = PVariable(new Variable(x->data.rows, x->data.cols));
    }

    x->data.dropout(r->data, rr->data, p);

    return r;
}
void FunctionDropout::backward(cuMat &p_grad, vector<PVariable > &inputs, vector<PVariable > &outputs){
    PVariable x = inputs.at(0);
    PVariable idx = rr;

    if (x->isGetGrad) idx->data.mul_plus(p_grad, x->grad, 1.0, 1.0);
}

FunctionIdentity::FunctionIdentity() : Function() {
    name = "FunctionIdentity";
}

PVariable FunctionIdentity::forward(vector<PVariable> &inputs, vector<PVariable> &outputs){

    PVariable x = inputs.at(0);

    PVariable r = variable_construct_for_function(this, x->data.rows, x->data.cols);

    r->data = x->data;

    return r;
}

void FunctionIdentity::backward(cuMat &p_grad, vector<PVariable> &inputs, vector<PVariable> &outputs){
    PVariable x = inputs.at(0);

    if (x->isGetGrad) p_grad.mul_plus(1.0, x->grad);
}

// LSTM ----------------------------------
FunctionLSTM::FunctionLSTM() : Function() {
    name = "FunctionLSTM";
}

PVariable FunctionLSTM::forward(vector<PVariable> &inputs, vector<PVariable> &outputs){

    PVariable x = inputs.at(0);
    PVariable c = inputs.at(1);
    PVariable c_next = inputs.at(2);

    int offset = x->data.rows/4;

    this->i = x->data.sliceRows(0, offset);
    this->f = x->data.sliceRows(offset, offset);
    this->g = x->data.sliceRows(offset*2, offset);
    this->o = x->data.sliceRows(offset*3, offset);

    cuMat _i = this->i;
    cuMat _f = this->f;
    cuMat _g = this->g;
    cuMat _o = this->o;

    this->i.sigmoid(_i);
    this->f.sigmoid(_f);
    this->g.tanh(_g);
    this->o.sigmoid(_o);

    c_next->data = _g * _i + _f * c->data;

    cuMat tmp = c_next->data;
    c_next->data.tanh(tmp);

    PVariable r = variable_construct_for_function(this, x->data.rows, x->data.cols);

    r->data = _o * tmp;

    return r;
}

void FunctionLSTM::backward(cuMat &gh, vector<PVariable> &inputs, vector<PVariable> &outputs){

    PVariable x = inputs.at(0);
    PVariable c = inputs.at(1);
    PVariable c_next = inputs.at(2);

    int offset = x->data.rows/4;

    cuMat co = c_next->data.tanh();

    c->grad = gh * this->o * co.tanh_d() + c_next->grad;

    cuMat gg = c->grad * this->i * this->g.tanh_d();

    cuMat gi = c->grad * this->g * this->i.sigmoid_d();

    cuMat gf = c->grad * c->data * this->f.sigmoid_d();

    cuMat go = gh * co * this->o.sigmoid_d();

    c->grad *= this->f;

    cuMat tmp = x->grad;
    tmp.joinRows(gi, 0, offset);
    tmp.joinRows(gf, offset, offset);
    tmp.joinRows(gg, offset*2, offset);
    tmp.joinRows(go, offset*3, offset);
    if (x->isGetGrad) x->grad += tmp;
}

//FullLSTM
FunctionFullLSTM::FunctionFullLSTM(
        Variable *f_c_w, Variable *f_h_w, Variable *f_x_w, Variable *f_x_b,
        Variable *i_c_w, Variable *i_h_w, Variable *i_x_w, Variable *i_x_b,
        Variable *o_c_w, Variable *o_h_w, Variable *o_x_w, Variable *o_x_b,
        Variable *g_h_w, Variable *g_x_w, Variable *g_x_b) : Function(){

    name = "FunctionFullLSTM";

    this->f_c_w = f_c_w; this->f_h_w = f_h_w; this->f_x_w = f_x_w; this->f_x_b = f_x_b;
    this->i_c_w = i_c_w; this->i_h_w = i_h_w; this->i_x_w = i_x_w; this->i_x_b = i_x_b;
    this->o_c_w = o_c_w; this->o_h_w = o_h_w; this->o_x_w = o_x_w; this->o_x_b = o_x_b;
    this->g_h_w = g_h_w; this->g_x_w = g_x_w; this->g_x_b = g_x_b;
}

PVariable FunctionFullLSTM::forward(vector<PVariable> &inputs, vector<PVariable> &outputs) {

    PVariable x = inputs.at(0);
    PVariable h = inputs.at(1);
    PVariable c = inputs.at(2);
    PVariable c_next = inputs.at(3);

    cuMat ones(1, x->data.cols);
    ones.ones();

    f_hat = f_c_w->data.dot(c->data) + f_h_w->data.dot(h->data) + f_x_w->data.dot(x->data) + f_x_b->data.dot(ones);
    f = f_hat.sigmoid();
    i_hat = i_c_w->data.dot(c->data) + i_h_w->data.dot(h->data) + i_x_w->data.dot(x->data) + i_x_b->data.dot(ones);
    i = i_hat.sigmoid();
    g_hat = g_h_w->data.dot(h->data) + g_x_w->data.dot(x->data) + g_x_b->data.dot(ones);
    g = g_hat.tanh();

    c_next->data = c->data * f + i * g;

    o_hat = o_c_w->data.dot(c_next->data) + o_h_w->data.dot(h->data) + o_x_w->data.dot(x->data) + o_x_b->data.dot(ones);
    o = o_hat.sigmoid();

    PVariable h_next = variable_construct_for_function(this, f_x_w->data.rows, x->data.cols);

    h_next->data = c_next->data.tanh() * o;

    return h_next;
}

void FunctionFullLSTM::backward(cuMat &delta_h, vector<PVariable> &inputs, vector<PVariable> &outputs) {

    PVariable x = inputs.at(0);
    PVariable h = inputs.at(1);
    PVariable c = inputs.at(2);
    PVariable c_next = inputs.at(3);
    PVariable f_for_grad = inputs.at(4);
    PVariable f_next_for_grad = inputs.at(5);
    PVariable i_for_grad = inputs.at(6);
    PVariable i_next_for_grad = inputs.at(7);
    PVariable o_for_grad = inputs.at(8);
    PVariable o_next_for_grad = inputs.at(9);
    PVariable g_for_grad = inputs.at(10);
    PVariable g_next_for_grad = inputs.at(11);

    cuMat ones(1, x->data.cols);
    ones.ones();

    cuMat delta_o = delta_h *  c_next->data.tanh() * o_hat.sigmoid_d();
    c_next->grad += delta_h * o * c_next->data.tanh_d() + o_c_w->data.transpose().dot(delta_o);

    cuMat delta_i = c_next->grad * g * i_hat.sigmoid_d();
    cuMat delta_f = c_next->grad * c->data * f_hat.sigmoid_d();
    cuMat delta_g = c_next->grad * i * g_hat.tanh_d();

    i_for_grad->grad = delta_i;
    f_for_grad->grad = delta_f;
    o_for_grad->grad = delta_o;
    g_for_grad->grad = delta_g;

    c->grad = c_next->grad * f + i_c_w->data.transpose().dot(delta_i) + f_c_w->data.transpose().dot(delta_f);

    o_c_w->grad += delta_o.dot(c_next->grad.transpose());
    i_c_w->grad += i_next_for_grad->grad.dot(c_next->grad.transpose());
    f_c_w->grad += f_next_for_grad->grad.dot(c_next->grad.transpose());

    x->grad += g_x_w->data.transpose().dot(delta_g)
               + i_x_w->data.transpose().dot(delta_i)
               + f_x_w->data.transpose().dot(delta_f)
               + o_x_w->data.transpose().dot(delta_o);

    h->grad += g_h_w->data.transpose().dot(delta_g)
               + i_h_w->data.transpose().dot(delta_i)
               + f_h_w->data.transpose().dot(delta_f)
               + o_h_w->data.transpose().dot(delta_o);

    g_x_w->grad += delta_g.dot(x->data.transpose());
    g_h_w->grad += g_next_for_grad->grad.dot(h->data.transpose());
    i_x_w->grad += delta_i.dot(x->data.transpose());
    i_h_w->grad += i_next_for_grad->grad.dot(h->data.transpose());
    f_x_w->grad += delta_f.dot(x->data.transpose());
    f_h_w->grad += f_next_for_grad->grad.dot(h->data.transpose());
    o_x_w->grad += delta_o.dot(x->data.transpose());
    o_h_w->grad += o_next_for_grad->grad.dot(h->data.transpose());

    g_x_b->grad += delta_g.dot(ones.transpose());
    i_x_b->grad += delta_i.dot(ones.transpose());
    f_x_b->grad += delta_f.dot(ones.transpose());
    o_x_b->grad += delta_o.dot(ones.transpose());
}

FunctionGRU::FunctionGRU(Variable *w_r, Variable *u_r, Variable *b_r,
                         Variable *w_z, Variable *u_z, Variable *b_z,
                         Variable *w_g, Variable *u_g, Variable *b_g){
    this->w_r = w_r;
    this->u_r = u_r;
    this->b_r = b_r;
    this->w_z = w_z;
    this->u_z = u_z;
    this->b_z = b_z;
    this->w_g = w_g;
    this->u_g = u_g;
    this->b_g = b_g;

    name = "FunctionGRU";
}

PVariable FunctionGRU::forward(vector<PVariable> &inputs, vector<PVariable> &outputs) {
    PVariable x = inputs[0];
    PVariable h = inputs[1];

    cuMat ones(w_z->data.rows, x->data.cols);
    ones.ones();
    cuMat ones_b(1, x->data.cols);
    ones_b.ones();

    r_hat = w_r->data.dot(h->data) + u_r->data.dot(x->data) + b_r->data.dot(ones_b);
    r = r_hat.sigmoid();
    z_hat = w_z->data.dot(h->data) + u_z->data.dot(x->data) + b_z->data.dot(ones_b);
    z = z_hat.sigmoid();
    g_hat = w_g->data.dot(h->data * r) + u_g->data.dot(x->data) + b_g->data.dot(ones_b);
    g = g_hat.tanh();

    PVariable h_new = variable_construct_for_function(this, w_r->data.rows, x->data.cols);

    h_new->data = h->data * (ones - z) + z * g;

    return h_new;
}

void FunctionGRU::backward(cuMat &delta_h, vector<PVariable> &inputs, vector<PVariable> &outputs) {
    PVariable x = inputs[0];
    PVariable h = inputs[1];

    cuMat zeros(w_z->data.rows, x->data.cols);
    //ones.ones();
    cuMat ones_b(1, x->data.cols);
    ones_b.ones();

    cuMat delta4 = (zeros - z) * delta_h;
    cuMat delta5 = delta_h * h->data;
    cuMat delta6 = zeros - delta5;
    cuMat delta7 = delta_h * g;
    cuMat delta8 = delta_h * z;

    cuMat delta9 = delta6 + delta7;

    cuMat delta10 = delta8 * g_hat.tanh_d();
    cuMat delta11 = delta9 * z_hat.sigmoid_d();

    cuMat delta12 = u_g->data.transpose().dot(delta10);
    cuMat delta13 = w_g->data.transpose().dot(delta10);
    cuMat delta14 = u_z->data.transpose().dot(delta11);
    cuMat delta15 = w_z->data.transpose().dot(delta11);

    cuMat delta16 = delta13 * h->data;
    cuMat delta17 = delta13 * r;
    cuMat delta18 = delta16 * r_hat.sigmoid_d();
    cuMat delta19 = delta17 + delta4;
    cuMat delta20 = u_r->data.transpose().dot(delta18);
    cuMat delta21 = w_r->data.transpose().dot(delta18);
    cuMat delta22 = delta21 + delta15;
    h->grad += delta19 + delta22;
    x->grad += delta12 + delta14 + delta20;

    w_r->grad += delta18.dot(h->data.transpose());
    u_r->grad += delta18.dot(x->data.transpose());
    w_z->grad += delta11.dot(h->data.transpose());
    u_z->grad += delta11.dot(x->data.transpose());

    w_g->grad += delta10.dot((h->data * r).transpose());
    u_g->grad += delta10.dot(x->data.transpose());

    b_r->grad += delta18.dot(ones_b.transpose());
    b_z->grad += delta11.dot(ones_b.transpose());
    b_g->grad += delta10.dot(ones_b.transpose());

}

FunctionBatchNorm::FunctionBatchNorm(int element_size, int channel_num, Variable *gamma, Variable *beta, Variable *x_mean, Variable *x_var) {
    this->gamma = gamma;
    this->beta = beta;

    this->x_mean = x_mean;
    this->x_var = x_var;

    this->element_size = element_size;
    this->channel_num = channel_num;

    xhat.resize(channel_num);
    rmu.resize(channel_num);
    xmu.resize(channel_num);
    ivar.resize(channel_num);
    sqrtvar.resize(channel_num);
    var.resize(channel_num);

}

PVariable FunctionBatchNorm::forward(vector<PVariable> &inputs, vector<PVariable> &outputs) {

    PVariable x_org = inputs[0];

    PVariable r = variable_construct_for_function(this, x_org->data.rows, x_org->data.cols);

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

        int idx = i*element_size;
        cuMat x_data = x_org->data.sliceRows(idx, element_size);

        int N = x_data.cols;
        int D = x_data.rows;

        cuMat ones(D, N);
        ones.ones();

        //step 1
        if (is_train) rmu[i] = 1.0 / N * x_data.batch_sum();
        else{
            rmu[i] = cuMat(element_size, 1);
            rmu[i].memSetDevice(this->x_mean->data.mDevice + idx);
        }
        cuMat mu = rmu[i].vec_to_mat(N);

        //step 2
        xmu[i] = x_data - mu;

        //step 3
        cuMat sq = xmu[i] * xmu[i];

        //step 4
        if (is_train) var[i] = 1.0 / N * sq.batch_sum();
        else {
            var[i] = cuMat(element_size, 1);
            var[i].memSetDevice(x_var->data.mDevice + idx);
            var[i] = ((float) N) / (((float) N) - 1.0) * var[i];
        } //use unbiased variance

        //step 5
        sqrtvar[i] = var[i].sqrt();

        //step 6
        ivar[i] = sqrtvar[i].inverse();
        cuMat tmp = ivar[i].vec_to_mat(N);

        //step 7
        xhat[i] = xmu[i] * tmp;

        //step 8
        cuMat gamma_tmp(element_size, 1);
        gamma_tmp.memSetDevice(gamma->data.mDevice + idx);
        cuMat gammax = xhat[i].mat_vec_mul(gamma_tmp, 0);

        //step 9
        cuMat beta_tmp(element_size, 1);
        beta_tmp.memSetDevice(beta->data.mDevice + idx);
        cuMat r_c = gammax + ones.mat_vec_mul(beta_tmp, 0);
        r->data.joinRows(r_c, idx, element_size);

    }

    return r;
}

void FunctionBatchNorm::backward(cuMat &dout_org, vector<PVariable> &inputs, vector<PVariable> &outputs) {

    PVariable x = inputs[0];

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

        int idx = i*element_size;
        cuMat dout = dout_org.sliceRows(idx, element_size);

        int N = dout.cols;
        int D = dout.rows;

        //step 9
        beta->grad.memSetDeviceRow(dout.batch_sum().mDevice, i);
        cuMat dgammax = dout;

        //step 8
        cuMat tmp = dgammax * xhat[i];
        gamma->grad.memSetDeviceRow(tmp.batch_sum().mDevice, i);
        cuMat gamma_tmp(element_size, 1);
        gamma_tmp.memSetDevice(gamma->data.mDevice + idx);
        cuMat dxhat = dgammax.mat_vec_mul(gamma_tmp, 0);

        //step 7
        tmp = dxhat * xmu[i];
        cuMat divar = tmp.batch_sum();
        cuMat dxmu1 = dxhat.mat_vec_mul(ivar[i], 0);

        //step 6
        //tmp = sqrtvar.inverse_d();
        tmp = -1.0 * sqrtvar[i].inverse() * sqrtvar[i].inverse();
        cuMat dsqrtvar = tmp * divar;

        //step 5
        cuMat dvar = var[i].sqrt_d() * dsqrtvar;

        //step 4
        cuMat dsq = 1.0 / N * dvar.vec_to_mat(N);

        //step 3
        cuMat dxmu2 = 2.0 * xmu[i] * dsq;

        //step 2
        cuMat dx1 = dxmu1 + dxmu2;
        cuMat dmu = -1.0 * dx1.batch_sum();

        //step 1
        cuMat dx2 = 1.0 / N * dmu.vec_to_mat(N);

        //step0
        cuMat dx3 = dx1 + dx2;
        x->grad.joinRows(dx3, idx, element_size);
    }
}

FunctionConv2D::FunctionConv2D(Variable *w, Variable *b, int batch_num, int channel_num, int w_size, int h_size, int filter_size, int filter_num, int stride, int padding){

    this->batch_num = batch_num;
    this->channel_num = channel_num;
    this->w_size = w_size;
    this->h_size = h_size;
    this->filter_size = filter_size;
    this->filter_num = filter_num;
    this->stride = stride;
    this->padding = padding;

    this->w = w;
    this->b = b;

    this->name = "FunctionConv2D";

    /**
    * Each dimension h and w of the output images is computed as followed:
    * outputDim = 1 + (inputDim + 2*pad - filterDim)/convolutionStride
    */
    this->outputDim_w = 1 + (w_size + (padding+padding) - filter_size) / stride;
    this->outputDim_h = 1 + (h_size + (padding+padding) - filter_size) / stride;

    ones = new Variable(this->outputDim_w * this->outputDim_h, 1);
    ones->ones();
}

FunctionConv2D::~FunctionConv2D(){
    delete ones;
}

cuMat FunctionConv2D::forward_one(cuMat &data){

    int output_dim_w, output_dim_h;

    cuMat stacked = data.im2col(w_size, h_size, channel_num, filter_size, filter_size, stride, stride, padding, padding, padding, padding, output_dim_w, output_dim_h);

    cols.push_back(stacked);

    cuMat r = stacked.dot(w->data.transpose()) + ones->data.dot(b->data.transpose());

    return r;
}

cuMat FunctionConv2D::backward_one(cuMat &col, cuMat &p_grad) {

    cuMat p_grad_t = p_grad.transpose();

    w->grad += p_grad_t.dot(col);

    b->grad += p_grad_t.dot(ones->data);

    cuMat dcol = p_grad.dot(w->data);

    cuMat dx = dcol.col2im(w_size, h_size, channel_num, filter_size, filter_size, stride, stride, padding, padding, padding, padding);

    return dx;
}

PVariable FunctionConv2D::forward(vector<PVariable> &inputs, vector<PVariable> &outputs){

    PVariable x = inputs[0];

    PVariable r = PVariable(new Variable(this, filter_num * outputDim_w * outputDim_h, batch_num));

    for(int i=0; i<batch_num; i++) {
        int data_index = i*(channel_num * w_size * h_size);
        float *one_m = x->data.mDevice + data_index;
        cuMat one_m_dev(w_size * h_size, channel_num);
        one_m_dev.memSetDevice(one_m);
        cuMat r_array = forward_one(one_m_dev);
        r->data.memSetDeviceCol(r_array.mDevice, i);
    }
    return r;
}

void FunctionConv2D::backward(cuMat &p_grad, vector<PVariable> &inputs, vector<PVariable> &outputs) {

    PVariable x = inputs[0];

    cuMat dx(channel_num * w_size * h_size, batch_num);

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

        int data_index = i*(filter_num * outputDim_w * outputDim_h);

        float *p_grad_one = p_grad.mDevice + data_index;

        cuMat p_grad_one_dev(outputDim_w * outputDim_h, filter_num);
        p_grad_one_dev.memSetDevice(p_grad_one);

        cuMat r_array = backward_one(cols[i], p_grad_one_dev);

        dx.memSetDeviceCol(r_array.mDevice, i);
    }

    x->grad += dx;
}

FunctionPooling::FunctionPooling(int width, int height, int depth, int windowWidth, int windowHeight, int stride, int padding){

    name = "FunctionPooling";

    this->width = width;
    this->height = height;
    this->depth = depth;
    this->windowWidth = windowWidth;
    this->windowHeight = windowHeight;
    this->stride = stride;
    this->padding = padding;
}

PVariable FunctionPooling::forward(vector<PVariable> &inputs, vector<PVariable> &outputs){
    PVariable x = inputs[0];

    int batch_num = x->data.cols;

    //* Pooling size as followed:
    //* outputDim = 1 + (inputDim + 2*padding - windowDim)/poolingStride;

    int pooled_w = 1 + (width + (padding+padding) - windowWidth)/stride;
    int pooled_h = 1 + (height + (padding+padding) - windowHeight)/stride;

    PVariable r = PVariable(new Variable(this, depth * pooled_w * pooled_h, batch_num));

    r->data = x->data.pooling(batch_num, width, height, depth, windowWidth, windowHeight, stride, stride, padding, padding, padding, padding);
    return r;
}

void FunctionPooling::backward(cuMat &p_grad, vector<PVariable> &inputs, vector<PVariable> &outputs){

    PVariable x = inputs[0];

    int batch_num = x->data.cols;

    int pooled_w = 1 + (width + (padding+padding) - windowWidth)/stride;
    int pooled_h = 1 + (height + (padding+padding) - windowHeight)/stride;

    x->grad = x->data.pooling_backward(batch_num, p_grad.mDevice, width, height, depth, windowWidth, windowHeight, stride, stride, padding, padding, padding, padding);
}


#include <iostream>
#include <string>
#include <algorithm>
#include <vector>
#include <chrono>
#include <sys/stat.h>
#include <sys/types.h>
#include <dirent.h>
#include <memory>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <yaml-cpp/yaml.h>
#include <Open3D/Open3D.h>
#include <glog/logging.h>

#include "teche_calibration.h"
#include "FileSystemTools.h"
#include "YamlFileIO.h"
#ifdef VISUALIZE_TRAJECTORY
#include "viewer.h"
#endif

/**
  * @brief save monocular camera pose in w2c form.
  * @param folder_name is name of file folder.
  * @param cam_id is the id of each camera.
  * @param cam_pose is the camera pose in w2c.
  * @return .
  */
bool saveCamPoseTxt(const std::string &filepath, const Eigen::Matrix4d &cam_pose)
{
    std::ofstream output_file(filepath);
    if (!output_file.is_open())
    {
        LOG(FATAL) << " Fail to open " << filepath << "\n";
        return false;
    }
    LOG(INFO) << " Write " << filepath << "\n";
    for (size_t i = 0; i < 4; ++i)
    {
        for (size_t j = 0; j < 4; ++j)

        {
            if (j != 3)
                output_file << cam_pose(i, j) << ", ";
            else
                output_file << cam_pose(i, j) << "\n";
        }
    }
    output_file.close();

    return true;
}

std::shared_ptr<open3d::geometry::PointCloud> convertPointClouds(const std::vector<Eigen::Vector3d> &v_pointclouds)
{
    assert(v_pointclouds.size());

    int pts_num = v_pointclouds.size();
    std::shared_ptr<open3d::geometry::PointCloud> pcl_ptr(new open3d::geometry::PointCloud);
    for(int i = 0; i < pts_num; ++i)
    {
        pcl_ptr->points_.push_back(v_pointclouds[i]);
    }

    return pcl_ptr;
}

int main(int argc, char **argv)
{
    if (argc < 8)
    {
        std::cerr << "Usage : test_techecalibrator [global_map.yaml] [cctag_result.yaml]\n \
        [cam0_intrin_file] [cam1_intrin_file] [cam2_intrin_file] [cam3_intrin_file]\n \
        [output_folder] \n";
        return -1;
    }
    // start timer
    // const auto tp_1 = std::chrono::steady_clock::now();

    // save w2c camera pose
    bool b_save_camera_pose_w_c = true;
    // save c2r camera pose
    bool b_save_camera_pose_c_r = true;

    std::string gt_map_file_name(argv[1]);
    std::string cctag_file_name(argv[2]);
    std::string cam0_calibration_file_name(argv[3]);
    std::string cam1_calibration_file_name(argv[4]);
    std::string cam2_calibration_file_name(argv[5]);
    std::string cam3_calibration_file_name(argv[6]);
    std::string output_folder(argv[7]);

    if (!common::fileExists(gt_map_file_name))
    {
        LOG(ERROR) << " FIle " << gt_map_file_name << " doesnot exist!";
        return -1;
    }
    if (!common::pathExists(output_folder))
    {
        if (!common::createPath(output_folder))
        {
            LOG(ERROR) << " Fail to create " << output_folder;
            return -1;
        }
    }

    std::vector<std::string> v_intrinsic_fns = {cam0_calibration_file_name, cam1_calibration_file_name,
                                                cam2_calibration_file_name, cam3_calibration_file_name};
    std::vector<std::string> v_calib_img_paths;
    std::vector<std::vector<cv::Point2d>> v_cctag_centers;
    bool sts = common::loadCCTagResultFile(cctag_file_name, v_calib_img_paths, v_cctag_centers);
    if (!sts)
    {
        LOG(ERROR) << " Failed to parse " << cctag_file_name;
        return -1;
    }

    TecheCalibrator teche_calibrator(camera::model_type_t::Fisheye, target::target_type_t::CCTAG, v_intrinsic_fns, gt_map_file_name, 4);
    sts = teche_calibrator.addCctagData(v_calib_img_paths, v_cctag_centers);
    if (!sts)
    {
        LOG(ERROR) << " Feail to add cctag data!";
        return -1;
    }

    sts = teche_calibrator.calibrate();
    if (!sts)
    {
        LOG(ERROR) << " Fail to calibrate teche 360 anywhere camera";
        return -1;
    }
    // const auto tp_2 = std::chrono::steady_clock::now();
    // const auto calib_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
    // TIMER_STREAM("[test_techecalibrator] caliration time is " << calib_time << " s");

    std::vector<Eigen::Matrix4d> v_cam_poses_w2c = teche_calibrator.validCameraPoses();
    // rotation axis 2 camera
    std::vector<Eigen::Matrix4d> v_cam_poses_r2c = teche_calibrator.validCameraPosesInRotationAxisFrame();

    // save camera pose and relative pose
    for (size_t i = 0; i < v_cam_poses_w2c.size(); ++i)
    {
        if (b_save_camera_pose_w_c)
        {
            std::string txt_filepath = output_folder + "/cam" + std::to_string(i) + "_w2c.txt";
            saveCamPoseTxt(txt_filepath, v_cam_poses_w2c[i]);
        }
        if (i == 0)
            continue;

        Eigen::Matrix4d T_cam0_cami = v_cam_poses_w2c[0].inverse() * v_cam_poses_w2c[i];
        std::string yml_filepath = output_folder + "/camera0"+"_to_camera"+ std::to_string(i) +".yml";
        common::saveExtFileOpencv(yml_filepath, T_cam0_cami);
    }

    // for (size_t i = 0; i < v_cam_poses_r2c.size(); ++i)
    // {
    //     if (b_save_camera_pose_c_r)
    //     {
    //         sts = writeCameraPosesInYml(output_folder, i, v_cam_poses_r2c[i]);
    //         if (!sts)
    //         {
    //             ERROR_STREAM("[test_techecalibrator] Fail to write frame " << i);
    //             return -1;
    //         }
    //     }
    // }

#ifdef VISUALIZE_TRAJECTORY
    Viewer viewer(v_visual_frames);
    std::vector<Eigen::Vector3d> v_map_points = teche_calibrator.objectPoints();
    // std::shared_ptr<open3d::geometry::PointCloud> targtboard_pcl_ptr = convertPointClouds(v_map_points);
    // open3d::io::WritePointCloudToPLY(output_folder + "/calibration_room.ply", *targtboard_pcl_ptr, true);
    viewer.setCurrentMapPoints(v_map_points);
    viewer.run();
#endif

    return 0;
}


// sdf: Triangle mesh to signed-distance function (SDF) library
// Copyright Alex Yu 2020
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. 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.
//
// 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 HOLDER 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.

#pragma once
#ifndef SDF_SDF_F15B6437_01FD_4DBE_AB0D_BC1EE8ACC4C4
#define SDF_SDF_F15B6437_01FD_4DBE_AB0D_BC1EE8ACC4C4

#include <Eigen/Core>
#include <cstdint>

#include <memory>
#include <vector>
#ifdef __GNUC__
#include <experimental/propagate_const>
#endif
#include <Eigen/Geometry>

namespace sdf {

using Index = uint32_t;
using Points = Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor>;
using Points2D = Eigen::Matrix<float, Eigen::Dynamic, 2, Eigen::RowMajor>;
using Triangles = Eigen::Matrix<Index, Eigen::Dynamic, 3, Eigen::RowMajor>;
using Matrix =
    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using Vector = Eigen::Matrix<float, Eigen::Dynamic, 1>;

namespace util {

template <class T>
// 3D point to line shortest distance SQUARED
T dist_point2line(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& p,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& b) {
    Eigen::Matrix<T, 1, 3> ap = p - a, ab = b - a;
    return (ap - (ap.dot(ab) / ab.squaredNorm()) * ab).squaredNorm();
}

// 3D point to line segment shortest distance SQUARED
template <class T>
T dist_point2lineseg(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& p,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& b) {
    Eigen::Matrix<T, 1, 3> ap = p - a, ab = b - a;
    T t = ap.dot(ab) / ab.squaredNorm();
    t = std::max(T(0.0), std::min(T(1.0), t));
    return (ap - t * ab).squaredNorm();
}

template <class T>
const Eigen::Matrix<T, 1, 3, Eigen::RowMajor> normal(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& b,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& c) {
    return (b - a).cross(c - a);
}

template <class T>
// Find barycentric coords of p in triangle (a,b,c) in 3D
// (p does NOT have to be projected to plane beforehand)
// normal, area_abc to be computed using util::normal,
// where normal is normalized vector, area is magnitude
Eigen::Matrix<T, 1, 3, Eigen::RowMajor> bary(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& p,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& b,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& c,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& normal,
    float area_abc) {
    float area_pbc = normal.dot((b - p).cross(c - p));
    float area_pca = normal.dot((c - p).cross(a - p));

    Eigen::Matrix<T, 1, 3> uvw;
    uvw.x() = area_pbc / area_abc;
    uvw.y() = area_pca / area_abc;
    uvw.z() = T(1.0) - uvw.x() - uvw.y();

    return uvw;
}

template <class T>
// 3D point to triangle shortest distance SQUARED
// normal, area_abc to be computed using util::normal,
// where normal is normalized vector, area is magnitude
T dist_point2tri(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& p,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& b,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& c,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& normal,
    float area) {
    const Eigen::Matrix<T, 1, 3> uvw = bary<T>(p, a, b, c, normal, area);
    if (uvw[0] < 0) {
        return dist_point2lineseg<T>(p, b, c);
    } else if (uvw[1] < 0) {
        return dist_point2lineseg<T>(p, a, c);
    } else if (uvw[2] < 0) {
        return dist_point2lineseg<T>(p, a, b);
    } else {
        return (uvw[0] * a + uvw[1] * b + uvw[2] * c - p).squaredNorm();
    }
}

template <class T>
Eigen::Matrix<T, 1, 3, Eigen::RowMajor> bary2d(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 2, Eigen::RowMajor>>& p,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 2, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 2, Eigen::RowMajor>>& b,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 2, Eigen::RowMajor>>& c) {
    Eigen::Matrix<T, 1, 2, Eigen::RowMajor> v0 = b - a, v1 = c - a, v2 = p - a;
    Eigen::Matrix<T, 1, 3> result;
    const float invden = 1.f / (v0.x() * v1.y() - v1.x() * v0.y());
    result[1] = (v2.x() * v1.y() - v1.x() * v2.y()) * invden;
    result[2] = (v0.x() * v2.y() - v2.x() * v0.y()) * invden;
    result[0] = 1.0f - result.template tail<2>().sum();
    return result;
}
}  // namespace util

// Signed distance function utility for watertight meshes.
//
// Basic usage: SDF sdf(verts, faces); Vector sdf_vals = sdf(query_points);
// Get nearest neighbor (verts) indices: sdf.nn(query_points);
// Check containment (returns bool): sdf.contains(query_points);
struct SDF {
    // Construct SDF instance from triangle mesh with given vertices and faces
    // Note: Mesh is assumed to be watertight and all vertex positions are
    // expected to be free of nans/infs
    //
    // Basic usage: SDF sdf(verts, faces); Vector sdf_vals = sdf(query_points);
    // Get nearest neighbor (verts) indices: sdf.nn(query_points);
    // Check containment (returns bool): sdf.contains(query_points);
    //
    // @param verts mesh vertices. If the contents of this matrix are modified,
    // please call SDF::update() to update the internal representation.
    // Else the results will be incorrect.
    // @param faces mesh faces. The contents of this matrix should not be
    // modified for the lifetime of this instance.
    // @param robust whether to use robust mode. In robust mode,
    // @param copy whether to make a copy of the data instead of referencing it
    // SDF/containment computation is robust to mesh self-intersections and
    // facewinding but is slower.
    SDF(Eigen::Ref<const Points> verts, Eigen::Ref<const Triangles> faces,
        bool robust = true, bool copy = false);
    ~SDF();

    /*** PRIMARY INTERFACE ***/
    // Fast approximate signed-distance function.
    // Points inside the mesh have positive sign and outside have negative sign.
    //
    // Method: computes minimum distance to a triangular face incident to
    // the nearest vertex for each point.
    //
    // @param points input points
    // @param trunc_aabb if true, returns -FLT_MAX for all points outside mesh's
    // bounding box
    // @return approx SDF values at input points
    //
    // WARNING: if robust=false (from constructor), this WILL FAIL if the mesh
    // has self-intersections. In particular, the signs of points inside the
    // mesh may be flipped.
    Vector operator()(Eigen::Ref<const Points> points,
                      bool trunc_aabb = false) const;

    // Return exact nearest neighbor vertex index for each point (index as in
    // input verts)
    Eigen::VectorXi nn(Eigen::Ref<const Points> points) const;

    // Return 1 for each point inside/on surface of the mesh and 0 for outside.
    //
    // @param points input points
    // @return indicator of whether each point is in OR on surface of mesh
    //
    // WARNING: if robust=false (from constructor), this WILL FAIL if the mesh
    // has self-intersections.
    Eigen::Matrix<bool, Eigen::Dynamic, 1> contains(
        Eigen::Ref<const Points> points) const;

    // Call if vertex positions have been updated to rebuild the KD tree
    // and update face normals+areas
    void update();

    /*** MISC UTILITIES ***/
    // Sample 'num_points' points uniformly on surface, output (num_points, 3).
    // Note: this takes O(num_points * log(num_faces)) time.
    Points sample_surface(int num_points) const;

    /*** DATA ACCESSORS ***/
    // Get adjacent faces of point at verts[pointid]
    const std::vector<int>& adj_faces(int pointid) const;

    // Get total surface area of mesh
    const float surface_area() const;

    // Get vector of face areas, shape (num_faces)
    const Vector& face_areas() const;

    // Get matrix of face normals, shape (num_faces, 3).
    // normal of face i (from faces passed to constructor) is in row i
    const Points& face_normals() const;

    // Get AABB of entire mesh, shape (6).
    // (minx, miny, minz, maxx, maxy, maxz)
    Eigen::Ref<const Eigen::Matrix<float, 6, 1>> aabb() const;

    // Get faces
    Eigen::Ref<const Triangles> faces() const;
    Eigen::Ref<Triangles> faces_mutable();

    // Get verts
    Eigen::Ref<const Points> verts() const;
    Eigen::Ref<Points> verts_mutable();

    // Whether SDF is in robust mode
    const bool robust;

    // Whether we own data
    const bool own_data;

   private:
    // Optional owned data
    Points owned_verts;
    Triangles owned_faces;

    struct Impl;
#ifdef __GNUC__
    std::experimental::propagate_const<std::unique_ptr<Impl>> p_impl;
#else
    std::unique_ptr<Impl> p_impl;
#endif
};

// Image-space raycast renderer utility for watertight meshes.
//
// Renders depth maps (render_depth), object mask (render_mask), and
// vertex ids (render_nn)
// using raycasting in image space. Also supports querying these for
// arbitrary continuous points (x, y) in image space
// (operator(), contains, nn).
//
// By image space we mean the space of (x,y,z)
// where a pinhole camera perspective projection was applied to x,y.
// This class is somehow similar to sdf::SDF but in
// image space and only available on the image plane.
//
// NOTE: We assume no objects are present where z <= 0. This allows
// us to use 2D data structures and skip a check.
//
// This is not a very efficient method.
// If object has relatively few points compared to image size,
// painter's algorithm (implemented in sxyu/avatar) probably performs better.
//
// Assumes camera is at origin facing +z, where up is -y and right is +x.
// Note the coordinate system is right-handed.
struct Renderer {
    // Construct software renderer
    // @param verts mesh vertices. If the contents of this matrix are modified,
    // please call Renderer::update() to update the internal representation.
    // Else the results will be incorrect.
    // @param faces mesh faces. The contents of this matrix should not be
    // modified for the lifetime of this instance.
    // @param width image width
    // @param height image height
    // @param fx focal length x
    // @param fy focal length y
    // @param cx principal point x
    // @param cy principal point y
    // @param copy whether to make a copy of the data instead of referencing it
    // SDF/containment computation is robust to mesh self-intersections and
    // facewinding but is slower.
    Renderer(Eigen::Ref<const Points> verts, Eigen::Ref<const Triangles> faces,
             int width = 1080, int height = 1080, float fx = 2600.f,
             float fy = 2600.f, float cx = 540.f, float cy = 540.f,
             bool copy = false);

    // Destructor, pImpl pattern needs this
    ~Renderer();

    // *** PRIMARY INTERFACE ***
    // Render (height, width) depth map of the mesh.
    // @return Each pixel will be distance from z=0 plane and 0 if no object is
    // present.
    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    render_depth() const;

    // Render (height, width) mask.
    // @return Each pixel is 1 where object is present, 0 else.
    Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    render_mask() const;

    // Render (height, width) vertex map, i.e. vertex id nearest to raycast hit
    // at each pixel. Each pixel is -1 if empty space, index of vertex in verts
    // else
    // @param fill_outside if true, instead of returning -1 for empty space,
    // finds nearest-neighbor vertex in 2d and uses its index
    Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    render_nn(bool fill_outside = false) const;

    // Compute depth at 2D points
    // (render_depth for continuous points)
    Vector operator()(Eigen::Ref<const Points2D> points) const;

    // Compute mask at 2D points (1 means inside)
    // (render_mask for continuous points)
    Eigen::Matrix<bool, Eigen::Dynamic, 1> contains(
        Eigen::Ref<const Points2D> points) const;

    // Compute vertex id hit by raycast at 2D points
    // (render_vertex for continuous points)
    // @param fill_outside if true, instead of returning -1 for empty space,
    // finds nearest-neighbor vertex in 2d and uses its index
    Eigen::VectorXi nn(Eigen::Ref<const Points2D> points,
                       bool fill_outside = false) const;

    // Call if vertex positions have been updated to rebuild the KD tree
    // and update face normals+areas
    void update();

    /*** DATA ACCESSORS ***/
    // Get faces
    Eigen::Ref<const Triangles> faces() const;
    Eigen::Ref<Triangles> faces_mutable();

    // Get verts
    Eigen::Ref<const Points> verts() const;
    Eigen::Ref<Points> verts_mutable();

    // Whether we own data
    const bool own_data;

   private:
    // Optional owned data
    Points owned_verts;
    Triangles owned_faces;

    struct Impl;
#ifdef __GNUC__
    std::experimental::propagate_const<std::unique_ptr<Impl>> p_impl;
#else
    std::unique_ptr<Impl> p_impl;
#endif
};

}  // namespace sdf

#endif  // ifndef SDF_SDF_F15B6437_01FD_4DBE_AB0D_BC1EE8ACC4C4


/*
 *  Copyright (c) 2009, Rene Wagner
 *  All rights reserved.
 *
 *  Author: Rene Wagner <rw@nelianur.org>
 *
 *  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 Rene Wagner nor the names of any
 *     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.
 */

#ifndef __UKFOM_LAPACK_INVERT_HPP__
#define __UKFOM_LAPACK_INVERT_HPP__

#include "lapack.h"

#include <Eigen/Core>

namespace ukf {
namespace lapack {

// import most common Eigen types
USING_PART_OF_NAMESPACE_EIGEN

template<size_t P>
Matrix<double, P, P> invert(const Matrix<double, P, P> &m)
{
	Matrix<double, P, P> tmp = m;

	/* first call DGETRF */
	int M = tmp.rows();
	int N = tmp.cols();
	int LDA = tmp.stride();
	int IPIV[M];

	int INFO;

	dgetrf_(&M, &N, tmp.data(), &LDA, IPIV, &INFO);

	if(INFO != 0)
		throw "dgetrf failed";

	/* now call DGETRI */
	int LWORK = 64 * N; // FIXME: retrieve block size from ILAENV
	double WORK[LWORK];

	dgetri_(&N, tmp.data(), &LDA, IPIV, WORK, &LWORK, &INFO);

	if(INFO != 0)
		throw "dgetri failed";

	return tmp;
}

} // namespace lapack
} // namespace ukf

#endif // __UKFOM_LAPACK_INVERT_HPP__