#include <smtbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>

#include <scitbx/array_family/accessors/c_grid_padded.h>
#include <scitbx/array_family/accessors/flex_grid.h>

#include <smtbx/ab_initio/density_modification.h>

namespace smtbx { namespace ab_initio { namespace boost_python {

  namespace af = scitbx::af;

  template<class FloatType, class AccessorType>
  struct density_modification_wrapper
  {
    typedef void (*f_t)(af::ref<FloatType, AccessorType> const&, FloatType);
    static void wrap() {
      using namespace boost::python;
      using namespace density_modification;
      def("flip_charges_in_place",
          static_cast<f_t>(flip_charges_in_place));
      def("low_density_elimination_in_place_tanaka_et_al_2001",
          static_cast<f_t>(low_density_elimination_in_place_tanaka_et_al_2001));
    }
  };

  void init_module() {
    density_modification_wrapper<double, af::c_grid_padded<3> >::wrap();
    density_modification_wrapper<double, af::flex_grid<> >::wrap();
  }

}}} // namespace smtbx::ab_initio::boost_python

BOOST_PYTHON_MODULE(smtbx_ab_initio_ext)
{
  smtbx::ab_initio::boost_python::init_module();
}


/*  $Id$
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib 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.
 *
 *  OpenCAMlib 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 OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <iostream>
#include <sstream>
#include <string>

#include <boost/foreach.hpp>

#include "cylcutter.hpp"
#include "bullcutter.hpp" // for offsetCutter()
#include "numeric.hpp"

namespace ocl
{

CylCutter::CylCutter() {
    std::cout << " usage: CylCutter( double diameter, double length ) \n";
    assert(0);
}

CylCutter::CylCutter(double d, double l) {
    diameter= d;                    assert( d > 0.0);
    radius= d/2.0;
    length = l;                     assert( l > 0.0 );
    xy_normal_length = radius;
    normal_length = 0.0;
    center_height = 0.0;
}

// drop-cutter vertexDrop is handled by the base-class method in MillingCutter
// drop-cutter facetDrop is handled by the base-class method in MillingCutter

// we handle the edge-drop here.
CC_CLZ_Pair CylCutter::singleEdgeDropCanonical(const Point& u1, const Point& u2) const {
    // along the x-axis the cc-point is at x-coord s or -s:
    double s = sqrt( square( radius ) - square( u1.y ) );
    Point cc1(  s, u1.y, 0);
    Point cc2( -s, u1.y, 0);
    cc1.z_projectOntoEdge(u1,u2);
    cc2.z_projectOntoEdge(u1,u2);
    // pick the higher one
    double cc_u;
    double cl_z;
    if (cc1.z > cc2.z) {
        cc_u = cc1.x;
        cl_z = cc1.z;
    } else {
        cc_u = cc2.x;
        cl_z = cc2.z;
    }
    return CC_CLZ_Pair( cc_u, cl_z);
}

// general purpose vertexPush, delegates to this->width(h)
bool CylCutter::vertexPush(const Fiber& f, Interval& i, const Triangle& t) const {
    bool result = false;
    BOOST_FOREACH( const Point& p, t.p) {
        if (this->singleVertexPush(f,i,p, VERTEX) )
            result = true;
    }

    Point p1, p2;
    if ( t.zslice_verts(p1, p2, f.p1.z) ) {
        p1.z = f.p1.z; // z-coord should be very close to f.p1.z, but set it exactly anyway.
        p2.z = f.p1.z;
        if ( this->singleVertexPush(f,i,p1, VERTEX_CYL) )
            result = true;
        if (this->singleVertexPush(f,i,p2, VERTEX_CYL))
            result = true;
    }

    return result;
}

std::string CylCutter::str() const {
    std::ostringstream o;
    o << *this;
    return o.str();
}

std::ostream& operator<<(std::ostream &stream, CylCutter c) {
  stream << "CylCutter (d=" << c.diameter << ", L=" << c.length <<  ")";
  return stream;
}

} // end namespace
// end file cylcutter.cpp


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE boolean circuit backend

#include <iostream>
#include <boost/test/included/unit_test.hpp>

#include "../src/boolean_circuit_backend.hpp"
#include "../src/boolean_circuit_backend_factory.hpp"

using namespace std;
using namespace homomorphine;

//
// Tests for TFHE backend
//

#ifdef __HAS_TFHE__

// Basic TFHE backend test
BOOST_AUTO_TEST_CASE( tfhe_check_encryption )
{
  int value = 12228;
  stringstream cipher;
  stringstream test_cipher;
  stringstream public_key;
  stringstream secret_key;

  BOOST_TEST_MESSAGE( "Testing a TFHE backend encryption..." );

  // create and initialize backend
  BooleanCircuitBackend* backend = BooleanCircuitBackendFactory::create("tfhe");
  backend->init();

  // generate keys and get key streams
  backend->generateKeys();
  backend->writePublicKeyToStream(public_key);
  backend->writeSecretKeyToStream(secret_key);

  // encrypt the value and write cipher to a stream
  backend->encryptToStream(value, cipher);

  // create new backend and initialize it
  BooleanCircuitBackend* test_backend = BooleanCircuitBackendFactory::create("tfhe");
  test_backend->init();
  test_backend->readPublicKeyFromStream(public_key);
  test_backend->readSecretKeyFromStream(secret_key);
  test_backend->encryptToStream(value, test_cipher);

  BOOST_TEST ( backend->decryptFromStream(cipher) == test_backend->decryptFromStream(test_cipher) );
}

// Basic TFHE backend test
BOOST_AUTO_TEST_CASE( tfhe_check_simple_operation )
{
  int value = 16;
  stringstream cipher;
  stringstream result_cipher;
  stringstream public_key;
  stringstream secret_key;

  BOOST_TEST_MESSAGE( "Testing a simple TFHE operations..." );

  // create and initialize backend
  BooleanCircuitBackend* backend = BooleanCircuitBackendFactory::create("tfhe");
  backend->init();

  // generate keys and get key streams
  backend->generateKeys();
  backend->writePublicKeyToStream(public_key);
  backend->writeSecretKeyToStream(secret_key);

  // encrypt the value and write cipher to a stream
  backend->encryptToStream(value, cipher);

  // create new backend and initialize it
  BooleanCircuitBackend* test_backend = BooleanCircuitBackendFactory::create("tfhe");
  test_backend->init();
  test_backend->readPublicKeyFromStream(public_key);
  test_backend->NOT(result_cipher, cipher);

  // decrypt the value and veryf the result
  BOOST_TEST ( backend->decryptFromStream(result_cipher) == 65519 );
}

// Basic TFHE backend test
BOOST_AUTO_TEST_CASE( tfhe_check_AND_operation_with_public_encoding )
{
  int value_x = 2069;   // 0000100000010101
  int value_y = 143;    // 0000000010001111
  stringstream cipher_x;
  stringstream cipher_y;
  stringstream result_cipher;
  stringstream public_key;
  stringstream secret_key;

  BOOST_TEST_MESSAGE( "Testing a AND TFHE operation - encoding at public end..." );

  // create and initialize backend
  BooleanCircuitBackend* backend = BooleanCircuitBackendFactory::create("tfhe");
  backend->init();

  // generate keys and get key streams
  backend->generateKeys();
  backend->writePublicKeyToStream(public_key);
  backend->writeSecretKeyToStream(secret_key);

  // encrypt the value and write cipher to a stream
  backend->encryptToStream(value_x, cipher_x);

  // create new backend and initialize it
  BooleanCircuitBackend* test_backend = BooleanCircuitBackendFactory::create("tfhe");
  test_backend->init();
  test_backend->readPublicKeyFromStream(public_key);

  // encode the value and write cipher to a stream
  test_backend->encodeToStream(value_y, cipher_y);

  test_backend->AND(result_cipher, cipher_x, cipher_y);

  // decrypt the value and veryf the result
  BOOST_TEST ( backend->decryptFromStream(result_cipher) == (value_x & value_y) );
}

#endif


#include <boost/program_options.hpp>

#include <iostream>
#include <fstream>
#include <string>
#include <streambuf>
#include <sstream>
#include <vector>

namespace {

std::string read_file_contents(const std::string &filename) {
	std::ifstream file(filename);
	return { std::istreambuf_iterator<char>(file), std::istreambuf_iterator<char>() };
}

template<typename T>
void write_file_contents(const std::string &filename, const T &contents) {
	std::ofstream file(filename);
	file << contents;
}

} /* namespace <anonymous> */

int main(int argc, char *argv[]) {
	namespace po = boost::program_options;

	const std::string dirname = "/sys/class/backlight/intel_backlight";

	unsigned int max_brightness;
	unsigned int brightness;

	double current_ratio;

	int delta;

	po::options_description desc("Options");
	desc.add_options()
		("inc,i", po::value<std::vector<unsigned int>>()->composing()
		 ->notifier([&delta](const auto &vs) { for(auto v: vs) delta += v; }), "increase brightness by percentage")
		("dec,d", po::value<std::vector<unsigned int>>()->composing()
		 ->notifier([&delta](const auto &vs) { for(auto v: vs) delta -= v; }), "decrease brightness by percentage")
		("print,p", "print the brightness")
	;

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

	{
		std::istringstream brstr(read_file_contents(dirname + "/brightness"));
		brstr >> brightness;
	}

	{
		std::istringstream mbrstr(read_file_contents(dirname + "/max_brightness"));
		mbrstr >> max_brightness;
	}

	current_ratio = static_cast<double>(brightness) / max_brightness;

	double new_ratio = current_ratio + static_cast<double>(delta) / 100;
	if(new_ratio < 0.0) new_ratio = 0.0;
	else if(new_ratio > 1.0) new_ratio = 1.0;

	brightness = static_cast<int>(new_ratio * max_brightness);

	write_file_contents(dirname + "/brightness", brightness);

	if(vm.find("print") != vm.end()) {
		std::cout << "brightness: " << brightness << '/' << max_brightness << " (" << static_cast<unsigned int>(new_ratio * 100) << "%)\n";
	}
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         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
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_PREDECESSOR_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_PREDECESSOR_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_ieee
 * \defgroup boost_simd_ieee_predecessor predecessor
 *
 * \par Description
 * With one parameter it is equivalent to \c prev
 * It is in the type \c A0, the greatest  \c A0 elementwise strictly less than  \c a0.
 * \par
 * With two parameters, the second is an integer value  \c n
 * and the result is equivalent to applying \c prev \c abs(n) times to  \c a0.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/predecessor.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template<class A0> inline
 *   A0 predecessor(A0 const& a0);
 *
 *   template<class A0,class A1> inline
 *   A0 predecessor(A0 const& a0,const A1& n);
 * }
 * \endcode
 *
 * \param a0 the first parameter of predecessor
 * \param a1 the second parameter of predecessor
 *
 * \return a value of type A0
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag predecessor_ of functor predecessor
     *        in namespace boost::simd::tag for toolbox boost.simd.ieee
    **/
    struct predecessor_ : ext::elementwise_<predecessor_> { typedef ext::elementwise_<predecessor_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::predecessor_, predecessor, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::predecessor_, predecessor, 1)
} }

#endif

