//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

#pragma once

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <vector>

#include "entities/Agent.hpp"
#include "spatial_trees/TreeImpl.hpp"

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

namespace sim_mob
{

typedef bg::model::point<float, 2, bg::cs::cartesian> point;
typedef bg::model::box<point> box;
typedef std::pair<box, const Agent *> value;
typedef bgi::rtree<value, bgi::linear<16> > RTree;

class PackingTreeAuraManager : public TreeImpl
{
private:
    /**The r-star tree with packing insert algorithm*/
    RTree *tree;

public:
    PackingTreeAuraManager();
    virtual ~PackingTreeAuraManager();

    /**
     * Update all agents in the simulation.
     *
     * @param time_step simulation time_step
     * @param removedAgentPointers temp container
     *
     * The pointers in removedAgentPointers will be deleted after this time tick; do *not* save them anywhere.
     */
    virtual void update(int time_step, const std::set<sim_mob::Entity *> &removedAgentPointers);

    /**
     * Return a collection of agents that are located in the axially-aligned rectangle.
     *
     * @param lowerLeft The lower left corner of the axially-aligned search rectangle.
     * @param upperRight The upper right corner of the axially-aligned search rectangle.
     * @param refAgent The agent performing the query. If non-null, certain implementations
     * (namely the Sim Tree) can make use of an optimized bottom-up query in some cases.
     * If null, the algorithm used will always be the slower, top-down query.
     *
     * @return a collection of agents
     * The caller is responsible to determine the "type" of each agent in the returned array.
     */
    virtual std::vector<Agent const *> agentsInRect(const Point &lowerLeft, const Point &upperRight, const sim_mob::Agent *refAgent) const;

    /**
     * Return a collection of agents that are on the left, right, front, and back of the specified
     * position.
     *
     * @param position The center of the search rectangle.
     * @param wayPoint The wapypoint (lane or turning path)
     * @param distanceInFront The forward distance of the search rectangle.
     * @param distanceBehind The backward distance of the search rectangle
     * @param refAgent The agent performing the query. If non-null, certain implementations
     * (namely the Sim Tree) can make use of an optimized bottom-up query in some cases.
     * If null, the algorithm used will always be the slower, top-down query.
     *
     * @return a collection of agents
     * This query is designed for Driver/Vehicle agents.  It calculates the search rectangle
     * based on position, lane, distanceInFront, and distanceBehind. position
     * should be the current location of the Driver agent and is within the boundary of lane.
     * The search rectangle is (not necessarily symmetrically) centered around position.
     * It includes the adjacent lanes on the left and right of position (effectively of
     * lane as well).  If lane is the leftmost lane and/or is the rightmost lane, the search
     * rectangle extends 300 centimeters to include the sidewalk or the road segment of the reverse
     * direction.
     */
    virtual std::vector<Agent const *> nearbyAgents(const Point &position, const WayPoint &wayPoint, double distanceInFront, double distanceBehind,
                                                    const sim_mob::Agent *refAgent) const;

};

}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [inverse_hyperbolic]
#include <boost/simd/hyperbolic.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/function/enumerate.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft =  bs::pack <float, 8>;

int main()
{
  pack_ft p = bs::enumerate<pack_ft>(-2.0f, 0.5f);
  std::cout << " p =  " << p << std::endl
            <<  " -> bs::acosh(p) =  " << bs::acosh(p) << std::endl
            <<  " -> bs::asinh(p) =  " << bs::asinh(p) << std::endl
            <<  " -> bs::atanh(p) =  " << bs::atanh(p) << std::endl
            <<  " -> bs::asech(p) =  " << bs::asech(p) << std::endl
            <<  " -> bs::acsch(p) =  " << bs::acsch(p) << std::endl;
  return 0;
}
//! [inverse_hyperbolic]


/*******************************************************************************
 *         Copyright 2003-2012 LASMEA UMR 6602 CNRS/U.B.P
 *         Copyright 2009-2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_TOOLBOX_LINALG_FUNCTIONS_INV_HPP_INCLUDED
#define NT2_TOOLBOX_LINALG_FUNCTIONS_INV_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <nt2/sdk/memory/container.hpp>
#include <nt2/sdk/meta/size_as.hpp>
#include <nt2/sdk/meta/value_as.hpp>
#include <nt2/core/container/dsl/value_type.hpp>
#include <nt2/core/container/dsl/size.hpp>
#include <nt2/include/functions/issquare.hpp>

/*!
 * @brief inv  inverse of square matrix.
 * inv(x) is the inverse of the square matrix expression x.
 *
 * Avoid to use this function if you do not have a very good reason
 * and whatsoever never use it to solve linear systems.
 * 99% of the time a solver is a better choice.
 **/

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag inv_ of functor inv
     *        in namespace nt2::tag for toolbox algebra
    **/
    struct inv_ :  ext::unspecified_<inv_> { typedef ext::unspecified_<inv_> parent; };
  }

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::inv_, inv, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::inv_, inv, 2)
}

namespace nt2 { namespace ext
{
  template<class Domain, class Expr,  int N> //N == 1 only
  struct  size_of<tag::inv_, Domain, N, Expr>
  {
    typedef typename boost::proto::result_of::child_c<Expr&,0>::value_type  c0_t;
    typedef typename c0_t::extent_type                               result_type;
    BOOST_FORCEINLINE result_type operator()(Expr& e) const
    {
      result_type sizee = boost::proto::child_c<0>(e).extent();
      BOOST_ASSERT_MSG(issquare(boost::proto::child_c<0>(e)),
                       "inverse of non square matrix expression requested");
      return sizee;
    }
  };

 template <class Domain, class Expr,  int N>
 struct value_type < tag::inv_, Domain,N,Expr>
  : meta::value_as<Expr,0>
 {};
} }

#endif


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#include <fstream>
#include <string>
#include <boost/program_options.hpp>

#include <omp.h>

#include "dpSubclustersSphereMM.hpp"

using namespace Eigen;
using std:string;
using std::ofstream;
using std::ifstream;
namespace po = boost::program_options;

{

  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("N,N", po::value<int>(), "number of input datapoints")
    ("D,D", po::value<int>(), "number of dimensions of the data")
    ("T,T", po::value<int>(), "iterations")
    ("alpha,a", po::value<double>(), "alpha parameter of the DP")
    ("base,b", po::value<string>(),
      "which base measure to use (only NIW right now)")
    ("params,p", po::value< vector<double> >()->multitoken(),
      "parameters of the base measure")
    ("input,i", po::value<string>(),
      "path to input dataset .csv file (rows: dimensions; cols: different datapoints)")
    ("output,o", po::value<string>(),
      "path to output labels .csv file (rows: time; cols: different datapoints)")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

#ifndef NDEBUG
  uint32_t nThreads = 1;
#else
  uint32_t nThreads = omp_get_max_threads(); // has to match number of threads on computer
#endif
  // number of iterations
  uint32_t T=1000;
  if (vm.count("T")) T = vm["T"].as<int>();
  uint32_t N=100;
  if (vm.count("N")) N = vm["N"].as<int>();
  uint32_t D=2;
  if (vm.count("D")) D = vm["D"].as<int>();
  cout << "T="<<T<<endl;
  // DP alpha parameter
  double alpha = 0.1;
  if (vm.count("alpha")) alpha = vm["alpha"].as<double>();
  cout << "alpha="<<alpha<<endl;
  // which base distribution
  string base = "NIW";
  if(vm.count("base")) base = vm["base"].as<string>();

//  DpMM *dpmm;
//  if(!base.compare("NIW"))
//  {
    MatrixXd Delta(D-1,D-1);
    VectorXd theta(D);
    double nu = D+3.0;
    double kappa = D+3.0;
//    Delta << nu,0.0,0.0,nu;
//    theta << 0.0,0.0;
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      nu = params[0];
      kappa = params[1];
      for(uint32_t i=0; i<D; ++i)
        theta(i) = params[2+i];
      for(uint32_t i=0; i<D-1; ++i)
        for(uint32_t j=0; j<D-1; ++j)
          Delta(i,j) = params[2+D-1+i+(D-1)*j];
      cout <<"nu="<<nu<<endl;
      cout <<"kappa="<<kappa<<endl;
      cout <<"theta="<<theta<<endl;
      cout <<"Delta="<<Delta<<endl;
    }
    niwSphere_sampled niw(D-1,kappa,nu, theta.data(),Delta.data());
//
//  }else{
//    cout<<"base "<<base<<" not supported"<<endl;
//    return 1;
//  }

  MatrixXd x(D,N);
  string pathIn ="";
  if(vm.count("input")) pathIn = vm["input"].as<string>();
  if (!pathIn.compare(""))
  {
    for(uint32_t i=0; i<N; ++i)
      if(i<N/2)
      {
        x.col(i) << VectorXd::Zero(D);
      }else{
        x.col(i) << 2.0*VectorXd::Ones(D);
      }
  }else{
    cout<<"loading data from "<<pathIn<<endl;
    ifstream fin(pathIn.data(),ifstream::in);
    for (uint32_t j=0; j<D; ++j)
      for (uint32_t i=0; i<N; ++i)
      {
        fin>>x(j,i);
      }
    //cout<<x<<endl;
  }
  string pathOut ="./labels.csv";
  if(vm.count("output"))
    pathOut = vm["output"].as<string>();
  cout<<"output to "<<pathOut<<endl;

  VectorXd z(N);
  z.setZero();
  cout<<"z.shape="<<z.size()<<endl;
  DpSubclustersSphereMM *dpmm = new DpSubclustersSphereMM(N,D,x.data(),
      z.data(), alpha, niw, nThreads,true,true);
  cout<<"-- init"<<endl;
  dpmm->initialize();

  ofstream fout(pathOut.data(),ofstream::out);
  for (uint32_t t=0; t<T; ++t)
  {
    cout<<"------------ t="<<t<<" -------------"<<endl;
    cout<<"-- sampling params"<<endl;
    dpmm->sample_params();
    cout<<"-- sampling superclusters"<<endl;
    dpmm->sample_superclusters();
    cout<<"-- sampling labels"<<endl;
    dpmm->sample_labels();

    if(t%50 == 0)
    {
      cout<<"-- random splits"<<endl;
      dpmm->propose_random_splits();
    }
    cout<<"-- random merges"<<endl;
    dpmm->propose_random_merges();
    cout<<"-- splits"<<endl;
    dpmm->propose_splits();

    cout<<"   logLike=\t"<<dpmm->joint_loglikelihood()<<endl;
    cout<<"   K=\t"<<dpmm->getK()<<endl;
    cout<<"   Nk=\t"<<dpmm->getNK()<<endl;

//    const VectorXi& z = dpmm->getLabels().transpose();
//    //cout<< "z_"<<t<<"= "<<z.transpose()<<endl;
    for (uint32_t i=0; i<z.size()-1; ++i)
      fout<<int(floor(z(i)))<<" ";
    fout<<int(floor(z(z.size()-1)))<<endl;
  }
  fout.close();
}