// modified by jt the 25/12/2010


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         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 BOOST_SIMD_REDUCTION_FUNCTIONS_POSMAX_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_POSMAX_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief posmax generic tag

     Represents the posmax function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct posmax_ : ext::unspecified_<posmax_>
    {
      /// @brief Parent hierarchy
      typedef ext::unspecified_<posmax_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_posmax_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::posmax_, Site> dispatching_posmax_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::posmax_, Site>();
   }
   template<class... Args>
   struct impl_posmax_;
  }
  /*!
    Returns the index of the first occurence of greatest element of the SIMD vector

    @par Semantic:

    For every parameter of type T0

    @code
    size_t r = posmax(a0);
    @endcode

    is similar to:

    @code
      scalar_of<T0> m =  maximum(a0);
      size_t r;
      for(size_t i=0; i < cardinal_of<T0>; i++)
        if (m == a0[i]) { r = i;  break; }
    @endcode

    @param a0

    @return a size_t value
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::posmax_, posmax, 1)
} }
#endif


#ifndef __MEMVALUES_HPP_DEFINED__
#define __MEMVALUES_HPP_DEFINED__

#include <iostream>
#include <map>

#include <ilasynth/ast/node.hpp>
#include <ilasynth/common.hpp>
#include <ilasynth/type.hpp>

#include <boost/python.hpp>
#include <z3++.h>

namespace ilasynth {
class Z3ExprAdapter;
class MemVar;

// This structure represents a memory with known values.
// It is passed into python during synthesis.
struct MemValues {
  typedef std::map<mp_int_t, mp_int_t> map_t;

  static z3::context c_eq;
  static z3::solver S_eq;

  NodeType type;
  mp_int_t MAX_ADDR;

  mp_int_t def_value;
  map_t values;

  // default.
  MemValues();
  // constructor.
  MemValues(int addrWidth, int dataWidth, const py::object& def_val);
  // constructor from imported memvalue.
  MemValues(int addrWidth, int dataWidth, const map_t values,
            const mp_int_t def_value);
  // construct from a model.
  MemValues(Z3ExprAdapter& c, const z3::model& m, const MemVar* mem);
  // copy constructor.
  MemValues(const MemValues& that);
  // destructor.
  ~MemValues();

  // assignment operator.
  MemValues& operator=(const MemValues& that);

  // return def_value
  py::object getDefault() const;
  // set def_value.
  void setDefault(const py::object& dv);
  // get nondefault values.
  py::object getValues() const;
  // __getitem__
  py::object getItem(const py::object& index) const;
  // __setitem__
  void setItem(const py::object& index, const py::object& value);

  // equality for python.
  bool eq(const MemValues& mv) const { return *this == mv; }
  // equality.
  bool operator==(const MemValues& mv) const;
  // equality using smt.
  bool semanticEqual(const MemValues& mv) const;
  // convert to z3.
  z3::expr toZ3(z3::context& c) const;
  // write to a string.
  std::string str() const;
  // gc: remove entries which are equal to the default.
  void gc();

  // support for __getitem__
  mp_int_t getItemInt(const mp_int_t& index) const;
};
std::ostream& operator<<(std::ostream& out, const MemValues& mv);
} // namespace ilasynth

#endif


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "FixedLink.hpp"

#include <rw/kinematics/Kinematics.hpp>
#include <rw/math/Q.hpp>
#include <rw/models/Joint.hpp>

#include <Eigen/Core>

using namespace rw::math;
using namespace rw::models;
using namespace rw::kinematics;
using namespace rwsim::dynamics;

namespace {

Eigen::MatrixXd getBlockDiagInertia (Device& dev, std::vector< FixedLink* >& parents,
                                     FixedLink* current)
{
    size_t n          = dev.getDOF ();
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero (n * 3, n * 3);
    for (size_t i = 0; i < parents.size () + 1; i++) {
        Eigen::Block Ar = A.block (i * 3, i * 3, i * 3 + 3, i * 3 + 3);
        if (i == parents.size ()) {
            Ar = current->getInertia ().e ();
        }
        else {
            Ar = parents[i]->getInertia ().e ();
        }
    }
    return A;
}

}    // namespace

FixedLink::FixedLink (const BodyInfo& info, rw::kinematics::Frame* base,
                      std::vector< FixedLink* > parents, rw::models::Device& dev,
                      rw::models::Joint& j, const std::vector< rw::kinematics::Frame* >& frames,
                      rw::kinematics::State& state) :
    Body (ConstraintNode::Link, j, frames),
    _jointFrame (j), _dev (dev), _parents (parents), _jac (dev.baseJframe (&j, state)),
    _jacRB (_jac), _base (base), _acc (0.0), _vel (0.0), _pos (*(j.getQ (state))),
    _wTbase (dev.worldTbase (state)), _baseTw (inverse (_wTbase)), _impulseIterations (0)

{
    _wTb = rw::kinematics::Kinematics::WorldTframe (&_jointFrame, state);
    _bTw = inverse (_wTb);

    _baseTb = rw::kinematics::Kinematics::FrameTframe (_base, &_jointFrame, state);
    _bTbase = inverse (_baseTb);

    _wTbase = rw::kinematics::Kinematics::WorldTframe (_base, state);
    _baseTw = inverse (_wTbase);
}

FixedLink::~FixedLink ()
{}

void FixedLink::rollBack (State& state)
{
    _torque = _torqueRB;
    _force  = _forceRB;

    _linImpulse = Vector3D<> (0.0, 0.0, 0.0);
    _angImpulse = Vector3D<> (0.0, 0.0, 0.0);

    _pos = *_jointFrame.getQ (state);
    _wTb = rw::kinematics::Kinematics::WorldTframe (&_jointFrame, state);
    _bTw = inverse (_wTb);

    _baseTb = rw::kinematics::Kinematics::FrameTframe (_base, &_jointFrame, state);
    _bTbase = inverse (_baseTb);

    _wTbase = rw::kinematics::Kinematics::WorldTframe (_base, state);
    _baseTw = inverse (_wTbase);

    _jac = _dev.baseJframe (&_jointFrame, state);

    _vel = _velRB;
    std::cout << "FIXEDLINK: ROLLBACK " << _vel << " " << _pos << std::endl;
    // std::cout << _wTb << std::endl;
    // std::cout << _baseTb << std::endl;
    // std::cout << _jac << std::endl;
}

void FixedLink::saveState (double h, rw::kinematics::State& state)
{
    // update rollBack variables
    _torqueRB = _torque;
    _forceRB  = _force;

    _jointFrame.setQ (state, &_pos);
    _velRB = _vel;
    std::cout << "FIXEDLINK: SAVESTATE " << _vel << " " << _pos << std::endl;
    // std::cout << _wTb << std::endl;
    // std::cout << _baseTb << std::endl;
    // std::cout << _jac << std::endl;
}

void FixedLink::resetState (rw::kinematics::State& state)
{
    _vel = 0;
    _pos = *_jointFrame.getQ (state);

    // std::cout << "FixedLink: Update position! " << _pos << " " << _vel << " " << h << std::endl;

    _wTb = rw::kinematics::Kinematics::WorldTframe (&_jointFrame, state);
    _bTw = inverse (_wTb);

    _baseTb = rw::kinematics::Kinematics::FrameTframe (_base, &_jointFrame, state);
    _bTbase = inverse (_baseTb);

    _jac = _dev.baseJframe (&_jointFrame, state);
}

void FixedLink::addQd (double vel)
{
    double epsilon = 0.00001;
    _vel += vel;
    // TODO: Clamp _vel to minimum [-VELMAX,VELMAX]
    // make sure velocity is no higher than target vel
    if (_targetVel < 0 && _vel < _targetVel) {
        _vel = _targetVel;
    }
    else if (_targetVel > 0 && _vel > _targetVel) {
        _vel = _targetVel;
    }
}

rw::math::Vector3D<> FixedLink::getPointVelW (const rw::math::Vector3D<>& wPp)
{
    rw::math::Q vels (_dev.getDOF ());
    size_t i;

    for (i = 0; i < _parents.size (); i++)
        vels (i) = _parents[i]->getQd ();

    vels (i) = _vel;
    for (i++; i < _dev.getDOF (); i++)
        vels (i) = 0;

    // Velocity of tool seen in base
    rw::math::Rotation3D<> tRb      = _bTbase.R ();         // tool to base
    rw::math::Transform3D<> wTt     = _wTbase * _baseTb;    // world to tool
    rw::math::Transform3D<> pTw     = inverse (rw::math::Transform3D<> (wPp, wTt.R ()));
    rw::math::Transform3D<> pTt     = pTw * wTt;
    rw::math::VelocityScrew6D<> bVt = _jac * vels;
    rw::math::VelocityScrew6D<> tVt = tRb * bVt;         // change reference frame
    rw::math::VelocityScrew6D<> pVp = pTt.P () * tVt;    // change reference point
    rw::math::VelocityScrew6D<> wVp = wTt.R () * pVp;    // change reference frame
                                                         /*
                                                                 std::cout << "tRb " << tRb << std::endl;
                                                                 std::cout << "wRt " << wTt.R() << std::endl;
                                                                 std::cout << "pPt " << pTt.P() << std::endl;
                                                                 std::cout << "Jac: " << _jac.size1() << " " << _jac.size2() << std::endl;
                                                                 std::cout << "bVt " << bVt << std::endl;
                                                                 std::cout << "tVt " << tVt << std::endl;
                                                                 std::cout << "pVp " << pVp << std::endl;
                                                                 std::cout << "Vels: " << vels << std::endl;
                                                                 std::cout << "wVp: " << wVp << std::endl;
                                                         */
    // Change the base of reference by multiplying with the inverserotation
    return wVp.linear ();
}

void FixedLink::updatePosition (double h, State& state)
{
    // Clamp _vel to minimum 0.0
    _pos += _vel * h;
    _jointFrame.setQ (state, &_pos);

    // std::cout << "FixedLink: Update position! " << _pos << " " << _vel << " " << h << std::endl;

    _wTb = rw::kinematics::Kinematics::WorldTframe (&_jointFrame, state);
    _bTw = inverse (_wTb);

    _baseTb = rw::kinematics::Kinematics::FrameTframe (_base, &_jointFrame, state);
    _bTbase = inverse (_baseTb);

    _wTbase = rw::kinematics::Kinematics::WorldTframe (_base, state);
    _baseTw = inverse (_wTbase);

    _jac = _dev.baseJframe (&_jointFrame, state);

    _impulseIterations = 0;
}

#include <rw/math/LinearAlgebra.hpp>

rw::math::InertiaMatrix<> FixedLink::getEffectiveMassW (const rw::math::Vector3D<>& wPc)
{
    std::cout << "Eff mass W" << std::endl;
    // calculate the block diagonal inertia matrix of all child bodies
    // the matrix has n blocks where n is number of dof
    // each block is 3x3 inertiamatrix so we get
    /*size_t n = _dev.getDOF();
    matrix<double> A( zero_matrix<double>(n*3,n*3) );
    for(size_t i=0;i<_parents.size()+1;i++){
            matrix_range<matrix<double> > Ar (A, range(i*3, i*3+3), range(i*3, i*3+3));
            if(i==_parents.size()){
                    Ar = getInertia().m();
            } else {
                    Ar = _parents[i]->getInertia().m();
            }
    }*/

    matrix< double > A = getBlockDiagInertia (_dev, _parents, this);

    // now calculate the contact jacobian expressed in world coordinates
    matrix< double > tmp        = prod (_jac.m (), A);
    matrix< double > effInertia = prod (tmp, trans (_jac.m ()));
    return InertiaMatrix<> (effInertia);
}

rw::math::InertiaMatrix<> FixedLink::getEffectiveMass ()
{
    std::cout << "Eff mass";
    // calculate the block diagonal inertia matrix of all child bodies
    // the matrix has n blocks where n is number of dof
    // each block is 3x3 inertiamatrix so we get
    /*
    size_t n = _dev.getDOF();
    matrix<double> A( zero_matrix<double>(n*3,n*3) );
    for(size_t i=0;i<_parents.size()+1;i++){
            matrix_range<matrix<double> > Ar (A, range(i*3, i*3+3), range(i*3, i*3+3));
            if(i==_parents.size()){
                    Ar = getInertia().m();
            } else {
                    Ar = _parents[i]->getInertia().m();
            }
    }
    */
    matrix< double > A = getBlockDiagInertia (_dev, _parents, this);

    // now calculate the effective mass inertia seen in the joint
    matrix< double > tmp        = prod (_jac.m (), A);
    matrix< double > effInertia = prod (tmp, trans (_jac.m ()));
    std::cout << " ! " << std::endl;
    return InertiaMatrix<> (effInertia);
}

void FixedLink::updateImpulse ()
{
    std::cout << "FixedLink: Update impulse!" << std::endl;
    Eigen::VectorXd cartImpulse (6);
    cartImpulse (0) = _linImpulse (0);
    cartImpulse (1) = _linImpulse (1);
    cartImpulse (2) = _linImpulse (2);
    cartImpulse (3) = _angImpulse (0);
    cartImpulse (4) = _angImpulse (1);
    cartImpulse (5) = _angImpulse (2);

    // too update joint velocities we use: deltaQd = M^-1 J^T P
    // InertiaMatrix<> K = getEffectiveMass();

    std::cout << "get diag inertia" << std::endl;
    Eigen::MatrixXd A = getBlockDiagInertia (_dev, _parents, this);
    std::cout << "Calc JT_P" << std::endl;
    Eigen::VectorXd JT_P = _jac.e ().transpose () * cartImpulse;
    std::cout << "Calc jointVel" << std::endl;
    Eigen::VectorXd jointVel = LinearAlgebra::pseudoInverse (A) * JT_P;

    // add the velocity to all joints that is affected
    std::cout << "Add vel to parents: "
              << " " << _parents.size () << " " << jointVel.size () << std::endl;

    for (size_t i = 0; i < _parents.size (); i++) {
        _parents[i]->addQd (jointVel (i));
    }
    addQd (jointVel (_parents.size ()));

    _linImpulse = Vector3D<> (0.0, 0.0, 0.0);
    _angImpulse = Vector3D<> (0.0, 0.0, 0.0);
    std::cout << "Update Vel END" << std::endl;
}

void FixedLink::updateVelocity (double h, State& state)
{
    std::cout << "FixedLink: Update velocity!" << _jointFrame.getName () << std::endl;
    Eigen::VectorXd cartForce (6);
    cartForce (0)               = _force (0);
    cartForce (1)               = _force (1);
    cartForce (2)               = _force (2);
    cartForce (3)               = _torque (0);
    cartForce (4)               = _torque (1);
    cartForce (5)               = _torque (2);
    Eigen::VectorXd tmp         = _jac.e ().transpose * cartForce;
    Eigen::VectorXd jointTorque = (_jac.e () * tmp) * h;

    for (size_t i = 0; i < _parents.size (); i++) {
        _parents[i]->addQd (jointTorque (i));
    }

    addQd (jointTorque (_parents.size ()) + _acc * h);

    //_vel += _acc*h + jointTorque();

    double MAX_VEL = 1;
    // clamp the velocity
    if (_vel < -MAX_VEL) {
        _vel = -MAX_VEL;
    }
    else if (_vel > MAX_VEL) {
        _vel = MAX_VEL;
    }
}

void FixedLink::addForceWToPosW (const rw::math::Vector3D<>& force, const rw::math::Vector3D<>& pos)
{
    // transform the force into body frame description
    rw::math::Vector3D<> forcebody = _baseTw.R () * force;

    // calculate the center force contribution
    _force += forcebody;
    rw::math::Vector3D<> posOnBody = _baseTw.R () * (pos - _wTb.P ());

    // calculate the torque contribution
    _torque += cross (posOnBody, forcebody);
}

void FixedLink::addImpulseWToPosW (const rw::math::Vector3D<>& impulse,
                                   const rw::math::Vector3D<>& pos)
{
    // std::cout << "FixedLink: Impulse at: " << pos << std::endl;
    // transform the force into body frame description
    rw::math::Vector3D<> ibody = _baseTw.R () * impulse;

    // calculate the center force contribution
    _linImpulse += ibody;
    rw::math::Vector3D<> posOnBody = _baseTw.R () * (pos - _wTb.P ());

    // calculate the torque contribution
    _angImpulse += cross (posOnBody, ibody);
}


#include "frame_loader.h"

#include <string>
#include <stdint.h>
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <armadillo>

using namespace std;

FrameLoader::FrameLoader() {
	// empty
}

bool FrameLoader::LoadBackground(session_param &ses_par, frames &frame_now) {

	bool bckg_loaded = true;

	frame_now.image_size.clear();
	frame_now.bckg_images.clear();

	for (int i=1; i<=ses_par.N_cam; i++) {

		string img_name = ses_par.session_loc+"/"+ses_par.bckg_loc+"/"+ses_par.bckg_name+to_string(i)+"."+ses_par.bckg_img_format;

		cv::Mat image = cv::imread(img_name, CV_LOAD_IMAGE_GRAYSCALE);

		if (image.empty()) {
			bckg_loaded = false;
		}
		else {
			arma::Col<int> bckg_vec = FrameLoader::CVMat2Vector(image);
			frame_now.bckg_images.push_back(bckg_vec);
			tuple<int, int> img_size = make_tuple(image.rows, image.cols);
			frame_now.image_size.push_back(img_size);
		}

	}

	return bckg_loaded;
}

bool FrameLoader::LoadSingleFrame(session_param &ses_par, frames &frame_now, int frame_nr) {

	bool frame_loaded = true;

	frame_now.single_raw_frame.clear();

	for (int i=0; i<ses_par.N_cam; i++) {

		string img_name = ses_par.session_loc+"/"+ses_par.mov_name+to_string(ses_par.mov_nr)+"/"+ses_par.cam_name+to_string(i+1)+"/"+ses_par.frame_name+
						to_string(ses_par.chrono_frame_nr[frame_nr])+"."+ses_par.frame_img_format;

		cv::Mat image = cv::imread(img_name, CV_LOAD_IMAGE_GRAYSCALE);

		if (image.empty()) {
			frame_loaded = false;
		}
		else {
			arma::Col<int> img_vec = FrameLoader::CVMat2Vector(image);
			frame_now.single_raw_frame.push_back(FrameLoader::BackgroundSubtract(img_vec,frame_now.bckg_images[i]));
		}

	}

	return frame_loaded;
}

bool FrameLoader::LoadFrameBatch(session_param &ses_par, frames &frame_now, int start_frame, int end_frame, int batch_nr) {

	bool frames_loaded = true;

	int N_frames = end_frame-start_frame+1;

	frame_now.batch_nr = batch_nr;
	frame_now.start_frame = start_frame;
	frame_now.end_frame = end_frame;
	frame_now.raw_frames.clear();

	for (int i=0; i<ses_par.N_cam; i++) {
		arma::Mat<int> frame_batch(frame_now.bckg_images[i].n_rows,N_frames);
		for (int j=0; j<N_frames; j++) {

			string img_name = ses_par.session_loc+"/"+ses_par.mov_name+to_string(ses_par.mov_nr)+"/"+ses_par.cam_name+to_string(i+1)+"/"+ses_par.frame_name+
							to_string(ses_par.chrono_frame_nr[start_frame+j])+"."+ses_par.frame_img_format;

			cv::Mat image = cv::imread(img_name, CV_LOAD_IMAGE_GRAYSCALE);

			if (image.empty()) {
				frames_loaded = false;
			}
			else {
				arma::Col<int> sub_img = FrameLoader::CVMat2Vector(image);
				frame_batch.col(j) = FrameLoader::BackgroundSubtract(sub_img,frame_now.bckg_images[i]);
			}
		}
		frame_now.raw_frames.push_back(frame_batch);
	}

	return frames_loaded;
}

arma::Col<int> FrameLoader::BackgroundSubtract(arma::Col<int> &img_vec, arma::Col<int> &bckg_vec) {

	arma::Col<int> s255;

	s255.set_size(img_vec.n_rows);

	s255.fill(255);

	arma::Col<int> sub_img = img_vec-(bckg_vec-s255);

	return sub_img;
}

arma::Col<int> FrameLoader::CVMat2Vector(cv::Mat &img) {

	arma::Col<int> img_vec;

	img.convertTo(img, CV_8UC1);
	arma::Mat<uint8_t> img_mat(reinterpret_cast<uint8_t*>(img.data), img.rows, img.cols);
	img_vec = arma::vectorise(arma::conv_to<arma::Mat<int>>::from(img_mat));

	return img_vec;
}

cv::Mat FrameLoader::Vector2CVMat(arma::Col<int> &img_vec, int N_row, int N_col) {

	arma::Mat<int> img_mat;

	img_mat = img_vec;

	img_mat.reshape(N_row,N_col);

	cv::Mat img(img_mat.n_rows, img_mat.n_cols, CV_32SC1, img_mat.memptr());

	img.convertTo(img, CV_8UC1);

	return img;
}

np::ndarray FrameLoader::ReturnSingleFrame(session_param &ses_par, frames &frame_now, int cam_nr) {

	int N_row = get<0>(frame_now.image_size[cam_nr]);
	int N_col = get<1>(frame_now.image_size[cam_nr]);

	p::tuple shape = p::make_tuple(N_row,N_col);
	np::dtype dtype = np::dtype::get_builtin<int>();
	np::ndarray array_out = np::zeros(shape,dtype);

	for (int i=0; i<N_row; i++) {
		for (int j=0; j<N_col; j++) {
			array_out[i][j] = frame_now.single_raw_frame[cam_nr](i*N_row+j);
		}
	}

	return array_out;
}


/*
 * ApproachPoseHeuristics.hpp
 *
 *  Created on: Oct 14, 2019
 *      Author: jelavice
 */

#pragma once

#include <Eigen/Dense>
#include "se2_planning/State.hpp"

namespace se2_planning {

struct ApproachPoseHeuristicParameters {
  static constexpr double kDegToRad = M_PI / 180.0;
  double yawTolerance_ = 360.0 * kDegToRad;
  double lateralTolerance_ = 10.0;
  bool isAlwaysSatisfied_ = true;
};

class ApproachPoseHeuristic {
 public:
  ApproachPoseHeuristic() = default;
  virtual ~ApproachPoseHeuristic() = default;

  virtual bool isHeuristicsSatisfied(const SE2state& approachPose) const;
  void setRobotCurrentPose(const SE2state& robotPose);
  void setTargetPosition(const XYstate& targetPosition);
  void setParameters(const ApproachPoseHeuristicParameters& params);