// Demonstration of analyzing RC trees as graphs
// to accompany "Analyzing On-Chip Interconnect with Modern C++"
// Author: Jeff Trull <edaskel@att.net>

/*
Copyright (c) 2014 Jeffrey E. Trull

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <iostream>

#include <boost/graph/visitors.hpp>
#include <boost/graph/undirected_dfs.hpp>

#include "ckt_graph.h"

// for use in algorithms:
template <typename Graph>
struct loop_detect_visitor : boost::default_dfs_visitor {
    typedef typename boost::graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_t;

    void tree_edge(edge_t e, Graph const& g) {
        // remember predecessor
        predecessors.emplace(target(e, g), source(e, g));
    }

    void back_edge(edge_t e, Graph const& g) {
        using namespace std;
        cout << "cycle detected: " << g[target(e, g)];
        for (auto v = source(e, g); v != target(e, g); v = predecessors.at(v)) {
            cout << "->" << g[v];
        }
        cout << "->" << g[target(e, g)] << endl;
    }

private:
    std::map<vertex_t, vertex_t> predecessors;
};

int main() {
    using namespace boost;
    using namespace std;

    // Build some interesting circuits
    quantity<si::resistance> kohm(1.0 * units::si::kilo  * units::si::ohms);
    quantity<si::capacitance> ff(1.0 * units::si::femto * units::si::farads);

    // First, one with a resistor loop
    ckt_graph_t r_loop;
    {
        auto gnd = r_loop.gnd();
        auto n1  = add_vertex("n1", r_loop);
        auto n2  = add_vertex("n2", r_loop);     // starting point for resistor loop
        add_edge(n1, n2, 100.*kohm, r_loop);
        add_edge(n2, gnd, 10.*ff, r_loop);
        auto n3  = add_vertex("n3", r_loop);     // ending point of loop
        add_edge(n2, n3, 2.71*kohm, r_loop);     // loop branch 1
        auto n2a  = add_vertex("n2a", r_loop);
        add_edge(n2, n2a, 3.14*kohm, r_loop);    // loop branch 2
        add_edge(n2a, n3, 1.*kohm, r_loop);      // loop branch 2
        add_edge(n3, gnd, 10.*ff, r_loop);
    }

    // perform DFS with loop visitor on filtered (resistor-only) graph
    typedef filtered_graph<ckt_graph_t, resistors_only> resonly_graph_t;
    resonly_graph_t res_graph(r_loop, resistors_only(&r_loop));
    loop_detect_visitor<resonly_graph_t> findloops;

    // supply edge color map for algorithm
    map<ckt_graph_t::edge_descriptor, default_color_type> edge_colors;
    auto cpmap = make_assoc_property_map(edge_colors);

    undirected_dfs(res_graph,
                   edge_color_map(cpmap)
                  .visitor(findloops));

}


#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Dense>

#ifdef USE_OPENBLAS
#include <cblas.h>
#endif

#include "eigenmat.h"

using namespace std;

/* ------------------------------ Utility routines ------------------------------ */

int get_leading_dimension(eigenmat* mat) {
  return mat->is_trans ? mat->size[1] : mat->size[0];
}

int get_nonleading_dimension(eigenmat* mat) {
  return mat->is_trans ? mat->size[0] : mat->size[1];
}

void set_transpose(eigenmat* mat, int is_trans) {
  mat->is_trans = is_trans;
}

inline char get_transpose_char(eigenmat* mat) {
  return mat->is_trans ? 't' : 'n';
}

/* ------------------------------ Allocating/moving data ------------------------------ */

int allocate_memory(eigenmat* mat) {
  unsigned int len = mat->size[0] * mat->size[1];
  mat->data = (float*)malloc(len * sizeof(float));
  return 0;
}

int copy_on_device(eigenmat* mat1, eigenmat* mat2) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::VectorXf> eig_mat1(mat1->data, len);
  Eigen::Map<Eigen::VectorXf> eig_mat2(mat2->data, len);
  eig_mat2 = eig_mat1;

  return 0;
}

int get_row_slice(eigenmat* source, eigenmat* target, unsigned int start, unsigned int end) {
  unsigned int height = source->size[0];
  unsigned int width = source->size[1];

  if ((end - start) != target->size[0] || source->size[1] != target->size[1] || start >= end || end > height)
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  return 0;
}

int set_row_slice(eigenmat* source, eigenmat* target, unsigned int start, unsigned int end) {
  unsigned int height = target->size[0];
  unsigned int width = target->size[1];

  if ((end - start) != source->size[0] || source->size[1] != target->size[1] || start >= end || end > height)
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  return 0;
}

int copy_transpose(eigenmat* source, eigenmat* target) {
  unsigned int height = source->size[0];
  unsigned int width = source->size[1];

  if (source->size[0] != target->size[1] || source->size[1] != target->size[0])
    return ERROR_INCOMPATIBLE_DIMENSIONS;
  Eigen::Map<Eigen::MatrixXf> eig_source(source->data, source->size[0], source->size[1]);
  Eigen::Map<Eigen::MatrixXf> eig_target(target->data, target->size[0], target->size[1]);
  eig_target = eig_source.transpose();

  return 0;
}

int set_shape(eigenmat* mat, unsigned int m, unsigned int n) {
  mat->size[0] = m;
  mat->size[1] = n;

  return 0;
}

int reshape(eigenmat* mat, int m, int n) {
    if (m < 0 && n < 0)
        return ERROR_GENERIC;

    if (m < 0)
        m = (mat->size[0] * mat->size[1]) / n;
    if (n < 0)
        n = (mat->size[0] * mat->size[1]) / m;

    if (mat->size[0] * mat->size[1] != m * n)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    mat->size[0] = m;
    mat->size[1] = n;

    return 0;
}

int get_slice(eigenmat* source, eigenmat* target, unsigned int first_col, unsigned int last_col) {
  if (source->is_trans)
    return ERROR_TRANSPOSED;

  if (last_col > source->size[1] || (first_col >= last_col))
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  int num_rows = source->size[0];

  target->data = source->data + first_col * num_rows;
  target->size[0] = source->size[0];
  target->size[1] = last_col - first_col;
  target->is_trans = 0;
  target->owns_data = 0;

  return 0;
}

int get_vector_slice(eigenmat* source, eigenmat* target, unsigned int first_ind, unsigned int last_ind) {
  // source must be a vector
  if (source->size[0] > 1 && source->size[1] > 1)
    return ERROR_GENERIC;

  if (source->is_trans)
    return ERROR_TRANSPOSED;

  if (first_ind >= last_ind)
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  int num_rows = source->size[0];

  target->data = source->data + first_ind * num_rows;
  target->is_trans = 0;
  target->owns_data = 0;

  if (source->size[0] > 1) {
    if (last_ind > source->size[0])
      return ERROR_INCOMPATIBLE_DIMENSIONS;

    target->size[0] = last_ind - first_ind;
    target->size[1] = 1;
  } else {
    if (last_ind > source->size[1])
      return ERROR_INCOMPATIBLE_DIMENSIONS;

    target->size[0] = 1;
    target->size[1] = last_ind - first_ind;
  }

  return 0;
}

/* ------------------------------ Initialization routines ------------------------------ */

void init_from_array(eigenmat* mat, float* data, int m, int n) {
  mat->data = data;
  mat->size[0] = m;
  mat->size[1] = n;
  mat->is_trans = 0;
  mat->owns_data = 1;
}

int init_empty(eigenmat* mat, int m, int n) {
  mat->size[0] = m;
  mat->size[1] = n;
  mat->is_trans = 0;
  mat->owns_data = 1;

  return allocate_memory(mat);
}

/* ------------------------------ Random number generation ------------------------------ */

int init_random(rnd_struct_e* rnd_state, int seed)
{
    if (rnd_state->pGenerator)
    {
        delete rnd_state->pGenerator;
    }
    rnd_state->pGenerator = new default_random_engine(seed);
    return 0;
}

float uniform(rnd_struct_e* rnd_state)
{
    uniform_real_distribution<float> distribution(0, 1);
    return distribution(*rnd_state->pGenerator);
}

float normal(rnd_struct_e* rnd_state)
{
    normal_distribution<float> distribution(0, 1);
    return distribution(*rnd_state->pGenerator);
}

int fill_with_rand(rnd_struct_e* rnd_state, eigenmat* mat) {
  unsigned int len = mat->size[0] * mat->size[1];
  for (int i = 0; i < len; i++)
    mat->data[i] = uniform(rnd_state);

  return 0;
}

int fill_with_randn(rnd_struct_e* rnd_state, eigenmat* mat) {
  unsigned int len = mat->size[0] * mat->size[1];
  for (int i = 0; i < len; i++)
    mat->data[i] = normal(rnd_state);

  return 0;
}

int sample_bernoulli(rnd_struct_e* rnd_state, eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];
  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for (int i = 0; i < len; i++)
    target->data[i] = mat->data[i] > uniform(rnd_state) ? 1 : 0;

  return 0;
}

int sample_bernoulli_tanh(rnd_struct_e* rnd_state, eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];
  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for (int i = 0; i < len; i++)
    target->data[i] = mat->data[i] > uniform(rnd_state) ? 1 : -1;

  return 0;
}

int sample_gaussian(rnd_struct_e* rnd_state, eigenmat* mat, eigenmat* target, float mult) {
  unsigned int len = mat->size[0] * mat->size[1];
  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for (int i = 0; i < len; i++)
    target->data[i] = mat->data[i] + mult * normal(rnd_state);

  return 0;
}

int perturb_energy(rnd_struct_e* rnd_state, eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];
  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for (int i = 0; i < len; i++)
    target->data[i] = mat->data[i] - log(-log(uniform(rnd_state)));

  return 0;
}

int perturb_prob(rnd_struct_e* rnd_state, eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];
  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for (int i = 0; i < len; i++)
    target->data[i] = -mat->data[i] / log(uniform(rnd_state));

  return 0;
}

int dropout(rnd_struct_e* rnd_state, eigenmat* mat, float dropprob, float val, float scale)
{
    unsigned int len = mat->size[0] * mat->size[1];
    for (int i=0; i<len; i++)
    {
        if (dropprob > uniform(rnd_state))
        {
            mat->data[i] = val;
        } else
        {
            mat->data[i] *= scale;
        }
    }

    return 0;
}

/* ------------------------------ Algebraic operations ------------------------------ */