 protected:
  double getYawAngleDeviation(const SE2state& approachPose) const;
  double getLateralDeviation(const SE2state& approachPose) const;

  XYstate targetPosition_;
  SE2state currentPose_;
  ApproachPoseHeuristicParameters parameters_;
};

}  // namespace se2_planning


// [===========================================================================]
// [                                M o n e t a                                ]
// [---------------------------------------------------------------------------]
// [                                                                           ]
// [                          Copyright (C) 2005-2015                          ]
// [                      Rodrigo Madera <madera@acm.org>                      ]
// [                                                                           ]
// [---------------------------------------------------------------------------]
// [         Distributed under the Boost Software License, Version 1.0         ]
// [ Read accompanying LICENSE or copy at http://www.boost.org/LICENSE_1_0.txt ]
// [===========================================================================]

#include "pch.hxx"
#include <moneta/traits/detail/indices_of.hxx>
#include <boost/mpl/at.hpp>
#include <boost/type_traits/is_float.hpp>

inline void static_test() {
	using moneta::traits::detail::indices_of;
	namespace mpl = boost::mpl;

	{
		typedef indices_of<
			mpl::vector<long, float, short, double, float, long, long double>,
			boost::is_float<mpl::_1>
		>::type indices_t;

		BOOST_MPL_ASSERT((boost::is_same<boost::mpl::at_c<indices_t, 0>::type, mpl::int_<1> >));
		BOOST_MPL_ASSERT((boost::is_same<boost::mpl::at_c<indices_t, 1>::type, mpl::int_<3> >));
		BOOST_MPL_ASSERT((boost::is_same<boost::mpl::at_c<indices_t, 2>::type, mpl::int_<4> >));
		BOOST_MPL_ASSERT((boost::is_same<boost::mpl::at_c<indices_t, 3>::type, mpl::int_<6> >));
	}

	{
		typedef indices_of<
			mpl::vector<long, float, short, double, float, long, long double>,
			boost::is_same<long, mpl::_1>
		>::type indices_t;

		BOOST_MPL_ASSERT((boost::is_same<boost::mpl::at_c<indices_t, 0>::type, mpl::int_<0> >));
		BOOST_MPL_ASSERT((boost::is_same<boost::mpl::at_c<indices_t, 1>::type, mpl::int_<5> >));
	}
}


#include <boost/test/unit_test.hpp>

#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/toolbox/metafunctions/pixel_bit_size.hpp>

using namespace boost;
using namespace gil;

BOOST_AUTO_TEST_SUITE( pixel_bit_size_test )

BOOST_AUTO_TEST_CASE( pixel_bit_size_test )
{
    typedef bit_aligned_image5_type< 16, 16, 16, 8, 8, devicen_layout_t< 5 > >::type image_t;
    BOOST_STATIC_ASSERT(( pixel_bit_size<image_t::view_t::reference>::value == 64 ));
}

BOOST_AUTO_TEST_SUITE_END()


/*!
@file
Adapts `std::ratio` for use with Hana.

@copyright Louis Dionne 2013-2016
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_EXT_STD_RATIO_HPP
#define BOOST_HANA_EXT_STD_RATIO_HPP

#include <boost/hana/bool.hpp>
#include <boost/hana/concept/integral_constant.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/fwd/core/to.hpp>
#include <boost/hana/fwd/core/tag_of.hpp>
#include <boost/hana/fwd/div.hpp>
#include <boost/hana/fwd/equal.hpp>
#include <boost/hana/fwd/less.hpp>
#include <boost/hana/fwd/minus.hpp>
#include <boost/hana/fwd/mod.hpp>
#include <boost/hana/fwd/mult.hpp>
#include <boost/hana/fwd/one.hpp>
#include <boost/hana/fwd/plus.hpp>
#include <boost/hana/fwd/zero.hpp>

#include <cstdint>
#include <ratio>
#include <type_traits>

#ifdef BOOST_HANA_DOXYGEN_INVOKED
namespace std {
    //! @ingroup group-ext-std
    //! Adaptation of `std::ratio` for Hana.
    //!
    //!
    //! Modeled concepts
    //! ----------------
    //! 1. `Comparable`\n
    //! `std::ratio`s are compared for equality using `std::ratio_equal`.
    //! @include example/ext/std/ratio/comparable.cpp
    //!
    //! 2. `Orderable`\n
    //! `std::ratio`s are ordered using `std::ratio_less`.
    //! @include example/ext/std/ratio/orderable.cpp
    //!
    //! 3. `Monoid`, `Group`, `Ring`, and `EuclideanRing`\n
    //! `std::ratio`s are added, subtracted, multiplied and divided using
    //! `std::ratio_add`, `std::ratio_subtract`, `std::ratio_multiply` and
    //! `std::ratio_divide`, respectively. Furthermore, the neutral element
    //! for the additive operation is `std::ratio<0, 1>{}`, and the neutral
    //! element for the multiplicative operation is `std::ratio<1, 1>{}`.
    //! @include example/ext/std/ratio/arithmetic.cpp
    template <std::intmax_t Num, std::intmax_t Denom>
    class ratio { };
}
#endif

BOOST_HANA_NAMESPACE_BEGIN
    namespace ext { namespace std { struct ratio_tag; }}

    template <std::intmax_t num, std::intmax_t den>
    struct tag_of<std::ratio<num, den>> {
        using type = ext::std::ratio_tag;
    };

    //////////////////////////////////////////////////////////////////////////
    // Conversion from IntegralConstants
    //////////////////////////////////////////////////////////////////////////
    template <typename C>
    struct to_impl<ext::std::ratio_tag, C, when<
        hana::IntegralConstant<C>::value
    >> {
        template <typename N>
        static constexpr auto apply(N const&) {
            return std::ratio<N::value>{};
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct equal_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr auto apply(R1 const&, R2 const&)
        { return hana::bool_c<std::ratio_equal<R1, R2>::value>; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Orderable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct less_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr auto apply(R1 const&, R2 const&)
        { return hana::bool_c<std::ratio_less<R1, R2>::value>; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Monoid
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct plus_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr std::ratio_add<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    template <>
    struct zero_impl<ext::std::ratio_tag> {
        static constexpr std::ratio<0> apply()
        { return {}; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Group
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct minus_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr std::ratio_subtract<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Ring
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct mult_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr std::ratio_multiply<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    template <>
    struct one_impl<ext::std::ratio_tag> {
        static constexpr std::ratio<1> apply()
        { return {}; }
    };

    //////////////////////////////////////////////////////////////////////////
    // EuclideanRing
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct div_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr std::ratio_divide<R1, R2> apply(R1 const&, R2 const&)
        { return {}; }
    };

    template <>
    struct mod_impl<ext::std::ratio_tag, ext::std::ratio_tag> {
        template <typename R1, typename R2>
        static constexpr std::ratio<0> apply(R1 const&, R2 const&)
        { return {}; }
    };
BOOST_HANA_NAMESPACE_END

#endif // !BOOST_HANA_EXT_STD_RATIO_HPP


#ifndef ASLAM_CAMERA_GEOMETRY_BASE_HPP
#define ASLAM_CAMERA_GEOMETRY_BASE_HPP

#include <Eigen/Core>
#include <aslam/Time.hpp>
#include <boost/serialization/split_member.hpp>
#include <sm/Id.hpp>
#include <sm/PropertyTree.hpp>
#include <sm/boost/serialization.hpp>

namespace sm {
namespace kinematics {
class Transformation;
}  // namespace kinematics
}  // namespace sm

namespace aslam {
// Forward declaration
class FrameBase;
namespace cameras {

class GridCalibrationTargetObservation;

SM_DEFINE_ID(CameraId);

// The base class for camera geometry...this will help exporting things to python or supporting
// runtime polymorphism but it will be a bit slower than pure templated code.
class CameraGeometryBase {
  public:
    typedef boost::shared_ptr<CameraGeometryBase> Ptr;
    typedef boost::shared_ptr<const CameraGeometryBase> ConstPtr;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CameraGeometryBase();
    virtual ~CameraGeometryBase();

    virtual size_t keypointDimension() const = 0;

    // The functions below take variable sized matrices and hence they are prefixed by "vs" to avoid confusion with
    // the fixed-size matrix versions available in derived classes. The default implementation of these "vs" functions
    // will be inefficient as it will call the fixed-size derived functions and copy the data into the variable size
    // matrices. Still, this base class is useful as it allows us to maintain a heterogeneous list of camera geometries
    // and simplify the python interface.
    virtual bool vsEuclideanToKeypoint(const Eigen::Vector3d& p, Eigen::VectorXd& outKeypoint) const = 0;
    virtual bool vsEuclideanToKeypoint(const Eigen::Vector3d& p, Eigen::VectorXd& outKeypoint,
                                       Eigen::MatrixXd& outJacobian) const = 0;

    virtual bool vsHomogeneousToKeypoint(const Eigen::Vector4d& ph, Eigen::VectorXd& outKeypoint) const = 0;

    virtual bool vsHomogeneousToKeypoint(const Eigen::Vector4d& ph, Eigen::VectorXd& outKeypoint,
                                         Eigen::MatrixXd& outJacobian) const = 0;

    // If the camera model is invertible, these functions produce 3d
    // points.  otherwise they produce points where the length has no
    // meaning. In that case the points are not necessarily on the
    // unit sphere in R^3 (that might be expensive to compute).
    virtual bool vsKeypointToEuclidean(const Eigen::VectorXd& keypoint, Eigen::Vector3d& outPoint) const = 0;

    virtual bool vsKeypointToEuclidean(const Eigen::VectorXd& keypoint, Eigen::VectorXd& outPoint,
                                       Eigen::MatrixXd& outJacobian) const = 0;

    virtual bool vsKeypointToHomogeneous(Eigen::VectorXd const& keypoint, Eigen::VectorXd& outPoint) const = 0;

    virtual bool vsKeypointToHomogeneous(Eigen::VectorXd const& keypoint, Eigen::VectorXd& outPoint,
                                         Eigen::MatrixXd& outJacobian) const = 0;

    // The amount of time elapsed between the start of the image and the
    // keypoint. For a global shutter camera, this can return Duration(0).
    virtual Duration vsTemporalOffset(const Eigen::VectorXd& keypoint) const = 0;

    // These functions are needed for unit testing.
    virtual Eigen::VectorXd createRandomKeypoint() const = 0;
    virtual Eigen::Vector3d createRandomVisiblePoint(double depth = -1) const = 0;

    virtual bool vsIsValid(const Eigen::VectorXd& keypoint) const = 0;
    virtual bool vsIsEuclideanVisible(const Eigen::Vector3d& p) const = 0;
    virtual bool vsIsHomogeneousVisible(const Eigen::Vector4d& ph) const = 0;
    virtual bool isProjectionInvertible() const = 0;
    virtual CameraId id() const = 0;
    virtual void setId(CameraId id) = 0;

    /// \brief initialize the intrinsics based on a list of views of a gridded calibration target
    /// \return true on success
    virtual bool initializeIntrinsics(const std::vector<GridCalibrationTargetObservation>& observations) = 0;

    /// \brief estimate the transformation of the camera with respect to the calibration target
    ///        On success out_T_t_c is filled in with the transformation that takes points from
    ///        the camera frame to the target frame
    /// \return true on success
    virtual bool estimateTransformation(const GridCalibrationTargetObservation& obs,
                                        sm::kinematics::Transformation& out_T_t_c) const = 0;

    /// \brief The width of the underlying image
    virtual int width() const = 0;

    /// \brief The height of the underlying image
    virtual int height() const = 0;

    /// \brief create an uninitialized frame base object. This object has the right
    ///        type for the underlying camera system but does not have the camera system
    ///        set.
    virtual boost::shared_ptr<FrameBase> createUninitializedFrameBase() const = 0;

    // The amount of time elapsed between the start of the image and the
    // keypoint. For a global shutter camera, this can return Duration(0).
    virtual Duration temporalOffset(const Eigen::VectorXd& keypoint) const = 0;

    static boost::shared_ptr<CameraGeometryBase> create(const sm::PropertyTree& config);

    /// \brief print the internal parameters. This is just for debugging.
    virtual void print(std::ostream& out) = 0;

    // aslam::backend compatibility

    /// \brief the minimal dimensions of the projection parameters
    virtual int minimalDimensionsProjection() const = 0;

    /// \brief the minimal dimensions of the distortion parameters
    virtual int minimalDimensionsDistortion() const = 0;

    /// \brief the minimal dimensions of the shutter parameters
    virtual int minimalDimensionsShutter() const = 0;

    // aslam::backend compatibility

    /// \brief update the intrinsics
    virtual void update(const double* v, bool projection, bool distortion, bool shutter) = 0;

    /// \brief Get the total number of dimensions of the intrinsic parameters
    virtual int minimalDimensions(bool projection, bool distortion, bool shutter) const = 0;

    /// \brief get the intrinsic parameters.
    virtual void getParameters(Eigen::MatrixXd& P, bool projection, bool distortion, bool shutter) const = 0;

    /// \brief set the intrinsic parameters.
    virtual void setParameters(const Eigen::MatrixXd& P, bool projection, bool distortion, bool shutter) = 0;

    /// \brief return the Jacobian of the projection with respect to the intrinsics.
    virtual void euclideanToKeypointIntrinsicsJacobian(const Eigen::Vector3d& p, Eigen::MatrixXd& outJi,
                                                       bool estimateProjection, bool estimateDistortion,
                                                       bool estimateShutter) const = 0;

    /// \brief return the Jacobian of the projection with respect to the intrinsics.
    virtual void homogeneousToKeypointIntrinsicsJacobian(const Eigen::Vector4d& p, Eigen::MatrixXd& outJi,
                                                         bool estimateProjection, bool estimateDistortion,
                                                         bool estimateShutter) const = 0;

    /// \brief return the temporal offset with respect to the intrinsics.
    virtual void temporalOffsetIntrinsicsJacobian(const Eigen::VectorXd& keypoint, Eigen::MatrixXd& outJi,
                                                  bool estimateProjection, bool estimateDistortion,
                                                  bool estimateShutter) const = 0;

    virtual bool hasMask() const;

    ///////////////////////////////////////////////////
    // Serialization support
    ///////////////////////////////////////////////////
    enum { CLASS_SERIALIZATION_VERSION = 0 };
    BOOST_SERIALIZATION_SPLIT_MEMBER();

    template <class Archive>
    void load(Archive& /* ar */, const unsigned int /* version */) {}

    template <class Archive>
    void save(Archive& /* ar */, const unsigned int /* version */) const {}
};

}  // namespace cameras
}  // namespace aslam