int add_col_vec(eigenmat* mat, eigenmat* vec, eigenmat* target)
{
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (mat->is_trans)
    return ERROR_TRANSPOSED;

  if (mat->size[0] != vec->size[0] || vec->size[1] != 1 ||
    mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXXf> eig_mat(mat->data, h, w);
  Eigen::Map<Eigen::ArrayXf> eig_vec(vec->data, h);
  Eigen::Map<Eigen::ArrayXXf> eig_target(target->data, h, w);

  eig_target = eig_mat.colwise() + eig_vec;

  return 0;
}

int add_mult_sign(eigenmat* mat, eigenmat* mat2, float mult) {
  for (int i = 0; i < mat->size[0] * mat->size[1]; i++) {
    mat->data[i] += (mat2->data[i] == 0) ? 0 : ((mat2->data[i] > 0) ? mult:-mult);
  }

  return 0;
}

int add_col_mult(eigenmat* mat, eigenmat* vec, eigenmat* target, float mult)
{
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (mat->is_trans)
    return ERROR_TRANSPOSED;

  if (mat->size[0] != vec->size[0] || vec->size[1] != 1 ||
    mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXXf> eig_mat(mat->data, h, w);
  Eigen::Map<Eigen::ArrayXf> eig_vec(vec->data, h);
  Eigen::Map<Eigen::ArrayXXf> eig_target(target->data, h, w);

  eig_target = eig_mat.colwise() + eig_vec * mult;

  return 0;
}

int add_to_each_pixel(eigenmat* mat1, eigenmat* mat2, eigenmat* target, float mult)
{
    unsigned int height = mat1->size[0];
    unsigned int width = mat1->size[1];
    unsigned int num_colors = mat2->size[1];
    unsigned int num_pix = height*width / num_colors;

    if (mat1->is_trans || mat2->is_trans)
        return ERROR_TRANSPOSED;

    if (mat1->size[0] != mat2->size[0] || mat1->size[1] % mat2->size[1] != 0 ||
        mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    #pragma omp parallel for
    for (unsigned int i=0; i<width*height; ++i)
    {
        target->data[i] = mat1->data[i] + mult * mat2->data[i % height + height * (i / num_pix)];
    }

    return 0;
}

int mult_diagonal_scalar(eigenmat* mat, float val, eigenmat* target)
{
    unsigned int w = mat->size[1];

    if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    #pragma omp parallel for
    for (int i=0; i<w; i++)
    {
        int idx = i*w+i;
        target->data[idx] = val * mat->data[idx];
    }

    return 0;
}

int add_diagonal_scalar(eigenmat* mat, float val, eigenmat* target)
{
    unsigned int w = mat->size[1];

    if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    #pragma omp parallel for
    for (int i=0; i<w; i++)
    {
        int idx = i*w+i;
        target->data[idx] = val + mat->data[idx];
    }

    return 0;
}

int mult_diagonal(eigenmat* mat, eigenmat* vec, eigenmat* target)
{
    unsigned int w = mat->size[1];

    if (mat->size[0] != vec->size[1] * vec->size[0] ||
        mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    #pragma omp parallel for
    for (int i=0; i<w; i++)
    {
        int idx = i*w+i;
        target->data[idx] = vec->data[i] * mat->data[idx];
    }

    return 0;
}

int add_diagonal(eigenmat* mat, eigenmat* vec, eigenmat* target)
{
    unsigned int w = mat->size[1];

    if (mat->size[0] != vec->size[1] * vec->size[0] ||
        mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    #pragma omp parallel for
    for(int i=0; i<w; i++)
    {
        int idx = i*w+i;
        target->data[idx] = vec->data[i] + mat->data[idx];
    }

    return 0;
}

int add_row_mult(eigenmat* mat, eigenmat* vec, eigenmat* target, float mult) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (mat->is_trans)
    return ERROR_TRANSPOSED;

  if (mat->size[1] != vec->size[1] || vec->size[0] != 1 ||
    mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXXf> eig_mat(mat->data, h, w);
  Eigen::Map<Eigen::ArrayXf> eig_vec(vec->data, w);
  Eigen::Map<Eigen::ArrayXXf> eig_target(target->data, h, w);

  eig_target = eig_mat.rowwise() + eig_vec.transpose() * mult;

  return 0;
}

int add_row_vec(eigenmat* mat, eigenmat* vec, eigenmat* target) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (mat->is_trans)
    return ERROR_TRANSPOSED;

  if (mat->size[1] != vec->size[1] || vec->size[0] != 1 ||
    mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXXf> eig_mat(mat->data, h, w);
  Eigen::Map<Eigen::ArrayXf> eig_vec(vec->data, w);
  Eigen::Map<Eigen::ArrayXXf> eig_target(target->data, h, w);

  eig_target = eig_mat.rowwise() + eig_vec.transpose();

  return 0;
}

int mult_by_col_vec(eigenmat* mat, eigenmat* vec, eigenmat* target) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (mat->is_trans)
    return ERROR_TRANSPOSED;

  if (mat->size[0] != vec->size[0] || vec->size[1] != 1 ||
    mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXXf> eig_mat(mat->data, h, w);
  Eigen::Map<Eigen::ArrayXf> eig_vec(vec->data, h);
  Eigen::Map<Eigen::ArrayXXf> eig_target(target->data, h, w);

  eig_target = eig_mat.colwise() * eig_vec;

  return 0;
}

int mult_by_row_vec(eigenmat* mat, eigenmat* vec, eigenmat* target) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (mat->is_trans)
    return ERROR_TRANSPOSED;

  if (mat->size[1] != vec->size[1] || vec->size[0] != 1 ||
    mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXXf> eig_mat(mat->data, h, w);
  Eigen::Map<Eigen::ArrayXf> eig_vec(vec->data, w);
  Eigen::Map<Eigen::ArrayXXf> eig_target(target->data, h, w);

  eig_target = eig_mat.rowwise() * eig_vec.transpose();

  return 0;
}

int div_by_col_vec(eigenmat* mat, eigenmat* vec, eigenmat* target) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (mat->is_trans)
    return ERROR_TRANSPOSED;

  if (mat->size[0] != vec->size[0] || vec->size[1] != 1 ||
    mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXXf> eig_mat(mat->data, h, w);
  Eigen::Map<Eigen::ArrayXf> eig_vec(vec->data, h);
  Eigen::Map<Eigen::ArrayXXf> eig_target(target->data, h, w);

  eig_target = eig_mat.colwise() / eig_vec;

  return 0;
}

int div_by_row_vec(eigenmat* mat, eigenmat* vec, eigenmat* target) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (mat->is_trans)
    return ERROR_TRANSPOSED;

  if (mat->size[1] != vec->size[1] || vec->size[0] != 1 ||
    mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXXf> eig_mat(mat->data, h, w);
  Eigen::Map<Eigen::ArrayXf> eig_vec(vec->data, w);
  Eigen::Map<Eigen::ArrayXXf> eig_target(target->data, h, w);

  eig_target = eig_mat.rowwise() / eig_vec.transpose();

  return 0;
}

int less_than(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0] * mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = mat1->data[i] < mat2->data[i] ? 1 : 0;

  return 0;
}

int less_than_scalar(eigenmat* mat, float val, eigenmat* target) {
  unsigned int len = mat->size[0]*mat->size[1];

  if (mat->is_trans != target->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = mat->data[i] < val ? 1 : 0;

  return 0;
}

int greater_than(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = mat1->data[i] > mat2->data[i] ? 1 : 0;

  return 0;
}

int upper_bound(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = mat1->data[i] < mat2->data[i] ? mat1->data[i] : mat2->data[i];

  return 0;
}

int lower_bound(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = mat1->data[i] > mat2->data[i] ? mat1->data[i] : mat2->data[i];
}

int greater_than_scalar(eigenmat* mat, float val, eigenmat* target) {
  unsigned int len = mat->size[0]*mat->size[1];

  if (mat->is_trans != target->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for (int i = 0; i < len; i++)
    target->data[i] = mat->data[i] > val ? 1:0;

  return 0;
}

int upper_bound_mod_scalar(eigenmat* mat, float val, eigenmat* target)
{
    unsigned int len = mat->size[0]*mat->size[1];

    if (mat->is_trans != target->is_trans)
      return ERROR_TRANSPOSEDNESS;

    if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
      return ERROR_INCOMPATIBLE_DIMENSIONS;

    for (int i=0; i<len; i++)
    {
        float curr = mat->data[i];
        if (curr > val)
        {
            target->data[i] = val;
        } else
        if (curr < -val)
        {
            target->data[i] = -val;
        } else
        {
            target->data[i] = curr;
        }
    }

    return 0;
}

int upper_bound_scalar(eigenmat* mat, float val, eigenmat* target) {
  unsigned int len = mat->size[0]*mat->size[1];

  if (mat->is_trans != target->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for (int i = 0; i < len; i++)
    target->data[i] = mat->data[i] < val ? mat->data[i] : val;

  return 0;
}

int lower_bound_scalar(eigenmat* mat, float val, eigenmat* target) {
  unsigned int len = mat->size[0]*mat->size[1];

  if (mat->is_trans != target->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for (int i = 0; i < len; i++)
    target->data[i] = mat->data[i] > val ? mat->data[i] : val;

  return 0;
}

int cumsum_by_axis(eigenmat* mat, eigenmat* target, int axis) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (axis == 0) {
    for (int i = 0; i < w; i++) {
      float *mat_data = &mat->data[i * h];
      float *target_data = &target->data[i * h];
      target_data[0] = mat_data[0];
      for (int j = 1; j < h; j++)
        target_data[j] =  target_data[j-1] + mat_data[j];
    }
  } else
  if (axis == 1) {
    for (int i = 0; i < h; i++) {
      float *mat_data = &mat->data[i];
      float *target_data = &target->data[i];
      target_data[0] = mat_data[0];
      for (int j = 1; j < w; j++)
        target_data[j*h] =  target_data[(j-1)*h] + mat_data[j*h];
    }
  } else {
    return ERROR_UNSUPPORTED;
  }

  return 0;
}

int max_by_axis(eigenmat* mat, eigenmat* target, int axis) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (axis == 0) {
    for (int i = 0; i < w; i++) {
      float *mat_data = &mat->data[i * h];
      float max = mat_data[0];
      for (int j = 1; j < h; j++)
        if (max < mat_data[j])
          max = mat_data[j];
      target->data[i] = max;
    }
  } else
  if (axis == 1) {
    for (int i = 0; i < h; i++) {
      float *mat_data = &mat->data[i];
      float max = mat_data[0];
      for (int j = 1; j < w; j++)
        if (max < mat_data[j*h])
          max = mat_data[j*h];
      target->data[i] = max;
    }
  } else {
    return ERROR_UNSUPPORTED;
  }

  return 0;
}

int choose_max_and_accumulate(eigenmat* mat, eigenmat* acc) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (mat->is_trans)
    return ERROR_TRANSPOSED;

  if (acc->size[0] != mat->size[0] || acc->size[1] != mat->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for (int i = 0; i < w; i++) {
    int argmax = 0;
    float *mat_data = &mat->data[i * h];
    for (int j = 1; j < h; j++)
      if (mat_data[argmax] < mat_data[j])
        argmax = j;
    acc->data[i * h + argmax] += 1;
  }

  return 0;
}

int choose_max_by_axis(eigenmat* mat, eigenmat* target, int axis) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (axis == 0) {
    for (int i = 0; i < w; i++) {
      int argmax = 0;
      float *mat_data = &mat->data[i * h];
      float *target_data = &target->data[i * h];
      for (int j = 1; j < h; j++)
        if (mat_data[argmax] < mat_data[j])
          argmax = j;
      for (int j = 0; j < h; j++)
        target_data[j] = (j == argmax) ? 1 : 0;
    }
  } else
  if (axis == 1) {
    for (int i = 0; i < h; i++) {
      int argmax = 0;
      float *mat_data = &mat->data[i];
      float *target_data = &target->data[i];
      for (int j = 1; j < w; j++)
        if (mat_data[argmax*h] < mat_data[j*h])
          argmax = j;
      for (int j = 0; j < w; j++)
        target_data[j*h] = (j == argmax) ? 1 : 0;
    }
  } else {
    return ERROR_UNSUPPORTED;
  }

  return 0;
}

int argmax_by_axis(eigenmat* mat, eigenmat* target, int axis) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (axis == 0) {
    for (int i = 0; i < w; i++) {
      int argmax = 0;
      float *mat_data = &mat->data[i * h];
      for (int j = 1; j < h; j++)
        if (mat_data[argmax] < mat_data[j])
          argmax = j;
      target->data[i] = argmax;
    }
  } else
  if (axis == 1) {
    for (int i = 0; i < h; i++) {
      int argmax = 0;
      float *mat_data = &mat->data[i];
      for (int j = 1; j < w; j++)
        if (mat_data[argmax*h] < mat_data[j*h])
          argmax = j;
      target->data[i] = argmax;
    }
  } else {
    return ERROR_UNSUPPORTED;
  }

  return 0;
}

int sqsum_by_axis(eigenmat* mat, eigenmat* target, int axis, float mult, float p) {
  unsigned int h = mat->size[0];
  unsigned int w = mat->size[1];

  if (axis == 0) {
    for (int i = 0; i < w; i++) {
      float sum = 0;
      float *mat_data = &mat->data[i * h];
      for (int j = 0; j < h; j++)
        sum += mat_data[j] * mat_data[j];
      target->data[i] = p * target->data[i] + mult * sum;
    }
  } else
  if (axis == 1) {
    for (int i = 0; i < h; i++) {
      float sum = 0;
      float *mat_data = &mat->data[i];
      for (int j = 0; j < w; j++)
        sum += mat_data[j*h] * mat_data[j*h];
      target->data[i] = p * target->data[i] + mult * sum;
    }
  } else {
    return ERROR_UNSUPPORTED;
  }

  return 0;
}

int sum_by_axis(eigenmat* mat, eigenmat* target, int axis, float mult, float p)
{
    unsigned int len_target = target->size[0] * target->size[1];

    Eigen::Map<Eigen::ArrayXXf> eig_mat(mat->data, mat->size[0], mat->size[1]);
    Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len_target);

    if (mat->is_trans)
        return ERROR_TRANSPOSED;

    if (axis == 0)
    {
        if (target->size[0] != 1 || target->size[1] != mat->size[1])
            return ERROR_INCOMPATIBLE_DIMENSIONS;

        eig_target = p * eig_target.transpose() + mult * eig_mat.colwise().sum();
    } else
    if (axis == 1)
    {
        if (target->size[1] != 1 || target->size[0] != mat->size[0])
            return ERROR_INCOMPATIBLE_DIMENSIONS;

        eig_target = p * eig_target + mult * eig_mat.rowwise().sum();
    } else
    {
        return ERROR_UNSUPPORTED;
    }

    return 0;
}

int normlimit_by_axis(eigenmat* mat, eigenmat* target, int axis, float norm, int constraint)
{
    unsigned int height = mat->size[0];
    unsigned int width = mat->size[1];

    if (mat->is_trans)
        return ERROR_TRANSPOSED;

    if (target->size[0] != mat->size[0] || target->size[1] != mat->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (axis == 0)
    {
        for (int column=0; column<width; ++column)
        {
            float cur_sum = 0;
            float *cur_data = &mat->data[column * height];
            float *target_data = &target->data[column * height];
            for (unsigned int i=0; i<height; ++i)
            {
                cur_sum += cur_data[i] * cur_data[i];
            }
            cur_sum = sqrt(cur_sum);
            cur_sum = (constraint == 1 || cur_sum > norm) ? (norm / cur_sum) : 1;
            for (unsigned int i=0; i<height; ++i)
            {
                target_data[i] = cur_data[i] * cur_sum;
            }
        }
    } else
    {
        for (int row=0; row<height; ++row)
        {
            float cur_sum = 0;
            float *cur_data = &mat->data[row];
            float *target_data = &target->data[row];
            for (unsigned int i=0; i<width; ++i)
            {
                cur_sum += cur_data[i * height] * cur_data[i * height];
            }
            cur_sum = sqrt(cur_sum);
            cur_sum = (constraint == 1 || cur_sum > norm) ? (norm / cur_sum) : 1;
            for (unsigned int i=0; i<width; ++i)
            {
                target_data[i * height] = cur_data[i * height] * cur_sum;
            }
        }
    }

    return 0;
}

int normalize_by_axis(eigenmat* mat, eigenmat* target, int axis)
{
    unsigned int height = mat->size[0];
    unsigned int width = mat->size[1];

    if (mat->is_trans)
        return ERROR_TRANSPOSED;

    if (target->size[0] != mat->size[0] || target->size[1] != mat->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (axis == 0)
    {
        for (int column=0; column<width; ++column)
        {
            float cur_sum = 0;
            float *cur_data = &mat->data[column * height];
            float *target_data = &target->data[column * height];
            for (unsigned int i=0; i<height; ++i)
            {
                cur_sum += cur_data[i];
            }

            cur_sum /= height;
            for (unsigned int i=0; i<height; ++i)
            {
                target_data[i] = cur_data[i] - cur_sum;
            }
        }
    } else
    {
        return ERROR_UNSUPPORTED;
    }

    return 0;
}

int sign(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0]*mat->size[1];

  if (mat->is_trans != target->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for(int i = 0; i < len; i++)
    target->data[i] = (mat->data[i] < 0) ? -1 : (mat->data[i] > 0 ? 1 : 0);

  return 0;
}

int apply_cos(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat.cos();

  return 0;
}

int apply_sin(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat.sin();

  return 0;
}

int softmax(eigenmat* mat, eigenmat* target)
{
    unsigned int width = mat->size[1];
    unsigned int height = mat->size[0];

    if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (mat->is_trans)
        return ERROR_TRANSPOSED;

    #pragma omp parallel for
    for (int i=0; i<width; i++)
    {
        float *mat_data = &mat->data[i * height];
        float *target_data = &target->data[i * height];
        float max = mat_data[0], sum = 0;
        for (int j=1; j<height; j++)
        {
            if (max < mat_data[j])
            {
                max = mat_data[j];
            }
        }

        for (int j=0; j<height; j++)
        {
            target_data[j] = exp(mat_data[j] - max);
            sum += target_data[j];
        }

        for (int j=0; j<height; j++)
        {
            target_data[j] /= sum;
        }
    }

    return 0;
}

int softmax_row_major(eigenmat* mat)
{
    return softmax_row_major_multi(mat, mat->size[1]);
}

int softmax_row_major_multi(eigenmat* mat, int numslices)
{
    unsigned int len = mat->size[0] * mat->size[1];
    unsigned int height = len / numslices;
    unsigned int width = numslices;

    if (len % numslices != 0)
      return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (mat->is_trans)
        return ERROR_TRANSPOSED;

    #pragma omp parallel for
    for (int i=0; i<height; i++)
    {
        float max = mat->data[i], sum = 0;
        for (int j=1; j<width; j++)
        {
            float curr = mat->data[j * height + i];
            if (max < curr)
            {
                max = curr;
            }
        }

        for (int j=0; j<width; j++)
        {
            int idx = j * height + i;
            mat->data[idx] = exp(mat->data[idx] - max);
            sum += mat->data[idx];
        }
        for (int j=0; j<width; j++)
        {
            mat->data[j * height + i] /= sum;
        }
    }

    return 0;
}

int apply_logistic_grad(eigenmat* mat1, eigenmat* mat2, eigenmat* out_grad)
{
    unsigned int len = mat1->size[0]*mat1->size[1];

    if (mat1->is_trans != mat2->is_trans)
        return ERROR_TRANSPOSEDNESS;

    if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
        mat1->size[0] != out_grad->size[0] || mat1->size[1] != out_grad->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    #pragma omp parallel for
    for (unsigned int i=0; i<len; ++i)
    {
        out_grad->data[i] = (mat2->data[i] < 0) ? 0 : (mat1->data[i] - mat2->data[i]);
    }

    return 0;
}

int apply_softmax_grad(eigenmat* mat, eigenmat* labels, eigenmat* target)
{
    unsigned int width = mat->size[1];
    unsigned int height = mat->size[0];

    if (target != mat)
    {
        #pragma omp parallel for
        for (int i=0; i<width*height; i++)
        {
            target->data[i] = mat->data[i];
        }
    }
    #pragma omp parallel for
    for (int i=0; i<width; i++)
    {
        target->data[i * height + (int)labels->data[i]] -= 1.0;
    }

    return 0;
}

int apply_softmax_grad_row_major(eigenmat* mat, eigenmat* labels, eigenmat* target)
{
    unsigned int width = mat->size[1];
    unsigned int height = mat->size[0];

    if (target != mat)
    {
        #pragma omp parallel for
        for (int i=0; i<width*height; i++)
        {
            target->data[i] = mat->data[i];
        }
    }
    #pragma omp parallel for
    for (int i=0; i<height; i++)
    {
        target->data[i + height*(int)labels->data[i]] -= 1.0;
    }

    return 0;
}

int get_softmax_cross_entropy(eigenmat* mat, eigenmat* labels, eigenmat* target, float tiny)
{
    unsigned int width = mat->size[1];
    unsigned int height = mat->size[0];

    #pragma omp parallel for
    for (int i=0; i<width; i++)
    {
        target->data[i] = -log(mat->data[i * height + (int)labels->data[i]]);
    }

    return 0;
}

int get_softmax_cross_entropy_row_major(eigenmat* mat, eigenmat* labels, eigenmat* target, float tiny)
{
    unsigned int height = mat->size[0];
    unsigned int width = mat->size[1];

    if (mat->is_trans)
        return ERROR_TRANSPOSED;

    if (target->size[0] != height || target->size[1] != 1)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (labels->size[0] != height || labels->size[1] != 1)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    #pragma omp parallel for
    for (unsigned int i=0; i<height; ++i)
    {
        target->data[i] = -log(mat->data[height * (int)labels->data[i] + i] + tiny);
    }

    return 0;
}

int get_softmax_correct(eigenmat* mat, eigenmat* labels, eigenmat* target)
{
    unsigned int width = mat->size[1];
    unsigned int height = mat->size[0];

    #pragma omp parallel for
    for (int i=0; i<width; i++)
    {
        int argmax = 0;
        int correct_label = (int)labels->data[i];
        float *mat_data = &mat->data[i * height];
        for (int j=1; j<height; j++)
        {
            if (mat_data[argmax] < mat_data[j])
            {
                argmax = j;
            }
        }
        target->data[i] = (correct_label == argmax) ? 1:0;
    }

    return 0;
}

int get_softmax_correct_row_major(eigenmat* mat, eigenmat* labels, eigenmat* target)
{
    unsigned int width = mat->size[1];
    unsigned int height = mat->size[0];

    if (mat->is_trans)
        return ERROR_TRANSPOSED;

    if (target->size[0] != height || target->size[1] != 1)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (labels->size[0] * labels->size[1] != height)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    #pragma omp parallel for
    for (int i=0; i<height; i++)
    {
        int argmax = 0;
        int correct_label = (int)labels->data[i];
        float *mat_data = &mat->data[i];
        for (int j=1; j<width; j++)
        {
            if (mat_data[argmax*height] < mat_data[j*height])
            {
                argmax = j;
            }
        }
        target->data[i] = (correct_label == argmax) ? 1:0;
    }

    return 0;
}

int hinge_loss_row_major(eigenmat* mat, eigenmat* labels, eigenmat* target, int quadratic, float margin)
{
    unsigned int height = mat->size[0];
    unsigned int width = mat->size[1];

    if (mat->is_trans)
        return ERROR_TRANSPOSED;

    if (target->size[0] != height || target->size[1] != width)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (labels->size[0] * labels->size[1] != height)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (quadratic == 1)
    {
        #pragma omp parallel for
        for (int j=0; j<height; j++)
        {
            int correct_label = (int)labels->data[j];
            float correct_label_score = mat->data[correct_label*height + j];
            float sum = 0;
            for (unsigned int i=0; i<width; i++)
            {
                float diff = margin + mat->data[i*height + j] - correct_label_score;
                float grad = (diff > 0) ? diff : 0;
                target->data[i*height + j] = (i == correct_label) ? 0 : grad;
                sum += (i == correct_label) ? 0 : grad;
            }
            target->data[correct_label*height + j] = -sum;
        }
    } else
    {
        #pragma omp parallel for
        for (int j=0; j<height; j++)
        {
            int correct_label = (int)labels->data[j];
            float correct_label_score = mat->data[correct_label*height + j];
            float sum = 0;
            for (unsigned int i=0; i<width; i++)
            {
                float diff = margin + mat->data[i*height + j] - correct_label_score;
                float grad = (diff > 0) ? 1 : 0;
                target->data[i*height + j] = (i == correct_label) ? 0 : grad;
                sum += (i == correct_label) ? 0 : grad;
            }
            target->data[correct_label*height + j] = -sum;
        }
    }

    return 0;
}

int get_logistic_correct_normalized(eigenmat* mat1, eigenmat* mat2, eigenmat* out)
{
    unsigned int width = mat1->size[1];
    unsigned int height = mat1->size[0];

    if (mat1->is_trans != mat2->is_trans)
        return ERROR_TRANSPOSEDNESS;

    if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
        mat1->size[0] != out->size[0] || 1 != out->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    #pragma omp parallel for
    for (int j=0; j<height; ++j)
    {
        float correct = 0;
        float total = 0;
        float p, t;
        for (int i=0; i<width; ++i)
        {
            p = mat1->data[i*height + j];
            t = mat2->data[i*height + j];
            correct += (t < 0) ? 0 : (((t >= 0.5 && p >= 0.5) || (t < 0.5 && p < 0.5)) ? 1:0);
            total += (t < 0) ? 0 : 1;
        }
        out->data[j] = (total > 0) ? (correct / total) : 0;
    }

    return 0;
}

float sum_all(eigenmat* mat) {
  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, mat->size[0] * mat->size[1]);

  return eig_mat.sum();
}

int apply_sigmoid(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = 1 / (1 + exp(-mat->data[i]));

  return 0;
}

int apply_tanh(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = 2 / (1 + exp(-mat->data[i])) - 1;

  return 0;
}

int apply_abs(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat.abs();

  return 0;
}

int apply_log_1_plus_exp(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  for (int i = 0; i < len; i++)
    target->data[i] = log(1+exp(mat->data[i]));

  return 0;
}

// target = 2 / (1 + exp(-mat * lambda)) - 1
int apply_relu_squash(eigenmat* mat, eigenmat* target, float lambda)
{
    unsigned int len = mat->size[0] * mat->size[1];

    if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    #pragma omp parallel for
    for (unsigned int i=0; i<len; ++i)
    {
        target->data[i] = 2 / (1 + exp(-lambda * mat->data[i])) - 1;
    }

    return 0;
}

int apply_log(eigenmat* mat, eigenmat* target, float tiny) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat.log();

  return 0;
}

int apply_exp(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat.exp();

  return 0;
}

int apply_ceil(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for(int i = 0; i < len; i++)
    target->data[i] = ceil(mat->data[i]);

  return 0;
}

int apply_floor(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for(int i = 0; i < len; i++)
    target->data[i] = floor(mat->data[i]);

  return 0;
}

int apply_sqrt(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat.sqrt();

  return 0;
}

int apply_pow(eigenmat* mat, float exponent, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat.pow(exponent);

  return 0;
}

int apply_pow_matrix(eigenmat* mat, eigenmat* exponent, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  if (mat->size[0] != exponent->size[0] || mat->size[1] != exponent->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = pow(mat->data[i], exponent->data[i]);

  return 0;
}

int compute_cross_entropy(eigenmat* mat1, eigenmat* mat2, eigenmat* target, float tiny) {
  unsigned int len = mat1->size[0] * mat1->size[1];

  if (mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  if (mat2->size[0] != mat2->size[0] || mat2->size[1] != mat2->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = - mat1->data[i] * log(mat2->data[i] + tiny);

  return 0;
}

int compute_cross_entropy_bernoulli(eigenmat* mat1, eigenmat* mat2, eigenmat* target, float tiny) {
  unsigned int len = mat1->size[0] * mat1->size[1];

  if (mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = - mat1->data[i] * log(mat2->data[i] + tiny) - (1-mat1->data[i]) * log(1 - mat2->data[i] + tiny);

  return 0;
}

int correct_preds(eigenmat* mat1, eigenmat* mat2, eigenmat* target, float cutoff) {
  unsigned int len = mat1->size[0] * mat1->size[1];

  if (mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = mat1->data[i] * mat2->data[i] >= cutoff + (1 - mat1->data[i]) * (mat2->data[i] < cutoff);

  return 0;
}

int reciprocal(eigenmat* mat, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];

  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);

  eig_target = eig_mat.inverse();

  return 0;
}

int dot(eigenmat* mat1, eigenmat* mat2, eigenmat* target, float beta, float alpha) {

  if (get_leading_dimension(mat1) != get_leading_dimension(target) ||
    get_nonleading_dimension(mat2) != get_nonleading_dimension(target) ||
    get_nonleading_dimension(mat1) != get_leading_dimension(mat2)) {
    return ERROR_INCOMPATIBLE_DIMENSIONS;
  }
  int m = get_leading_dimension(mat1),
    k = get_leading_dimension(mat2),
    n = get_nonleading_dimension(mat2);

  Eigen::Map<Eigen::MatrixXf> eig_mat1(mat1->data, mat1->size[0], mat1->size[1]);
  Eigen::Map<Eigen::MatrixXf> eig_mat2(mat2->data, mat2->size[0], mat2->size[1]);
  Eigen::Map<Eigen::MatrixXf> eig_target(target->data, target->size[0], target->size[1]);

  eig_target = beta * eig_target;
  if (mat1->is_trans && mat2->is_trans) {
    eig_target.noalias() += alpha * (eig_mat1.transpose() * eig_mat2.transpose());
  } else if (mat1->is_trans) {
    eig_target.noalias() += alpha * (eig_mat1.transpose() * eig_mat2);
  } else if (mat2->is_trans) {
    eig_target.noalias() += alpha * (eig_mat1 * eig_mat2.transpose());
  } else {
    eig_target.noalias() += alpha * (eig_mat1 * eig_mat2);
  }

  return 0;
}

float vdot(eigenmat* mat1, eigenmat* mat2, int* err_code) {
  unsigned int len = mat1->size[0]*mat1->size[1];
  float res;

  if (mat1->is_trans != mat2->is_trans) {
    *err_code = ERROR_TRANSPOSEDNESS;
    return 0;
  }

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1]) {
    *err_code = ERROR_INCOMPATIBLE_DIMENSIONS;
    return 0;
  }

  Eigen::Map<Eigen::VectorXf> eig_mat1(mat1->data, len);
  Eigen::Map<Eigen::VectorXf> eig_mat2(mat2->data, len);

  return eig_mat1.dot(eig_mat2);
}

/* Perform the operation mat1 = mat1 + alpha * mat2. mat1 and mat2 must
  have the same transposedness. */
int add_mult(eigenmat* mat1, eigenmat* mat2, float alpha) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat1(mat1->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_mat2(mat2->data, len);
  eig_mat1 += eig_mat2 * alpha;

  return 0;
}

int add_elementwise(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat1(mat1->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_mat2(mat2->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat1 + eig_mat2;

  return 0;
}

int subtract_elementwise(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat1(mat1->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_mat2(mat2->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat1 - eig_mat2;

  return 0;
}

int divide_elementwise(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;
  Eigen::Map<Eigen::ArrayXf> eig_mat1(mat1->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_mat2(mat2->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat1 / eig_mat2;

  return 0;
}

/* Elementwise multiplication of 2 matrices */
int mult_elementwise(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat1(mat1->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_mat2(mat2->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat1 * eig_mat2;

  return 0;
}

int apply_sin_deriv(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  return 0;
}

int apply_cos_deriv(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  return 0;
}

int apply_logistic_deriv(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = mat1->data[i] * mat2->data[i] * (1-mat2->data[i]);

  return 0;
}

int apply_tanh_deriv(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = 0.5 * mat1->data[i] * (1 + mat2->data[i]) * (1 - mat2->data[i]);

  return 0;
}

int apply_rectified_linear_deriv(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = mat1->data[i] * (mat2->data[i] > 0 ? 1 : 0);

  return 0;
}

int apply_rectified_linear_smooth_deriv(eigenmat* mat1, eigenmat* mat2, eigenmat* target) {
  unsigned int len = mat1->size[0]*mat1->size[1];

  if (mat1->is_trans != mat2->is_trans)
    return ERROR_TRANSPOSEDNESS;

  if (mat1->size[0] != mat2->size[0] || mat1->size[1] != mat2->size[1] ||
    mat1->size[0] != target->size[0] || mat1->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  #pragma omp parallel for
  for (int i = 0; i < len; i++)
    target->data[i] = mat1->data[i] * (1 - exp(-mat2->data[i]));

  return 0;
}

int write_at(eigenmat* mat, int row, int col, float val)
{
    if (row >= mat->size[0] || col >= mat->size[1] || row < 0 || col < 0)
      return ERROR_INCOMPATIBLE_DIMENSIONS;

    mat->data[col * mat->size[0] + row] = val;

    return 0;
}

float read_from(eigenmat* mat, int row, int col, int* err_code)
{
    *err_code = 0;
    if (row >= mat->size[0] || col >= mat->size[1] || row < 0 || col < 0)
        *err_code = ERROR_INCOMPATIBLE_DIMENSIONS;

    return mat->data[col * mat->size[0] + row];
}

int assign_scalar(eigenmat* mat, float alpha) {
  unsigned int len = mat->size[0]*mat->size[1];
  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  eig_mat.setConstant(alpha);

  return 0;
}

int mult_by_scalar(eigenmat* mat, float alpha, eigenmat* target) {
  unsigned int len = mat->size[0]*mat->size[1];
  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat * alpha;

  return 0;
}

int divide_by_scalar(eigenmat* mat, float alpha, eigenmat* target) {
  unsigned int len = mat->size[0]*mat->size[1];
  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat / alpha;

  return 0;
}

int add_scalar(eigenmat* mat, float alpha, eigenmat* target) {
  unsigned int len = mat->size[0] * mat->size[1];
  if (mat->size[0] != target->size[0] || mat->size[1] != target->size[1])
    return ERROR_INCOMPATIBLE_DIMENSIONS;

  Eigen::Map<Eigen::ArrayXf> eig_mat(mat->data, len);
  Eigen::Map<Eigen::ArrayXf> eig_target(target->data, len);
  eig_target = eig_mat + alpha;

  return 0;
}

float euclid_norm(eigenmat* mat) {
  unsigned int len = mat->size[0]*mat->size[1];
  Eigen::Map<Eigen::VectorXf> eig_mat(mat->data, len);

  return eig_mat.norm();
}

int selectCols(eigenmat* source, eigenmat* target, eigenmat* indices){
  unsigned int n = indices->size[1] * indices->size[0];
  unsigned int h = source->size[0];

  int target_offset, source_offset;
  #pragma omp parallel for
  for (int i = 0; i < n; i++) {
    target_offset = i * h;
    source_offset = (int)indices->data[i] * h;
    for (int j = 0; j < h; j++) {
      target->data[target_offset + j] = source->data[source_offset + j];
    }
  }

  return 0;
}

int selectRows(eigenmat* source, eigenmat* target, eigenmat* indices){
  unsigned int n = indices->size[1] * indices->size[0];
  unsigned int w = source->size[1];
  unsigned int h_source = source->size[0];
  unsigned int h_target = target->size[0];

  int target_offset, source_offset;
  #pragma omp parallel for
  for (int j = 0; j < w; j++) {
    for (int i = 0; i < n; i++) {
      target->data[j * h_target + i] = source->data[j * h_source + (int)indices->data[i]];
    }
  }

  return 0;
}

int swapColumns(eigenmat* source, eigenmat* target, eigenmat* indices1, eigenmat* indices2){
  unsigned int n = indices1->size[1] * indices1->size[0];
  unsigned int h = source->size[0];

  int target_offset, source_offset;
  #pragma omp parallel for
  for (int i = 0; i < n; i++) {
    source_offset = (int)indices1->data[i] * h;
    target_offset = (int)indices2->data[i] * h;
    for (int j = 0; j < h; j++) {
      float temp = target->data[target_offset + j];
      target->data[target_offset + j] = source->data[source_offset + j];
      source->data[source_offset + j] = temp;
    }
  }

  return 0;
}

int shuffleColumns(eigenmat* source, eigenmat* rand_perm_indices)
{
    unsigned int height = source->size[0];
    unsigned int width = source->size[1];

    if (rand_perm_indices->size[0] != 1 || rand_perm_indices->size[1] != width)
    {
        return ERROR_INCOMPATIBLE_DIMENSIONS;
    }

    float *indices = rand_perm_indices->data;
    float *src = source->data;
    unsigned int column, row, pos1, pos2;
    for (unsigned int i=0; i<height*((width+1)/2); ++i)
    {
        column = 2 * (i / height);
        row = i % height;
        pos1 = height * (int)indices[column] + row;
        pos1 = height * (int)indices[column] + row;
        if (column + 1 < width)
        {
            pos2 = height * (int)indices[column + 1] + row;
            swap(src[pos1], src[pos2]);
        }
    }

    return 0;
}

int swapRows(eigenmat* source, eigenmat* target, eigenmat* indices1, eigenmat* indices2){
  unsigned int n = indices1->size[1] * indices1->size[0];
  unsigned int w = source->size[1];
  unsigned int h_source = source->size[0];
  unsigned int h_target = target->size[0];

  int target_offset, source_offset;
  #pragma omp parallel for
  for (int j = 0; j < w; j++) {
    for (int i = 0; i < n; i++) {
      int source_index = j * h_source + (int)indices1->data[i];
      int target_index = j * h_target + (int)indices2->data[i];
      float temp = target->data[target_index];
      target->data[target_index] = source->data[source_index];
      source->data[source_index] = temp;
    }
  }

  return 0;
}

int setSelectedCols(eigenmat* source, eigenmat* target, eigenmat* indices)
{
  unsigned int n = indices->size[1] * indices->size[0];
  unsigned int h = source->size[0];

  int target_offset, source_offset;
  #pragma omp parallel for
  for (int i = 0; i < n; i++) {
    source_offset = i * h;
    target_offset = (int)indices->data[i] * h;
    for (int j = 0; j < h; j++) {
      target->data[target_offset + j] = source->data[source_offset + j];
    }
  }

  return 0;
}

int setSelectedRows(eigenmat* source, eigenmat* target, eigenmat* indices)
{
  unsigned int n = indices->size[1] * indices->size[0];
  unsigned int w = source->size[1], h_source = source->size[0], h_target = target->size[0];

  int target_offset, source_offset;
  #pragma omp parallel for
  for (int j = 0; j < w; j++) {
    for (int i = 0; i < n; i++) {
      target->data[j * h_target + (int)indices->data[i]] = source->data[j * h_source + i];
    }
  }

  return 0;
}

int extract_patches(eigenmat* images, eigenmat* patches, eigenmat* width_offset,
                    eigenmat* height_offset, eigenmat* flip, int img_width,
                    int img_height, int patch_width, int patch_height)
{
    unsigned int num_images = images->size[1];
    unsigned int num_colors = images->size[0] / (img_width * img_height);

    if (patches->size[1]  != num_colors * patch_width * patch_height || patches->size[0] != num_images)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (width_offset->size[0] * width_offset->size[1] != num_images)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (height_offset->size[0] * height_offset->size[1] != num_images)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (flip->size[0] * flip->size[1] != num_images)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    for (int image_id=0; image_id<num_images; ++image_id)
    {
        for (int dest_col=0; dest_col<patch_width; ++dest_col)
        {
            int source_col = int(width_offset->data[image_id]) + dest_col;
            if (flip->data[image_id] > 0.5)
            {
                source_col = (img_width - source_col - 1);
            }

            for (int dest_row=0; dest_row<patch_height; ++dest_row)
            {
                int source_row = int(height_offset->data[image_id]) + dest_row;
                for (int color=0; color<num_colors; ++color)
                {
                    unsigned long dest_index = image_id + num_images * (dest_col  + patch_width * (dest_row + patch_height * color));
                    unsigned long source_index = source_col + img_width * (source_row + img_height * (color + num_colors * image_id));

                    patches->data[dest_index] = images->data[source_index];
                }
            }
        }
    }

    return 0;
}

int rectify_bounding_boxes(eigenmat* boxes, eigenmat* width_offset, eigenmat* height_offset,
                           eigenmat* flip, int patch_width, int patch_height)
{
    int num_images = boxes->size[0];

    if (width_offset->size[0] * width_offset->size[1] != num_images)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (height_offset->size[0] * height_offset->size[1] != num_images)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    if (flip->size[0] * flip->size[1] != num_images)
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    int num_locs = boxes->size[1] / 4;

    float *boxesd = boxes->data;
    float *width_offsetd = width_offset->data;
    float *height_offsetd = height_offset->data;
    float *flipd = flip->data;
    for (int loc_id=0; loc_id<num_locs; ++loc_id)
    {
        float *xmin_block = boxesd + num_images * loc_id;
        float *ymin_block = boxesd + num_images * (loc_id + num_locs    );
        float *xmax_block = boxesd + num_images * (loc_id + num_locs * 2);
        float *ymax_block = boxesd + num_images * (loc_id + num_locs * 3);

        for (int image_id=0; image_id<num_images; ++image_id)
        {
            float xmin = (flipd[image_id] > 0.5) ? (256.0/patch_width - xmax_block[image_id]) : xmin_block[image_id];
            float xmax = (flipd[image_id] > 0.5) ? (256.0/patch_width - xmin_block[image_id]) : xmax_block[image_id];
            float ymin = ymin_block[image_id];
            float ymax = ymax_block[image_id];
            float wo = width_offsetd[image_id];
            float ho = height_offsetd[image_id];

            xmin_block[image_id] = xmin - wo / patch_width;
            xmax_block[image_id] = xmax - wo / patch_width;

            ymin_block[image_id] = ymin - ho / patch_height;
            ymax_block[image_id] = ymax - ho / patch_height;
        }
    }

    return 0;
}

int adagrad(eigenmat* history, eigenmat* grad, float delta)
{
    unsigned int len = history->size[0] * history->size[1];

    if (history->is_trans != grad->is_trans)
        return ERROR_TRANSPOSEDNESS;

    if (len != grad->size[0] * grad->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    float *history_ = history->data;
    float *grad_ = grad->data;
    for (unsigned int i=0; i<len; ++i)
    {
        float curr_norm = history_[i] - delta;
        history_[i] = delta + sqrt(curr_norm * curr_norm + grad_[i] * grad_[i]);
    }

    return 0;
}

int rms_prop(eigenmat* history, eigenmat* grad, float factor)
{
    unsigned int len = history->size[0] * history->size[1];

    if (history->is_trans != grad->is_trans)
        return ERROR_TRANSPOSEDNESS;

    if (len != grad->size[0] * grad->size[1])
        return ERROR_INCOMPATIBLE_DIMENSIONS;

    float *history_ = history->data;
    float *grad_ = grad->data;
    for (unsigned int i=0; i<len; ++i)
    {
        history_[i] = sqrt(factor * history_[i] * history_[i] + (1-factor) * grad_[i] * grad_[i]);
    }

    return 0;
}

int blockify(eigenmat* source, eigenmat* target, int blocksize) {
  unsigned int w = source->size[1];
  unsigned int h = source->size[0];

  #pragma omp parallel for
  for (int i = 0; i < w; i++) {
    const int off = i * h;
    for (int j = 0; j < h; j++) {
      target->data[off + j] = source->data[off + (j / blocksize) * blocksize];
    }
  }

  return 0;
}

/*inline float tanh(float x)
{
    return (1.0f - exp(-x)) / (1.0f + exp(-x));
}*/

inline float relu(float x)
{
    return ((x > 0) ? x : 0);
}

inline float sigmoid(float x)
{
    return 1.0f / (1.0f + exp(-x));
}

inline float deriv_of_relu(float y)
{
    return ((y > 0) ? 1 : 0);
}

inline float deriv_of_sigmoid(float y)
{
    return y * (1 - y);
}

inline float deriv_of_tanh(float y)
{
    return 1 - y*y;
}

void sgemm(bool rowMajor, bool TransA, bool TransB, int M, int N, int K, float alpha, float* A, int lda, float* B, int ldb, float beta, float* C, int ldc)
{
#ifdef USE_OPENBLAS
    CBLAS_TRANSPOSE ta;
    if (TransA)
    {
        ta = CblasTrans;
    } else
    {
        ta = CblasNoTrans;
    }
    CBLAS_TRANSPOSE tb;
    if (TransB)
    {
        tb = CblasTrans;
    } else
    {
        tb = CblasNoTrans;
    }
    CBLAS_ORDER order;
    if (rowMajor)
    {
        order = CblasRowMajor;
    } else
    {
        order = CblasColMajor;
    }

    cblas_sgemm(order, ta, tb, M, N, K, alpha, A, lda, B, ldb, beta, C, ldc);
#else
    int dia, dla;
    if (TransA)
    {
        dia = 1;
        dla = lda;
    } else
    {
        dia = lda;
        dla = 1;
    }
    int djb, dlb;
    if (TransB)
    {
        djb = ldb;
        dlb = 1;
    } else
    {
        djb = 1;
        dlb = ldb;
    }
    int dic = 1, djc = ldc;

    if (!rowMajor)
    {
        std::swap(dia, dla);
        std::swap(djb, dlb);
        std::swap(dic, djc);
    }

    for (int i=0, ia=0, ic=0; i<M; ++i, ia+=dia, ic+=djc)
    {
        for (int j=0, jb=0, jc=0; j<N; ++j, jb+=djb, jc+=dic)
        {
            float res = 0;
            for (int l=0, la=0, lb=0; l<K; ++l, la+=dla, lb+=dlb)
            {
                res += A[la + ia] * B[lb + jb];
            }
            int target_ind = jc + ic;
            C[target_ind] = beta * C[target_ind] + alpha * res;
        }
    }
#endif
}

int lstm_fprop(eigenmat* s_in, eigenmat* s_out, eigenmat* w_dense, eigenmat* w_diag, eigenmat* b, bool init, bool use_relu)
{
    unsigned int numcases = s_in->size[0];
    unsigned int num_lstms = s_in->size[1] / 6;
    unsigned int numEls = numcases * num_lstms;

    if (!init)
    {
        sgemm(false, false, true,
              numcases, 4 * num_lstms, num_lstms,
              1, s_in->data, numcases,
                 w_dense->data, 4 * num_lstms,
              1, s_out->data + numcases * num_lstms * 2, numcases);
    }

    float *h_out = s_out->data;
    float *c_out = s_out->data +     numEls;
    float *i_out = s_out->data + 2 * numEls;
    float *f_out = s_out->data + 3 * numEls;
    float *a_out = s_out->data + 4 * numEls;
    float *o_out = s_out->data + 5 * numEls;

    float *c_in = s_in->data + 1 * numEls;

    float *w_i = w_diag->data;
    float *w_f = w_diag->data +     num_lstms;
    float *w_o = w_diag->data + 2 * num_lstms;

    float *b_i = b->data;
    float *b_f = b->data +     num_lstms;
    float *b_a = b->data + 2 * num_lstms;
    float *b_o = b->data + 3 * num_lstms;

    float i, f, a, o, c, h;
    for (unsigned int p=0; p<numEls; ++p)
    {
        int j = p / numcases;
        i = i_out[p];
        f = f_out[p];
        a = a_out[p];
        o = o_out[p];
        c = init ? 0 : c_in[p];

        i = sigmoid(i + c * w_i[j] + b_i[j]);
        f = sigmoid(f + c * w_f[j] + b_f[j]);
        a = use_relu ? relu(a + b_a[j]) : tanh(a + b_a[j]);
        c = c * f + i * a;
        o = sigmoid(o + c * w_o[j] + b_o[j]);
        h = c * o;

        i_out[p] = i;
        f_out[p] = f;
        a_out[p] = a;
        o_out[p] = o;
        c_out[p] = c;
        h_out[p] = h;
    }

    return 0;
}

int lstm_bprop(eigenmat* s_in, eigenmat* s_out, eigenmat* d_in, eigenmat* d_out, eigenmat* w_dense, eigenmat* w_diag, bool init, bool use_relu)
{
    unsigned int numcases = s_in->size[0];
    unsigned int num_lstms = s_in->size[1] / 6;
    unsigned int numEls = numcases * num_lstms;

    float *s_c_out = s_out->data +     numEls;
    float *s_i_out = s_out->data + 2 * numEls;
    float *s_f_out = s_out->data + 3 * numEls;
    float *s_a_out = s_out->data + 4 * numEls;
    float *s_o_out = s_out->data + 5 * numEls;

    float *s_c_in  = s_in->data  + 1 * numEls;

    float *d_h_out = d_out->data;
    float *d_c_out = d_out->data +     numEls;
    float *d_i_out = d_out->data + 2 * numEls;
    float *d_f_out = d_out->data + 3 * numEls;
    float *d_a_out = d_out->data + 4 * numEls;
    float *d_o_out = d_out->data + 5 * numEls;

    float *d_c_in  = d_in->data  + 1 * numEls;

    float *w_i = w_diag->data;
    float *w_f = w_diag->data +     num_lstms;
    float *w_o = w_diag->data + 2 * num_lstms;

    float i, f, a, o, c, grad_i, grad_f, grad_a, grad_o, grad_c, grad_h, c_old;
    for (unsigned int p=0; p<numEls; ++p)
    {
        int j = p / numcases;
        grad_h = d_h_out[p];
        grad_c = d_c_out[p];
        i = s_i_out[p];
        f = s_f_out[p];
        a = s_a_out[p];
        o = s_o_out[p];
        c = s_c_out[p];
        c_old = init ? 0 : s_c_in[p];

        grad_o = grad_h * c * deriv_of_sigmoid(o);
        grad_c += grad_o * w_o[j] + grad_h * o;

        grad_a = grad_c * i * (use_relu ? deriv_of_relu(a) : deriv_of_tanh(a));
        grad_i = grad_c * a * deriv_of_sigmoid(i);
        grad_f = grad_c * c_old * deriv_of_sigmoid(f);
        grad_c = grad_c * f + grad_f * w_f[j] + grad_i * w_i[j];

        d_i_out[p] = grad_i;
        d_f_out[p] = grad_f;
        d_o_out[p] = grad_o;
        d_a_out[p] = grad_a;
        if (!init)
        {
            d_c_in[p] = grad_c;
        }
    }

    if (!init)
    {
        sgemm(false, false, false,
              numcases, num_lstms, 4 * num_lstms,
              1, d_out->data + numcases * num_lstms * 2, numcases,
                 w_dense->data, 4 * num_lstms,
              1, d_in->data, numcases);
    }

    return 0;
}

int lstm_outp(eigenmat* s_in, eigenmat* s_out, eigenmat* d_out, eigenmat* dw_dense, eigenmat* dw_diag, eigenmat* db, bool init)
{
    unsigned int numcases = s_in->size[0];
    unsigned int num_lstms = s_in->size[1] / 6;

    if (!init)
    {
        sgemm(false, true, false,
              4 * num_lstms, num_lstms, numcases,
              1, d_out->data + numcases * num_lstms * 2, numcases,
                 s_in->data, numcases,
              1, dw_dense->data, 4 * num_lstms);
    }

    #pragma omp parallel for
    for (unsigned int lstm_id=0; lstm_id<num_lstms; ++lstm_id)
    {
        float* d_i     = d_out->data + numcases * (num_lstms * 2 + lstm_id);
        float* d_f     = d_out->data + numcases * (num_lstms * 3 + lstm_id);
        float* d_a     = d_out->data + numcases * (num_lstms * 4 + lstm_id);
        float* d_o     = d_out->data + numcases * (num_lstms * 5 + lstm_id);
        float* s_c     = s_out->data + numcases * (num_lstms * 1 + lstm_id);
        float* s_c_old = s_in->data  + numcases * (num_lstms * 1 + lstm_id);

        float dwi = 0, dwf = 0, dwo = 0, dbi = 0, dbf = 0, dba = 0, dbo = 0;
        float c_old, grad_i, grad_f, grad_a, grad_o;
        for (unsigned int i=0; i<numcases; ++i)
        {
            c_old = init ? 0 : s_c_old[i];
            grad_i = d_i[i];
            grad_f = d_f[i];
            grad_a = d_a[i];
            grad_o = d_o[i];
            dwi += c_old * grad_i;
            dwf += c_old * grad_f;
            dwo += s_c[i] * grad_o;
            dbi += grad_i;
            dbf += grad_f;
            dba += grad_a;
            dbo += grad_o;
        }

        dw_diag->data[lstm_id] += dwi;
        dw_diag->data[lstm_id + num_lstms] += dwf;
        dw_diag->data[lstm_id + num_lstms * 2] += dwo;
        db->data[lstm_id] += dbi;
        db->data[lstm_id + num_lstms] += dbf;
        db->data[lstm_id + num_lstms * 2] += dba;
        db->data[lstm_id + num_lstms * 3] += dbo;
    }

    return 0;
}

int bn_bprop(eigenmat* deriv, eigenmat* input, eigenmat* gamma, eigenmat* mu,
             eigenmat* sigma, eigenmat* target, float scale_targets) {
  int height = deriv->size[0];
  int width  = deriv->size[1];

  for (unsigned int column = 0; column < width; ++column) {
    float mu_val = mu->data[column];
    float sigma_val = sigma->data[column];
    float gamma_val = gamma->data[column];
    float *cur_x = input->data + column * height;
    float *cur_d = deriv->data + column * height;
    float *cur_target = target->data + column * height;
    float cur_sum = 0, cur_sum2 = 0, val;
    for (unsigned int i = 0; i < height; ++i) {
      cur_sum += (cur_x[i] - mu_val) * cur_d[i];
    }
    cur_sum /= ((height - 1) * sigma_val * sigma_val);
    for (unsigned int i = 0; i < height; ++i) {
      val = gamma_val * (cur_d[i] - (cur_x[i] - mu_val) * cur_sum) / sigma_val;
      cur_sum2 += val;
      cur_target[i] = scale_targets * cur_target[i] + val;
    }
    cur_sum2 /= height;
    for (unsigned int i = 0; i < height; ++i) {
      cur_target[i] -= cur_sum2;
    }
  }
  return 0;
}

int bn_bprop_inplace(eigenmat* deriv, eigenmat* acts, eigenmat* dgamma) {
  int height = deriv->size[0];
  int width  = deriv->size[1];
  for (unsigned int column = 0; column < width; ++column) {
    float cur_sum = 0;
    float *cur_data1 = deriv->data + column * height;
    float *cur_data2 = acts->data  + column * height;
    for (unsigned int i = 0; i < height; ++i) {
      cur_sum += cur_data1[i] * cur_data2[i];
    }
    float stat = cur_sum / height;
    dgamma->data[column] = stat;
    cur_sum = 0;
    for (unsigned int i = 0; i < height; ++i) {
      cur_data1[i] -= stat * cur_data2[i];
      cur_sum += cur_data1[i];
    }
    stat = cur_sum / height;
    for (unsigned int i = 0; i < height; ++i){
      cur_data1[i] -= stat;
    }
  }
}

int bn_grad(eigenmat* deriv, eigenmat* input, eigenmat* mu, eigenmat* sigma,
            eigenmat* dgamma, eigenmat* dbeta) {
  int height = deriv->size[0];
  int width  = deriv->size[1];
  for (unsigned int column = 0; column < width; ++column) {
    float mu_val = mu->data[column];
    float sigma_val = sigma->data[column];
    float *cur_x = input->data + column * height;
    float *cur_d = deriv->data + column * height;
    float z, d, sum_gamma = 0, sum_beta = 0;
    for (unsigned int i = 0; i < height; ++i) {
      z = (cur_x[i] - mu_val) / sigma_val;
      d = cur_d[i];
      sum_gamma += z * d;
      sum_beta  += d;
    }
    dgamma->data[column] = sum_gamma;
    dbeta->data[column]  = sum_beta;
  }
  return 0;
}


#ifndef VEC2D_HPP_INCLUDED
#define VEC2D_HPP_INCLUDED

/*! \file vector.hpp
    \ingroup common
    \brief vector data types
    \details this file imports the vector data types from boost.ublas for use under less verbose
            type names.
*/

#include <cassert>
#include <numeric>
#include <boost/numeric/ublas/vector.hpp>

//! \addtogroup common
//! \{

//! vector of doubles with constant maximum length \p N
template<int N>
using c_vector = boost::numeric::ublas::c_vector<double, N>;

/*! normal generic vector type with at most three entries.
    uses c_vector, i.e. preallocated space so it does not
    require any dynamic memory management to create these
    objects.
*/
using gen_vect = c_vector<3>;

/// integer vector of at most three dimension.
using int_vect = boost::numeric::ublas::c_vector<int, 3>;

/// cross product function, very ugly, not generic or easy to use.
/// \todo should be removed or significantly improved.
template<class Result, class Vector>
void crossProduct( Result& target, const Vector& v1, const Vector& v2)
{
    assert( target.size() == 3 );
    assert( v1.size() == 3);
    assert( v2.size() == 3 );
    target[0] =  v1[1]*v2[2] - v1[2]*v2[1];
    target[1] = -v1[0]*v2[2] + v1[2]*v2[0];
    target[2] = v1[0]*v2[1] - v1[1]*v2[0];
}

template<class Vec1, class Vec2>
double dotProduct(Vec1&& v1, Vec2&& v2) {
    using std::begin;
    using std::end;
    return std::inner_product(begin(v1), end(v1), begin(v2), 0.0);
}

//! \}

#endif // VEC2D_HPP_INCLUDED


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 the Delft University of Technology 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 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120319    K. Kumar          File created.
 *
 *    References
 *        Wakker, K.F. "Astrodynamics I, AE4-874", Delft University of Technology, 2007.
 *        Howell, K.C. Three-dimensional, periodic, 'Halo' orbits, Celestial Mechanics, 32. 53-71,
 *          1984.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <boost/bind.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKutta4Integrator.h"

#include "Tudat/Astrodynamics/Gravitation/stateDerivativeCircularRestrictedThreeBodyProblem.h"

namespace tudat
{
namespace unit_tests
{

//! Test if state derivative for circular restricted three-body problem is computed correctly.
BOOST_AUTO_TEST_CASE( testStateDerivativeCircularRestrictedThreeBodyProblem )
{
    namespace crtbp = gravitation::circular_restricted_three_body_problem;
    using crtbp::StateDerivativeCircularRestrictedThreeBodyProblem;
    using crtbp::xCartesianPositionIndex;
    using crtbp::zCartesianPositionIndex;
    using crtbp::yCartesianVelocityIndex;

    // Test 1: test state derivative at L1.
    {
        // Set mass parameter for Earth-moon system. Value from Table 3.1 (Wakker, 2007).
        double massParameter = 0.01215;

        // Initialize position L1, from Table 3.4 (Wakker, 2007).
        Eigen::VectorXd stateAtL1 = Eigen::VectorXd::Zero( 6 );
        stateAtL1( xCartesianPositionIndex ) = 0.836914;

        // Declare state derivative object.
        StateDerivativeCircularRestrictedThreeBodyProblem stateDerivative( massParameter );

        // Compute state derivative at L1.
        Eigen::VectorXd stateDerivativeAtL1 = stateDerivative.computeStateDerivative(
                    0.0, stateAtL1 );

        // Check if expected state derivative (all zeros) matches computed.
        TUDAT_CHECK_MATRIX_BASE( Eigen::VectorXd::Zero( 6 ), stateDerivativeAtL1 )
            BOOST_CHECK_SMALL( stateDerivativeAtL1.coeff( row, col ), 1.0e-4 );
    }

    // Test 2: test Halo orbit around L1. Note that this is actually a system test, since it
    //         requires that the Runge-Kutta 4 numerical integrator is functioning correctly.
    {
        // Set mass parameter. Value from Table I (Howell, 1984).
        double massParameter = 0.04;

        // Initialize position on Halo orbit around L1 (Howell, 1984).
        Eigen::VectorXd initialStateOnHaloOrbit = Eigen::VectorXd::Zero( 6 );
        initialStateOnHaloOrbit( xCartesianPositionIndex ) = 0.723268;
        initialStateOnHaloOrbit( zCartesianPositionIndex ) = 0.04;
        initialStateOnHaloOrbit( yCartesianVelocityIndex ) = 0.198019;

        // Declare state derivative object.
        StateDerivativeCircularRestrictedThreeBodyProblem stateDerivative( massParameter );

        // Declare Runge-Kutta 4 integrator.
        numerical_integrators::RungeKutta4IntegratorXd rungeKutta4Integrator(
                    boost::bind(
                        &StateDerivativeCircularRestrictedThreeBodyProblem::computeStateDerivative,
                        &stateDerivative, _1, _2 ),
                    0.0, initialStateOnHaloOrbit );

        // Integrate Halo orbit over one period.
        Eigen::VectorXd finalStateOnHaloOrbit = rungeKutta4Integrator.integrateTo(
                    2.0 * 1.300177, 0.1 );

        // Check if expected final state (same as initial) matches computed.
        BOOST_CHECK_CLOSE_FRACTION( initialStateOnHaloOrbit( 0 ),
                                    finalStateOnHaloOrbit( 0 ), 1.0e-3 );
        BOOST_CHECK_SMALL( finalStateOnHaloOrbit( 1 ), 1.0e-3 );
        BOOST_CHECK_CLOSE_FRACTION( initialStateOnHaloOrbit( 2 ),
                                    finalStateOnHaloOrbit( 2 ), 1.0e-3 );
        BOOST_CHECK_SMALL( finalStateOnHaloOrbit( 3 ), 1.0e-2 );
        BOOST_CHECK_CLOSE_FRACTION( initialStateOnHaloOrbit( 4 ),
                                    finalStateOnHaloOrbit( 4 ), 1.0e-2 );
        BOOST_CHECK_SMALL( finalStateOnHaloOrbit( 5 ), 1.0e-3 );
    }
}

} // namespace unit_tests
} // namespace tudat


/**
 * $Id$
 *
 * Software License Agreement (GNU General Public License)
 *
 *  Copyright (C) 2015:
 *
 *    Johann Prankl, prankl@acin.tuwien.ac.at
 *    Aitor Aldoma, aldoma@acin.tuwien.ac.at
 *
 *      Automation and Control Institute
 *      Vienna University of Technology
 *      Gusshausstraße 25-29
 *      1170 Vienn, Austria
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  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
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * @author Johann Prankl, Aitor Aldoma
 *
 */

#ifndef KP_INV_POSE_HPP
#define KP_INV_POSE_HPP

#include <v4r/common/rotation.h>
#include <Eigen/Dense>

namespace v4r {

inline void invPose(const Eigen::Matrix4f &pose, Eigen::Matrix4f &inv_pose) {
  inv_pose.setIdentity();
  inv_pose.topLeftCorner<3, 3>() = pose.topLeftCorner<3, 3>().transpose();
  inv_pose.block<3, 1>(0, 3) = -1 * (inv_pose.topLeftCorner<3, 3>() * pose.block<3, 1>(0, 3));
}

inline void invPose(const Eigen::Matrix4d &pose, Eigen::Matrix4d &inv_pose) {
  inv_pose.setIdentity();
  inv_pose.topLeftCorner<3, 3>() = pose.topLeftCorner<3, 3>().transpose();
  inv_pose.block<3, 1>(0, 3) = -1 * (inv_pose.topLeftCorner<3, 3>() * pose.block<3, 1>(0, 3));
}

/**
 * invPose6
 */
template <typename T1, typename T2, typename T3, typename T4>
inline void invPose6(const T1 r[3], const T2 t[3], T3 inv_r[3], T4 inv_t[3]) {
  inv_r[0] = T1(-1) * r[0];
  inv_r[1] = T1(-1) * r[1];
  inv_r[2] = T1(-1) * r[2];

  v4r::AngleAxisRotatePoint(inv_r, t, inv_t);

  inv_t[0] = T4(-1) * inv_t[0];
  inv_t[1] = T4(-1) * inv_t[1];
  inv_t[2] = T4(-1) * inv_t[2];
}

}  // namespace v4r

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_NORMS_INCLUDE
#define MTL_NORMS_INCLUDE

#include <boost/numeric/mtl/operation/one_norm.hpp>
#include <boost/numeric/mtl/operation/two_norm.hpp>
#include <boost/numeric/mtl/operation/infinity_norm.hpp>
#include <boost/numeric/mtl/operation/frobenius_norm.hpp>

#endif // MTL_NORMS_INCLUDE