SM_DEFINE_ID_HASH(aslam::cameras::CameraId);

SM_BOOST_CLASS_VERSION(aslam::cameras::CameraGeometryBase);

#endif /* ASLAM_CAMERA_GEOMETRY_BASE_HPP */


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Thu 10 May 2018 15:01:38

#ifndef NUHMSSM_susy_parameters_H
#define NUHMSSM_susy_parameters_H

#include "betafunction.hpp"
#include "NUHMSSM_input_parameters.hpp"

#include <iosfwd>
#include <string>
#include <vector>
#include <Eigen/Core>

namespace flexiblesusy {

#ifdef TRACE_STRUCT_TYPE
   #undef TRACE_STRUCT_TYPE
#endif
#define TRACE_STRUCT_TYPE Susy_traces

class NUHMSSM_susy_parameters : public Beta_function {
public:
   explicit NUHMSSM_susy_parameters(const NUHMSSM_input_parameters& input_ = NUHMSSM_input_parameters());
   NUHMSSM_susy_parameters(double scale_, int loops_, int thresholds_, const NUHMSSM_input_parameters& input_, const Eigen::Matrix<double,3,3>& Yd_, const Eigen::Matrix<double,3,3>& Ye_
   , const Eigen::Matrix<double,3,3>& Yu_, double Mu_, double g1_, double g2_,
   double g3_, double vd_, double vu_
);
   NUHMSSM_susy_parameters(const NUHMSSM_susy_parameters&) = default;
   NUHMSSM_susy_parameters(NUHMSSM_susy_parameters&&) = default;
   virtual ~NUHMSSM_susy_parameters() = default;
   NUHMSSM_susy_parameters& operator=(const NUHMSSM_susy_parameters&) = default;
   NUHMSSM_susy_parameters& operator=(NUHMSSM_susy_parameters&&) = default;

   virtual Eigen::ArrayXd beta() const override;
   virtual Eigen::ArrayXd get() const override;
   virtual void print(std::ostream&) const;
   virtual void set(const Eigen::ArrayXd&) override;
   const NUHMSSM_input_parameters& get_input() const;
   NUHMSSM_input_parameters& get_input();
   void set_input_parameters(const NUHMSSM_input_parameters&);

   NUHMSSM_susy_parameters calc_beta() const;
   NUHMSSM_susy_parameters calc_beta(int) const;
   virtual void clear();

   void set_Yd(const Eigen::Matrix<double,3,3>& Yd_) { Yd = Yd_; }
   void set_Yd(int i, int k, const double& value) { Yd(i,k) = value; }
   void set_Ye(const Eigen::Matrix<double,3,3>& Ye_) { Ye = Ye_; }
   void set_Ye(int i, int k, const double& value) { Ye(i,k) = value; }
   void set_Yu(const Eigen::Matrix<double,3,3>& Yu_) { Yu = Yu_; }
   void set_Yu(int i, int k, const double& value) { Yu(i,k) = value; }
   void set_Mu(double Mu_) { Mu = Mu_; }
   void set_g1(double g1_) { g1 = g1_; }
   void set_g2(double g2_) { g2 = g2_; }
   void set_g3(double g3_) { g3 = g3_; }
   void set_vd(double vd_) { vd = vd_; }
   void set_vu(double vu_) { vu = vu_; }

   const Eigen::Matrix<double,3,3>& get_Yd() const { return Yd; }
   double get_Yd(int i, int k) const { return Yd(i,k); }
   const Eigen::Matrix<double,3,3>& get_Ye() const { return Ye; }
   double get_Ye(int i, int k) const { return Ye(i,k); }
   const Eigen::Matrix<double,3,3>& get_Yu() const { return Yu; }
   double get_Yu(int i, int k) const { return Yu(i,k); }
   double get_Mu() const { return Mu; }
   double get_g1() const { return g1; }
   double get_g2() const { return g2; }
   double get_g3() const { return g3; }
   double get_vd() const { return vd; }
   double get_vu() const { return vu; }

   Eigen::Matrix<double,3,3> get_SqSq() const;
   Eigen::Matrix<double,3,3> get_SlSl() const;
   double get_SHdSHd() const;
   double get_SHuSHu() const;
   Eigen::Matrix<double,3,3> get_SdRSdR() const;
   Eigen::Matrix<double,3,3> get_SuRSuR() const;
   Eigen::Matrix<double,3,3> get_SeRSeR() const;

protected:
   Eigen::Matrix<double,3,3> Yd{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> Ye{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> Yu{Eigen::Matrix<double,3,3>::Zero()};
   double Mu{};
   double g1{};
   double g2{};
   double g3{};
   double vd{};
   double vu{};

   NUHMSSM_input_parameters input{};

private:
   static const int numberOfParameters = 33;

   struct Susy_traces {
      double traceYdAdjYd{};
      double traceYeAdjYe{};
      double traceYdAdjYdYdAdjYd{};
      double traceYdAdjYuYuAdjYd{};
      double traceYeAdjYeYeAdjYe{};
      double traceYuAdjYu{};
      double traceAdjYdYd{};
      double traceAdjYeYe{};
      double traceAdjYuYu{};
      double traceAdjYdYdAdjYdYd{};
      double traceAdjYeYeAdjYeYe{};
      double traceAdjYuYuAdjYdYd{};
      double traceAdjYuYuAdjYuYu{};
      double traceAdjYdYdAdjYdYdAdjYdYd{};
      double traceAdjYeYeAdjYeYeAdjYeYe{};
      double traceAdjYuYuAdjYuYuAdjYdYd{};
      double traceYuAdjYuYuAdjYu{};
      double traceAdjYdYdAdjYuYuAdjYdYd{};
      double traceAdjYuYuAdjYuYuAdjYuYu{};

   };
   Susy_traces calc_susy_traces(int) const;

   Eigen::Matrix<double,3,3> calc_beta_Yd_1_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yd_2_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yd_3_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Ye_1_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Ye_2_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Ye_3_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yu_1_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yu_2_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yu_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Mu_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Mu_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Mu_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g1_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g1_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g1_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g2_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g2_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g2_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g3_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g3_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g3_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vd_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vd_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vd_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vu_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vu_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vu_3_loop(const TRACE_STRUCT_TYPE&) const;

};

std::ostream& operator<<(std::ostream&, const NUHMSSM_susy_parameters&);

#undef TRACE_STRUCT_TYPE

} // namespace flexiblesusy

#endif


/**
 * @file ffn_impl.hpp
 * @author Marcus Edel
 *
 * Definition of the FFN class, which implements feed forward neural networks.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_FFN_IMPL_HPP
#define MLPACK_METHODS_ANN_FFN_IMPL_HPP

// In case it hasn't been included yet.
#include "ffn.hpp"

#include "visitor/forward_visitor.hpp"
#include "visitor/backward_visitor.hpp"
#include "visitor/deterministic_set_visitor.hpp"
#include "visitor/gradient_set_visitor.hpp"
#include "visitor/gradient_visitor.hpp"
#include "visitor/set_input_height_visitor.hpp"
#include "visitor/set_input_width_visitor.hpp"

#include <boost/serialization/variant.hpp>

namespace mlpack {
namespace ann /** Artificial Neural Network. */ {

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::FFN(
    OutputLayerType outputLayer, InitializationRuleType initializeRule) :
    outputLayer(std::move(outputLayer)),
    initializeRule(std::move(initializeRule)),
    width(0),
    height(0),
    reset(false),
    numFunctions(0),
    deterministic(true)
{
  /* Nothing to do here */
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::~FFN()
{
  std::for_each(network.begin(), network.end(),
      boost::apply_visitor(deleteVisitor));
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::ResetData(
    arma::mat predictors, arma::mat responses)
{
  numFunctions = responses.n_cols;
  this->predictors = std::move(predictors);
  this->responses = std::move(responses);
  this->deterministic = true;
  ResetDeterministic();

  if (!reset)
    ResetParameters();
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType, typename... CallbackTypes>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Train(
      arma::mat predictors,
      arma::mat responses,
      OptimizerType& optimizer,
      CallbackTypes&&... callbacks)
{
  ResetData(std::move(predictors), std::move(responses));

  // Train the model.
  Timer::Start("ffn_optimization");
  const double out = optimizer.Optimize(*this, parameter, callbacks...);
  Timer::Stop("ffn_optimization");

  Log::Info << "FFN::FFN(): final objective of trained model is " << out
      << "." << std::endl;
  return out;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType, typename... CallbackTypes>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Train(
    arma::mat predictors,
    arma::mat responses,
    CallbackTypes&&... callbacks)
{
  ResetData(std::move(predictors), std::move(responses));

  OptimizerType optimizer;

  // Train the model.
  Timer::Start("ffn_optimization");
  const double out = optimizer.Optimize(*this, parameter, callbacks...);
  Timer::Stop("ffn_optimization");

  Log::Info << "FFN::FFN(): final objective of trained model is " << out
      << "." << std::endl;
  return out;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Forward(
    arma::mat inputs, arma::mat& results)
{
  if (parameter.is_empty())
    ResetParameters();

  if (!deterministic)
  {
    deterministic = true;
    ResetDeterministic();
  }

  currentInput = std::move(inputs);
  Forward(std::move(currentInput));
  results = boost::apply_visitor(outputParameterVisitor, network.back());
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Forward(
    arma::mat inputs, arma::mat& results, const size_t begin, const size_t end)
{
  boost::apply_visitor(ForwardVisitor(std::move(inputs), std::move(
      boost::apply_visitor(outputParameterVisitor, network[begin]))),
      network[begin]);

  for (size_t i = 1; i < end - begin + 1; ++i)
  {
    boost::apply_visitor(ForwardVisitor(std::move(boost::apply_visitor(
        outputParameterVisitor, network[begin + i - 1])), std::move(
        boost::apply_visitor(outputParameterVisitor, network[begin + i]))),
        network[begin + i]);
  }

  results = boost::apply_visitor(outputParameterVisitor, network[end]);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Backward(
    arma::mat targets, arma::mat& gradients)
{
  double res = outputLayer.Forward(std::move(boost::apply_visitor(
      outputParameterVisitor, network.back())), std::move(targets));

  for (size_t i = 0; i < network.size(); ++i)
  {
    res += boost::apply_visitor(lossVisitor, network[i]);
  }

  outputLayer.Backward(std::move(boost::apply_visitor(outputParameterVisitor,
      network.back())), std::move(targets), std::move(error));

  gradients = arma::zeros<arma::mat>(parameter.n_rows, parameter.n_cols);

  Backward();
  ResetGradients(gradients);
  Gradient(std::move(currentInput));

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Predict(
    arma::mat predictors, arma::mat& results)
{
  if (parameter.is_empty())
    ResetParameters();

  if (!deterministic)
  {
    deterministic = true;
    ResetDeterministic();
  }

  arma::mat resultsTemp;
  Forward(std::move(arma::mat(predictors.colptr(0),
      predictors.n_rows, 1, false, true)));
  resultsTemp = boost::apply_visitor(outputParameterVisitor,
      network.back()).col(0);

  results = arma::mat(resultsTemp.n_elem, predictors.n_cols);
  results.col(0) = resultsTemp.col(0);

  for (size_t i = 1; i < predictors.n_cols; i++)
  {
    Forward(std::move(arma::mat(predictors.colptr(i),
        predictors.n_rows, 1, false, true)));

    resultsTemp = boost::apply_visitor(outputParameterVisitor,
        network.back());
    results.col(i) = resultsTemp.col(0);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Evaluate(
    arma::mat predictors, arma::mat responses)
{
  if (parameter.is_empty())
    ResetParameters();

  if (!deterministic)
  {
    deterministic = true;
    ResetDeterministic();
  }

  Forward(std::move(predictors));

  double res = outputLayer.Forward(std::move(boost::apply_visitor(
      outputParameterVisitor, network.back())), std::move(responses));

  for (size_t i = 0; i < network.size(); ++i)
  {
    res += boost::apply_visitor(lossVisitor, network[i]);
  }

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Evaluate(
    const arma::mat& parameters)
{
  double res = 0;
  for (size_t i = 0; i < predictors.n_cols; ++i)
    res += Evaluate(parameters, i, 1, true);

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Evaluate(
    const arma::mat& /* parameters */,
    const size_t begin,
    const size_t batchSize,
    const bool deterministic)
{
  if (parameter.is_empty())
    ResetParameters();

  if (deterministic != this->deterministic)
  {
    this->deterministic = deterministic;
    ResetDeterministic();
  }

  Forward(std::move(predictors.cols(begin, begin + batchSize - 1)));
  double res = outputLayer.Forward(
      std::move(boost::apply_visitor(outputParameterVisitor, network.back())),
      std::move(responses.cols(begin, begin + batchSize - 1)));

  for (size_t i = 0; i < network.size(); ++i)
  {
    res += boost::apply_visitor(lossVisitor, network[i]);
  }

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Evaluate(
    const arma::mat& parameters, const size_t begin, const size_t batchSize)
{
  return Evaluate(parameters, begin, batchSize, true);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename GradType>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::
EvaluateWithGradient(const arma::mat& parameters, GradType& gradient)
{
  double res = 0;
  for (size_t i = 0; i < predictors.n_cols; ++i)
    res += EvaluateWithGradient(parameters, i, gradient, 1);

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename GradType>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::
EvaluateWithGradient(const arma::mat& /* parameters */,
                     const size_t begin,
                     GradType& gradient,
                     const size_t batchSize)
{
  if (gradient.is_empty())
  {
    if (parameter.is_empty())
      ResetParameters();

    gradient = arma::zeros<arma::mat>(parameter.n_rows, parameter.n_cols);
  }
  else
  {
    gradient.zeros();
  }

  if (this->deterministic)
  {
    this->deterministic = false;
    ResetDeterministic();
  }

  Forward(std::move(predictors.cols(begin, begin + batchSize - 1)));
  double res = outputLayer.Forward(
      std::move(boost::apply_visitor(outputParameterVisitor, network.back())),
      std::move(responses.cols(begin, begin + batchSize - 1)));

  for (size_t i = 0; i < network.size(); ++i)
  {
    res += boost::apply_visitor(lossVisitor, network[i]);
  }

  outputLayer.Backward(
      std::move(boost::apply_visitor(outputParameterVisitor, network.back())),
      std::move(responses.cols(begin, begin + batchSize - 1)),
      std::move(error));

  Backward();
  ResetGradients(gradient);
  Gradient(std::move(predictors.cols(begin, begin + batchSize - 1)));

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Gradient(
    const arma::mat& parameters,
    const size_t begin,
    arma::mat& gradient,
    const size_t batchSize)
{
  this->EvaluateWithGradient(parameters, begin, gradient, batchSize);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Shuffle()
{
  math::ShuffleData(predictors, responses, predictors, responses);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::ResetParameters()
{
  ResetDeterministic();

  // Reset the network parameter with the given initialization rule.
  NetworkInitialization<InitializationRuleType,
                        CustomLayers...> networkInit(initializeRule);
  networkInit.Initialize(network, parameter);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::ResetDeterministic()
{
  DeterministicSetVisitor deterministicSetVisitor(deterministic);
  std::for_each(network.begin(), network.end(),
      boost::apply_visitor(deterministicSetVisitor));
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::ResetGradients(arma::mat& gradient)
{
  size_t offset = 0;
  for (size_t i = 0; i < network.size(); ++i)
  {
    offset += boost::apply_visitor(GradientSetVisitor(std::move(gradient),
        offset), network[i]);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::Forward(arma::mat&& input)
{
  boost::apply_visitor(ForwardVisitor(std::move(input), std::move(
      boost::apply_visitor(outputParameterVisitor, network.front()))),
      network.front());

  if (!reset)
  {
    if (boost::apply_visitor(outputWidthVisitor, network.front()) != 0)
    {
      width = boost::apply_visitor(outputWidthVisitor, network.front());
    }

    if (boost::apply_visitor(outputHeightVisitor, network.front()) != 0)
    {
      height = boost::apply_visitor(outputHeightVisitor, network.front());
    }
  }

  for (size_t i = 1; i < network.size(); ++i)
  {
    if (!reset)
    {
      // Set the input width.
      boost::apply_visitor(SetInputWidthVisitor(width), network[i]);

      // Set the input height.
      boost::apply_visitor(SetInputHeightVisitor(height), network[i]);
    }

    boost::apply_visitor(ForwardVisitor(std::move(boost::apply_visitor(
        outputParameterVisitor, network[i - 1])), std::move(
        boost::apply_visitor(outputParameterVisitor, network[i]))), network[i]);

    if (!reset)
    {
      // Get the output width.
      if (boost::apply_visitor(outputWidthVisitor, network[i]) != 0)
      {
        width = boost::apply_visitor(outputWidthVisitor, network[i]);
      }

      // Get the output height.
      if (boost::apply_visitor(outputHeightVisitor, network[i]) != 0)
      {
        height = boost::apply_visitor(outputHeightVisitor, network[i]);
      }
    }
  }

  if (!reset)
    reset = true;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Backward()
{
  boost::apply_visitor(BackwardVisitor(std::move(boost::apply_visitor(
      outputParameterVisitor, network.back())), std::move(error), std::move(
      boost::apply_visitor(deltaVisitor, network.back()))), network.back());

  for (size_t i = 2; i < network.size(); ++i)
  {
    boost::apply_visitor(BackwardVisitor(std::move(boost::apply_visitor(
        outputParameterVisitor, network[network.size() - i])), std::move(
        boost::apply_visitor(deltaVisitor, network[network.size() - i + 1])),
        std::move(boost::apply_visitor(deltaVisitor,
        network[network.size() - i]))), network[network.size() - i]);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::Gradient(arma::mat&& input)
{
  boost::apply_visitor(GradientVisitor(std::move(input), std::move(
      boost::apply_visitor(deltaVisitor, network[1]))), network.front());

  for (size_t i = 1; i < network.size() - 1; ++i)
  {
    boost::apply_visitor(GradientVisitor(std::move(boost::apply_visitor(
        outputParameterVisitor, network[i - 1])), std::move(
        boost::apply_visitor(deltaVisitor, network[i + 1]))), network[i]);
  }

  boost::apply_visitor(GradientVisitor(std::move(boost::apply_visitor(
      outputParameterVisitor, network[network.size() - 2])), std::move(error)),
      network[network.size() - 1]);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename Archive>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::serialize(
    Archive& ar, const unsigned int version)
{
  ar & BOOST_SERIALIZATION_NVP(parameter);
  ar & BOOST_SERIALIZATION_NVP(width);
  ar & BOOST_SERIALIZATION_NVP(height);
  ar & BOOST_SERIALIZATION_NVP(currentInput);

  // Earlier versions of the FFN code did not serialize whether or not the model
  // was reset.
  if (version > 0)
  {
    ar & BOOST_SERIALIZATION_NVP(reset);
  }

  // Be sure to clear other layers before loading.
  if (Archive::is_loading::value)
  {
    std::for_each(network.begin(), network.end(),
        boost::apply_visitor(deleteVisitor));
    network.clear();
  }

  ar & BOOST_SERIALIZATION_NVP(network);

  // If we are loading, we need to initialize the weights.
  if (Archive::is_loading::value)
  {
    // The behavior in earlier versions was to always assume the weights needed
    // to be reset.
    if (version == 0)
      reset = false;

    size_t offset = 0;
    for (size_t i = 0; i < network.size(); ++i)
    {
      offset += boost::apply_visitor(WeightSetVisitor(std::move(parameter),
          offset), network[i]);

      boost::apply_visitor(resetVisitor, network[i]);
    }

    deterministic = true;
    ResetDeterministic();
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::Swap(FFN& network)
{
  std::swap(outputLayer, network.outputLayer);
  std::swap(initializeRule, network.initializeRule);
  std::swap(width, network.width);
  std::swap(height, network.height);
  std::swap(reset, network.reset);
  std::swap(this->network, network.network);
  std::swap(predictors, network.predictors);
  std::swap(responses, network.responses);
  std::swap(parameter, network.parameter);
  std::swap(numFunctions, network.numFunctions);
  std::swap(error, network.error);
  std::swap(currentInput, network.currentInput);
  std::swap(deterministic, network.deterministic);
  std::swap(delta, network.delta);
  std::swap(inputParameter, network.inputParameter);
  std::swap(outputParameter, network.outputParameter);
  std::swap(gradient, network.gradient);
};

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::FFN(
    const FFN& network):
    outputLayer(network.outputLayer),
    initializeRule(network.initializeRule),
    width(network.width),
    height(network.height),
    reset(network.reset),
    predictors(network.predictors),
    responses(network.responses),
    parameter(network.parameter),
    numFunctions(network.numFunctions),
    error(network.error),
    currentInput(network.currentInput),
    deterministic(network.deterministic),
    delta(network.delta),
    inputParameter(network.inputParameter),
    outputParameter(network.outputParameter),
    gradient(network.gradient)
{
  // Build new layers according to source network
  for (size_t i = 0; i < network.network.size(); ++i)
  {
    this->network.push_back(boost::apply_visitor(copyVisitor,
        network.network[i]));
  }
};

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::FFN(
    FFN&& network):
    outputLayer(std::move(network.outputLayer)),
    initializeRule(std::move(network.initializeRule)),
    width(network.width),
    height(network.height),
    reset(network.reset),
    predictors(std::move(network.predictors)),
    responses(std::move(network.responses)),
    parameter(std::move(network.parameter)),
    numFunctions(network.numFunctions),
    error(std::move(network.error)),
    currentInput(std::move(network.currentInput)),
    deterministic(network.deterministic),
    delta(std::move(network.delta)),
    inputParameter(std::move(network.inputParameter)),
    outputParameter(std::move(network.outputParameter)),
    gradient(std::move(network.gradient))
{
  this->network = std::move(network.network);
};

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
FFN<OutputLayerType, InitializationRuleType, CustomLayers...>&
FFN<OutputLayerType, InitializationRuleType,
    CustomLayers...>::operator = (FFN network)
{
  Swap(network);
  return *this;
};

} // namespace ann
} // namespace mlpack

#endif


/******************************************************************************
* Copyright (C) 2008 - Ikaro Games   www.ikarogames.com                       *
*                                                                             *
* 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 2              *
* 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, write to the Free Software                 *
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA. *
*                                                                             *
*                                                                             *
******************************************************************************/
#include "CSimulationManager.h"

#include <stdlib.h>
#include <cmath>
#include <boost/filesystem/operations.hpp>

#include "projectfootball_config.h"
#include "CSimulationWorld.h"
#include "message/CMessageDispatcher.h"
#include "message/MessageTypes.h"
#include "entity/CEntityManager.h"
#include "entity/CFootballPlayer.h"
#include "entity/CReferee.h"
#include "entity/CTeam.h"
#include "entity/CField.h"
#include "entity/CBall.h"
#include "tactic/CFormation.h"

#include "../CSinglePlayerGame.h"
#include "../db/bean/CPfMatches.h"
#include "../db/bean/CPfTeams.h"
#include "../db/dao/factory/IDAOFactory.h"
#include "../event/match/CStartMatchEvent.h"
#include "../event/match/CEndMatchEvent.h"
#include "../event/match/CGoalMatchEvent.h"
#include "../option/CSinglePlayerOptionManager.h"
#include "../wh/CSimulatorWindowHandler.h"

#include "../../audio/CAudioSystem.h"
#include "../../bullet/LinearMath/btVector3.h"
#include "../../bullet/BulletDynamics/Dynamics/btRigidBody.h"
#include "../../engine/CGameEngine.h"
#include "../../engine/event/CEventManager.h"
#include "../../engine/option/CSystemOptionManager.h"
#include "../../engine/utils/CTimer.h"
#include "../../utils/CLog.h"
#include "../../utils/CDate.h"

extern std::string dataPath;

CSimulationManager::CSimulationManager(int xMatch, CSinglePlayerGame &game, CSimulatorWindowHandler &simulatorWH) :
	m_goalEvents(),
	m_game(game),
	m_simulatorWindowHandler(simulatorWH)
{
    LOG_DEBUG("CSimulationManager()");

    CSystemOptionManager* optionManager = CSystemOptionManager::getInstance();

    m_maxBallVelocity           = optionManager->getSimulationMaxBallVelocity();
    m_maxPlayerVelocity         = optionManager->getSimulationMaxPlayerVelocity();
    m_maxKickPower              = optionManager->getSimulationMaxKickPower();
    m_maxKickDistance           = optionManager->getSimulationMaxKickDistance();
    m_nearestPlayerToBallCycles = optionManager->getSimulationNearestPlayerToBallCycles();
    m_nearestPlayerToBallTime   = optionManager->getSimulationNearestPlayerToBallTime() * 0.01;
    m_logicTimer                = new CTimer(optionManager->getSimulationLogicFrequency());
    m_physicsTimer              = new CTimer(optionManager->getSimulationPhysicsFrequency());

    m_simWorld = new CSimulationWorld();

    m_field = new CField();
    m_simWorld->addObject(m_field);

    m_referee = new CReferee(this);
    m_simWorld->addObject(m_referee);

    m_ball = new CBall();
    m_simWorld->addObject(m_ball);

    IPfMatchesDAO *matchesDAO = m_game.getIDAOFactory()->getIPfMatchesDAO();
    m_match = matchesDAO->findByXMatch(xMatch);

    int sideLeft = true;
    std::string currentTimestamp = m_game.getCurrentTime().getTimestamp();

    // Home team
    CPfTeams *homeTeam = m_game.getIDAOFactory()->getIPfTeamsDAO()->findByXTeam(m_match->getXFkTeamHome());
    m_homeTeam = new CTeam(this, homeTeam, sideLeft);
    CPfFormations *formation = m_game.getIDAOFactory()->getIPfFormationsDAO()->findActiveByXTeam(m_homeTeam->getXTeam(), currentTimestamp);
    m_homeFormation = loadFormation(formation);
    m_homeTeam->setFormation(m_homeFormation);
    m_homeTeamPlayers = loadTeamPlayers(m_homeTeam);
    m_homeTeam->setPlayers(m_homeTeamPlayers);
    m_homeTeam->assignSide(sideLeft);
    delete homeTeam;
    delete formation;

    // Away team
    CPfTeams *awayTeam = m_game.getIDAOFactory()->getIPfTeamsDAO()->findByXTeam(m_match->getXFkTeamAway());
    m_awayTeam = new CTeam(this, awayTeam, !sideLeft);
    formation = m_game.getIDAOFactory()->getIPfFormationsDAO()->findActiveByXTeam(m_awayTeam->getXTeam(), currentTimestamp);
    m_awayFormation = loadFormation(formation);
    m_awayTeam->setFormation(m_awayFormation);
    m_awayTeamPlayers = loadTeamPlayers(m_awayTeam);
    m_awayTeam->setPlayers(m_awayTeamPlayers);
    m_awayTeam->assignSide(!sideLeft);
    delete awayTeam;
    delete formation;

    m_homeTeam->setOpponentTeam(m_awayTeam);
    m_awayTeam->setOpponentTeam(m_homeTeam);

    srand(time(NULL));
}

CSimulationManager::~CSimulationManager()
{
    LOG_DEBUG("~CSimulationManager()");

    CMessageDispatcher::getInstance()->reset();
    CEntityManager::getInstance()->reset();
    if(m_simWorld != NULL) {
        delete m_simWorld;
    }
    if(m_logicTimer != NULL) {
        delete m_logicTimer;
    }
    if(m_physicsTimer != NULL) {
        delete m_physicsTimer;
    }
    if(m_homeTeam != NULL) {
        delete m_homeTeam;
        delete m_homeFormation;

    }
    if(m_awayTeam != NULL) {
        delete m_awayTeam;
        delete m_awayFormation;
    }

    delete m_match;
}

void CSimulationManager::update()
{
    if(m_logicTimer->nextTick()) {
        calculateNearestPlayersToBall();
        m_referee->update();
        m_homeTeam->update();
        m_awayTeam->update();
    }

    if(m_physicsTimer->nextTick()) {
        m_simWorld->update();
    }
    CMessageDispatcher::getInstance()->dispatchDelayedMessages();
}

void CSimulationManager::addToLog(const std::string &text)
{
	m_simulatorWindowHandler.addToLog(text);
}

CSimulationWorld* CSimulationManager::getSimulationWorld()
{
    return m_simWorld;
}

CReferee* CSimulationManager::getReferee()
{
    return m_referee;
}

CBall* CSimulationManager::getBall()
{
    return m_ball;
}

CField* CSimulationManager::getField()
{
    return m_field;
}

CTeam* CSimulationManager::getHomeTeam()
{
    return m_homeTeam;
}

CTeam* CSimulationManager::getAwayTeam()
{
    return m_awayTeam;
}

void CSimulationManager::changePlayersSide()
{
    m_homeTeam->changeSide();
    m_awayTeam->changeSide();
}

void CSimulationManager::startMatch()
{
    CMessageDispatcher::getInstance()->dispatchMsg(0, -1, m_referee->getID(), Msg_StartMatch, 0);
}

void CSimulationManager::goalMatchEvent(CTeam *teamScorer, CFootballPlayer *playerScorer, int minute, bool ownGoal)
{
    CDate goalDate(m_match->getDMatch());
    goalDate.setMin(goalDate.getMin()+minute);

    m_goalEvents.push_back(new CGoalMatchEvent(goalDate, m_match->getXMatch(), teamScorer->getXTeam(), playerScorer->getXTeamPlayer(), minute, ownGoal));
    m_simulatorWindowHandler.updateScore();
}

void CSimulationManager::endMatchEvent()
{
	CEventManager	*eventMngr	= CGameEngine::getInstance()->getEventManager();

    // Start match event
	eventMngr->addEvent(new CStartMatchEvent(m_match->getDMatch(), m_match->getXMatch()));

    // Goals events
    std::vector<CGoalMatchEvent*>::iterator it;
    for(it = m_goalEvents.begin(); it!=m_goalEvents.end(); it++) {
    	eventMngr->addEvent((*it));
    }

    // End match event
    CDate endDate(m_match->getDMatch());
    endDate.setMin(endDate.getMin()+90);
    eventMngr->addEvent(new CEndMatchEvent(endDate, m_match->getXMatch()));

    // Send notitify to simulator screen
    m_simulatorWindowHandler.endMatchEvent();
}

void CSimulationManager::changeFormationEvent(int pos)
{
    CPfTeams *team = m_game.getIDAOFactory()->getIPfTeamsDAO()->findByXTeam(m_game.getOptionManager()->getGamePlayerTeam());
    int xTeam = team->getXTeam();
    if(xTeam == m_homeTeam->getXTeam()) {
    	CPfFormations *formation = m_game.getIDAOFactory()->getIPfFormationsDAO()->findByXFormation(pos);
    	CFormation *aux = m_homeFormation;
    	m_homeFormation = loadFormation(formation);
        m_homeTeam->setFormation(m_homeFormation);
        delete formation;
        delete aux;
    } else if(xTeam == m_awayTeam->getXTeam()) {
    	CPfFormations *formation = m_game.getIDAOFactory()->getIPfFormationsDAO()->findByXFormation(pos);
    	CFormation *aux = m_awayFormation;
    	m_awayFormation = loadFormation(formation);
        m_awayTeam->setFormation(m_awayFormation);
        delete formation;
        delete aux;
    } else {
        LOG_ERROR("Can't switch formation, team %d is not playing the match.", xTeam);
    }
    delete team;
}

const std::string& CSimulationManager::getHomeTeamName()
{
    return m_homeTeam->getName();
}

const std::string& CSimulationManager::getAwayTeamName()
{
    return m_awayTeam->getName();
}

btVector3 CSimulationManager::getBallPosition() const
{
    return m_ball->getPosition();
}

int CSimulationManager::getMatchDuration() const
{
    return m_game.getOptionManager()->getMatchDuration();
}

void CSimulationManager::dash(CFootballPlayer *player, btVector3 power)
{
    if(player->canDoActions()) {
        btVector3 impulse;
        double maxPlayerDash = player->getMaxVelocity();
        btRigidBody *body = player->getBody();
        btVector3 currentVelocity = body->getLinearVelocity();
        btVector3 newVelocity = currentVelocity;
        newVelocity += power;
        if(maxPlayerDash > m_maxPlayerVelocity) {
            truncateVector(&newVelocity, m_maxPlayerVelocity);
        } else {
            truncateVector(&newVelocity, maxPlayerDash);
        }
        //printf("Cycle: %d Player: %s Power: %f Mass: %f NewVelocity: %f\n", m_referee->getCycle(), player->getIdent().c_str(), power.length(), 1.0/body->getInvMass(), newVelocity.length());
        body->setLinearVelocity(newVelocity);
    }
}

void CSimulationManager::move(CFootballPlayer *player, int x, int z)
{
    if(m_referee->isMoveLegal()) {
        btRigidBody *body = player->getBody();
        body->clearForces();
        body->setLinearVelocity(btVector3(0,0,0));
        body->setAngularVelocity(btVector3(0,0,0));
        player->setPosition(x,0,z);
    }
}

void CSimulationManager::kick(CFootballPlayer *player, btVector3 power)
{
    if(player->canKickBall(m_referee->getCycle()) && player->canDoActions()) {
        btVector3 impulse;
        double maxPlayerKickPower = player->getMaxKickPower();
        btRigidBody *ballBody = m_ball->getBody();
        btVector3 velocity;

        if (maxPlayerKickPower > m_maxKickPower) {
            truncateVector(&power, m_maxKickPower);
        } else {
            truncateVector(&power, maxPlayerKickPower);
        }
        ballBody->applyCentralImpulse(power);
        velocity = ballBody->getLinearVelocity();
        truncateVector(&velocity, m_maxBallVelocity);
        ballBody->setLinearVelocity(velocity);
        CMessageDispatcher::getInstance()->dispatchMsg(0, player->getID(), m_referee->getID(), Msg_TouchBall, 0);
        CAudioSystem::getInstance()->lowKick()->play();
    }
}

void CSimulationManager::truncateVector(btVector3 *v, double max)
{
    if (v->length() > max) {
        v->normalize();
        v->operator *=(max);
    }
}

void CSimulationManager::calculateNearestPlayersToBall()
{
    std::vector<CFootballPlayer*> *homePlayers = getHomeTeam()->getPlayers();
    std::vector<CFootballPlayer*> *awayPlayers = getAwayTeam()->getPlayers();

    int cycle = 0;
    double time = 0.0;
    bool found = false;
    btScalar minHomeDist = 1000;
    btScalar minAwayDist = 1000;
    btScalar auxDist;
    std::vector<CFootballPlayer*>::iterator it;

    CFootballPlayer* nearestPlayer = NULL;
    CFootballPlayer* nearestHomePlayer = NULL;
    CFootballPlayer* nearestAwayPlayer = NULL;

    while(cycle < m_nearestPlayerToBallCycles && !found) {
        btVector3 ballPos = m_ball->futurePosition(time);

        for(it = homePlayers->begin(); it!=homePlayers->end(); it++) {
            btVector3 playerPos = (*it)->getFuturePlayerPosition(time, ballPos);
            auxDist = playerPos.distance(ballPos);
            if(auxDist < minHomeDist) {
                minHomeDist = auxDist;
                nearestHomePlayer = (*it);
            }
        }

        for(it = awayPlayers->begin(); it!=awayPlayers->end(); it++) {
            btVector3 playerPos = (*it)->getFuturePlayerPosition(time, ballPos);
            auxDist = playerPos.distance(ballPos);
            if(auxDist < minAwayDist) {
                minAwayDist = auxDist;
                nearestAwayPlayer = (*it);
            }
        }

        if(minHomeDist < minAwayDist) {
            nearestPlayer = nearestHomePlayer;
            if(minHomeDist < m_maxKickDistance) {
                found = true;
            }
        } else {
            nearestPlayer = nearestAwayPlayer;
            if(minAwayDist < m_maxKickDistance) {
                found = true;
            }
        }

        cycle++;
        time += m_nearestPlayerToBallTime;
    }
    getHomeTeam()->setNearestPlayerToBall(nearestHomePlayer);
    getAwayTeam()->setNearestPlayerToBall(nearestAwayPlayer);
}

std::vector<CFootballPlayer*>* CSimulationManager::loadTeamPlayers(CTeam *team)
{
	int xTeam = team->getXTeam();
	std::string currentTimestamp = m_game.getCurrentTime().getTimestamp();
	std::vector<CPfTeamPlayers*> *playersVector = m_game.getIDAOFactory()->getIPfTeamPlayersDAO()->findLineUpByXFkTeam(xTeam, currentTimestamp);

	std::vector<CFootballPlayer*>* teamPlayers = new std::vector<CFootballPlayer*>();

    // TODO: Initialize football players on a loop
    CFootballPlayer *player = new CFootballPlayer(this, playersVector->at(0), 1, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/goalie.lua"));
    player->getFSM()->setGlobalState("SGoalie_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

    player = new CFootballPlayer(this, playersVector->at(1), 2, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/player.lua"));
    player->getFSM()->setGlobalState("SPl_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

    player = new CFootballPlayer(this, playersVector->at(2), 3, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/player.lua"));
    player->getFSM()->setGlobalState("SPl_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

    player = new CFootballPlayer(this, playersVector->at(3), 4, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/player.lua"));
    player->getFSM()->setGlobalState("SPl_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

    player = new CFootballPlayer(this, playersVector->at(4), 5, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/player.lua"));
    player->getFSM()->setGlobalState("SPl_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

    player = new CFootballPlayer(this, playersVector->at(5), 6, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/player.lua"));
    player->getFSM()->setGlobalState("SPl_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

    player = new CFootballPlayer(this, playersVector->at(6), 7, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/player.lua"));
    player->getFSM()->setGlobalState("SPl_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

    player = new CFootballPlayer(this, playersVector->at(7), 8, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/player.lua"));
    player->getFSM()->setGlobalState("SPl_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

    player = new CFootballPlayer(this, playersVector->at(8), 9, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/player.lua"));
    player->getFSM()->setGlobalState("SPl_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

    player = new CFootballPlayer(this, playersVector->at(9), 10, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/player.lua"));
    player->getFSM()->setGlobalState("SPl_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

    player = new CFootballPlayer(this, playersVector->at(10), 11, team);
    CLuaManager::getInstance()->runScript(CONCAT_PATH(dataPath, "scripts/player.lua"));
    player->getFSM()->setGlobalState("SPl_Global");
    teamPlayers->push_back(player);
    m_simWorld->addObject(player);

	m_game.getIDAOFactory()->getIPfTeamPlayersDAO()->freeVector(playersVector);

	return teamPlayers;
}

CFormation* CSimulationManager::loadFormation(CPfFormations *formation)
{

	CFormation *teamFormation = new CFormation(formation->getSName().c_str());

	btVector3 point;
	btVector3 topLeft, bottomRight;
	CStrategicPosition *pos;
	std::vector<CPfStrategicPositions*> *positions = m_game.getIDAOFactory()->getIPfStrategicPositionsDAO()->findByXFkFormation(formation->getXFormation());

	std::vector<CPfStrategicPositions*>::iterator it;
	for(it = positions->begin(); it != positions->end(); it++) {
		pos = teamFormation->getPlayerStrategicPosition((*it)->getNLineupOrder() - 1);

		point.setValue((*it)->getNInitialPosX(),0,(*it)->getNInitialPosZ());
		pos->setInitialPosition(&point);
		point.setValue((*it)->getNDefensivePosX(),0,(*it)->getNDefensivePosZ());
		pos->setDefensivePosition(&point);
		point.setValue((*it)->getNOffensivePosX(),0,(*it)->getNOffensivePosZ());
		pos->setOffensivePosition(&point);

		topLeft.setValue((*it)->getNTopLeftAreaX(),0,(*it)->getNTopLeftAreaZ());
		bottomRight.setValue((*it)->getNBottomRightAreaX(),0,(*it)->getNBottomRightAreaZ());
		pos->setPlayingArea(&topLeft, &bottomRight);

		pos->setAttractionX((*it)->getNAttractionX());
		pos->setAttractionZ((*it)->getNAttractionZ());

	}
	//TODO make this in tactics?
	if(formation->getXFormation() == 1) {
		teamFormation->setKickOffPlayerId(10);
		teamFormation->setKickInPlayerId(5);
		teamFormation->setRightCornerKickPlayerId(6);
		teamFormation->setLeftCornerKickPlayerId(7);
		teamFormation->setRightTrowInPlayerId(6);
		teamFormation->setLeftTrowInPlayerId(7);
		teamFormation->setGoalKickPlayerId(0);
	} else {
		teamFormation->setKickOffPlayerId(8);
		teamFormation->setKickInPlayerId(5);
		teamFormation->setRightCornerKickPlayerId(6);
		teamFormation->setLeftCornerKickPlayerId(7);
		teamFormation->setRightTrowInPlayerId(6);
		teamFormation->setLeftTrowInPlayerId(7);
		teamFormation->setGoalKickPlayerId(0);
	}

	m_game.getIDAOFactory()->getIPfStrategicPositionsDAO()->freeVector(positions);

	return teamFormation;
}