// Copyright 2020 The Defold Foundation
// Licensed under the Defold License version 1.0 (the "License"); you may not use
// this file except in compliance with the License.
//
// You may obtain a copy of the License, together with FAQs at
// https://www.defold.com/license
//
// 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 <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <assert.h>
#include "shared_library.h"
#include "crypt.h"

#include <dlib/endian.h>
#include <mbedtls/md5.h>
#include <mbedtls/base64.h>
#include <mbedtls/sha1.h>
#include <mbedtls/sha256.h>
#include <mbedtls/sha512.h>
#include <mbedtls/pk.h>
#include <mbedtls/pk_internal.h>
#include <mbedtls/rsa.h>
#include <mbedtls/entropy.h>
#include <mbedtls/ctr_drbg.h>
//Added by dotGears
#include <mbedtls/md.h>

#include <dlib/log.h> // For debugging the manifest verification issue

namespace dmCrypt
{
    const uint32_t NUM_ROUNDS = 32;

    static inline uint64_t EncryptXTea(uint64_t v, uint32_t* key)
    {
        uint32_t v0 = (uint32_t) (v >> 32);
        uint32_t v1 = (uint32_t) (v & 0xffffffff);

        uint32_t sum = 0, delta = 0x9e3779b9;
        for (uint32_t i = 0; i < NUM_ROUNDS; i++) {
            v0 += (((v1 << 4) ^ (v1 >> 5)) + v1) ^ (sum + dmEndian::ToHost(key[sum & 3]));
            sum += delta;
            v1 += (((v0 << 4) ^ (v0 >> 5)) + v0) ^ (sum + dmEndian::ToHost(key[(sum>>11) & 3]));
        }
        uint64_t ret = dmEndian::ToHost((((uint64_t) v0) << 32 | v1));
        return ret;
    }

    static void EncryptXTeaCTR(uint8_t* data, uint32_t datalen, const uint8_t* key, uint32_t keylen)
    {
        assert(keylen <= 16);
        const uint32_t block_len = 8;
        uint8_t paddedkey[16] = {0};
        memcpy(paddedkey, key, keylen);

        uint64_t counter = 0;

        uint32_t i = 0;
        uint64_t* d = (uint64_t*) data;
        for (i = 0; i < datalen / block_len; i++) {
            uint64_t enc_counter = EncryptXTea(counter, (uint32_t*) paddedkey);
            d[i] ^= enc_counter;
            data += block_len;
            counter++;
        }

        uint64_t enc_counter = EncryptXTea(counter, (uint32_t*) paddedkey);
        uint32_t rest = datalen & (block_len - 1);
        uint8_t* ec = (uint8_t*) &enc_counter;
        for (uint32_t j = 0; j < rest; j++) {
            data[j] ^= ec[j];
        }
    }

    Result Encrypt(Algorithm algo, uint8_t* data, uint32_t datalen, const uint8_t* key, uint32_t keylen)
    {
        EncryptXTeaCTR(data, datalen, key, keylen);
        return RESULT_OK;
    }

    Result Decrypt(Algorithm algo, uint8_t* data, uint32_t datalen, const uint8_t* key, uint32_t keylen)
    {
        EncryptXTeaCTR(data, datalen, key, keylen);
        return RESULT_OK;
    }

    // Same as rsa_alt_decrypt_wrap() except with a MBEDTLS_RSA_PUBLIC
    static int rsa_alt_decrypt_public_wrap( void *ctx,
                        const unsigned char *input, size_t ilen,
                        unsigned char *output, size_t *olen, size_t osize,
                        int (*f_rng)(void *, unsigned char *, size_t), void *p_rng )
    {
        mbedtls_rsa_context * rsa = (mbedtls_rsa_context *) ctx;

        if( ilen != mbedtls_rsa_get_len( rsa ) )
            return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );

        return( mbedtls_rsa_pkcs1_decrypt( rsa, f_rng, p_rng,
                    MBEDTLS_RSA_PUBLIC, olen, input, output, osize ) );
    }

    Result Decrypt(const uint8_t* key, uint32_t keylen, const uint8_t* data, uint32_t datalen, uint8_t** output, uint32_t* outputlen)
    {
        // https://tls.mbed.org/discussions/generic/parsing-public-key-from-memory
        Result result = RESULT_OK;

        const char* pers = "defold_pk_decrypt";
        mbedtls_pk_context pk;
        mbedtls_entropy_context entropy;
        mbedtls_ctr_drbg_context ctr_drbg;
        mbedtls_pk_init(&pk);
        mbedtls_ctr_drbg_init( &ctr_drbg );
        mbedtls_entropy_init( &entropy );

        uint32_t signature_hash_len = MBEDTLS_MD_MAX_SIZE;

        int ret;
        if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func, &entropy, (const unsigned char *) pers, strlen(pers) ) ) != 0 )
        {
            dmLogError("Decrypt: mbedtls_ctr_drbg_seed failed: %d", ret);
            result = RESULT_ERROR;
            goto exit;
        }

        if ((ret = mbedtls_pk_parse_public_key(&pk, key, keylen) != 0))
        {
            dmLogError("Decrypt: mbedtls_pk_parse_public_key failed: %d", ret);
            result = RESULT_ERROR;
            goto exit;
        }

        *output = (uint8_t*)malloc(signature_hash_len);
        size_t _outputlen;
        if ((ret = rsa_alt_decrypt_public_wrap(pk.pk_ctx,
                    data, datalen,
                    (uint8_t*)*output, &_outputlen, signature_hash_len,
                    mbedtls_ctr_drbg_random, &ctr_drbg )) != 0)
        {
            dmLogError("Decrypt: rsa_alt_decrypt_public_wrap failed: %d", ret);
            free(*output);
            result = RESULT_ERROR;
            goto exit;
        }

        *outputlen = (uint32_t)_outputlen;

    exit:
        mbedtls_ctr_drbg_free( &ctr_drbg );
        mbedtls_entropy_free( &entropy );
        mbedtls_pk_free(&pk);
        return result;
    }

    void HashSha1(const uint8_t* buf, uint32_t buflen, uint8_t* digest)
    {
        mbedtls_sha1_context ctx;
        mbedtls_sha1_init(&ctx);
        mbedtls_sha1_starts_ret(&ctx);
        mbedtls_sha1_update_ret(&ctx, (const unsigned char*)buf, (size_t)buflen);
        int ret = mbedtls_sha1_finish_ret(&ctx, (unsigned char*)digest);
        mbedtls_sha1_free(&ctx);
        if (ret != 0) {
            memset(digest, 0, 20);
        }
    }

    void HashSha256(const uint8_t* buf, uint32_t buflen, uint8_t* digest)
    {
        int ret = mbedtls_sha256_ret((const unsigned char*)buf, (size_t)buflen, (unsigned char*)digest, 0);
        if (ret != 0) {
            memset(digest, 0, 20);
        }
    }

    void HashSha512(const uint8_t* buf, uint32_t buflen, uint8_t* digest)
    {
        int ret = mbedtls_sha512_ret((const unsigned char*)buf, (size_t)buflen, (unsigned char*)digest, 0);
        if (ret != 0) {
            memset(digest, 0, 20);
        }
    }

    void HashMd5(const uint8_t* buf, uint32_t buflen, uint8_t* digest)
    {
        int ret = mbedtls_md5_ret((const unsigned char*)buf, (size_t)buflen, (unsigned char*)digest);
        if (ret != 0) {
            memset(digest, 0, 20);
        }
    }

    bool Base64Encode(const uint8_t* src, uint32_t src_len, uint8_t* dst, uint32_t* dst_len)
    {
        size_t out_len = 0;
        int r = mbedtls_base64_encode(dst, *dst_len, &out_len, src, src_len);
        if (r != 0)
        {
            if (*dst_len == 0)
                *dst_len = (uint32_t)out_len; // Seems to return 1 more than necessary, but better to err on the safe side! (see test_crypt.cpp)
            else
                *dst_len = 0xFFFFFFFF;
            return false;
        }
        *dst_len = (uint32_t)out_len;
        return true;
    }

    bool Base64Decode(const uint8_t* src, uint32_t src_len, uint8_t* dst, uint32_t* dst_len)
    {
        size_t out_len = 0;
        int r = mbedtls_base64_decode(dst, *dst_len, &out_len, src, src_len);
        if (r != 0)
        {
            if (*dst_len == 0)
                *dst_len = (uint32_t)out_len;
            else
                *dst_len = 0xFFFFFFFF;
            return false;
        }
        *dst_len = (uint32_t)out_len;
        return true;
    }

    /* Added by dotGears/TrungB
     * This function will sign your content with given private key by RSA PKCS v15, and digest with SHA256.
     */
    unsigned char * RS256SignKey( unsigned char * signing_content, unsigned char * private_key )
    {
        int ret = 1;
        mbedtls_pk_context pk;
        mbedtls_entropy_context entropy;
        mbedtls_ctr_drbg_context ctr_drbg;

        mbedtls_pk_init( &pk );
        mbedtls_entropy_init( &entropy );
        mbedtls_ctr_drbg_init( &ctr_drbg );

        unsigned char hash[32];
        unsigned char buf[MBEDTLS_MPI_MAX_SIZE]; // As [1024]

        size_t  olen = 0,
                dlen = 344+1,
                buflen = 256;
        unsigned char dst[344+1];

        const char * pers = "rsa_sign_pss";
        size_t pkey_len = strlen((char*)private_key)+1;
        /*
         * Key Sign need a Random Generator Function, so here's one :
         */
        if(( ret = mbedtls_ctr_drbg_seed( &ctr_drbg,  mbedtls_entropy_func,  &entropy, (const unsigned char *) pers, strlen( pers ))) != 0 )
        {
             printf( "\ncrypt -- error: mbedtls_ctr_drbg_seed returned %d", ret );
             goto exit;
        }
        /*
         * Parse private key, wonder why pkey_len had to +1 ?
         */
        if(( ret = mbedtls_pk_parse_key( &pk, private_key, pkey_len, NULL, NULL)) != 0)
        {
            printf( "  ! mbedtls_pk_parse_public_keyfile returned %d", ret );
            goto exit;
        }
        /*
         * Check for valid RSA key.
         */
        if( !mbedtls_pk_can_do( &pk, MBEDTLS_PK_RSA ))
        {
            printf( "\ncrypt -- error: Key is not an RSA key: %s\n", private_key);
            goto exit;
        }
        /*
         * Important: MBEDTLS_RSA_PKCS_V21 won't work for Google OAuth v2, but V15.
         */
        mbedtls_rsa_set_padding( mbedtls_pk_rsa(pk), MBEDTLS_RSA_PKCS_V15, MBEDTLS_MD_SHA256 );
        /*
         * Compute the SHA-256 hash of the input file.
         */
        if(( ret = mbedtls_md(mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), signing_content, strlen((char*)signing_content), hash)) != 0 )
        {
            printf( "\ncrypt -- mbedtls_md_info_from_type\n" );
            goto exit;
        }
        /*
         * then calculate the RSA signature of the hash.
         */
        if(( ret = mbedtls_pk_sign(&pk, MBEDTLS_MD_SHA256, hash, 0, buf, &olen, mbedtls_ctr_drbg_random, &ctr_drbg)) != 0 )
        {
            printf( "\ncrypt -- error: mbedtls_pk_sign returned %d\n", ret );
            goto exit;
        }
        /*
         * encode given signature > base64
         */
        mbedtls_base64_encode(dst, dlen, &olen, buf, buflen);

    exit:
        /*
         * free resources.
         */
        mbedtls_ctr_drbg_free( &ctr_drbg );
        mbedtls_entropy_free( &entropy );
        mbedtls_pk_free(&pk);

        return dst;
    }
}

extern "C" {
    DM_DLLEXPORT int EncryptXTeaCTR(uint8_t* data, uint32_t datalen, const uint8_t* key, uint32_t keylen)
    {
        return dmCrypt::Encrypt(dmCrypt::ALGORITHM_XTEA, data, datalen, key, keylen);
    }

    DM_DLLEXPORT int DecryptXTeaCTR(uint8_t* data, uint32_t datalen, const uint8_t* key, uint32_t keylen)
    {
        return dmCrypt::Decrypt(dmCrypt::ALGORITHM_XTEA, data, datalen, key, keylen);
    }

}


#include <veil/budget.h>
#include <test/test_veil.h>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(monthly_rewards_tests, BasicTestingSetup)

/**
 *  Tests to make suret that the proper rewards are sent with the given block height
 */

BOOST_AUTO_TEST_CASE(testRewardAfterSet)
{
    CAmount nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment = 0;
    int nBlocksPerPeriod = 43200;
    veil::Budget().GetBlockRewards(1, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 50 * COIN);
    BOOST_CHECK(nFounderPayment == 10 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nLabPayment == 10 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nBudgetPayment == 30 * nBlocksPerPeriod * COIN);

    veil::Budget().GetBlockRewards(2, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 50 * COIN);
    BOOST_CHECK(nFounderPayment == 0);
    BOOST_CHECK(nLabPayment == 0);
    BOOST_CHECK(nBudgetPayment == 0);

    veil::Budget().GetBlockRewards(nBlocksPerPeriod, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 50 * COIN);
    BOOST_CHECK(nFounderPayment == 10 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nLabPayment == 10 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nBudgetPayment == 30 * nBlocksPerPeriod * COIN);

    veil::Budget().GetBlockRewards(nBlocksPerPeriod + 1, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 50 * COIN);
    BOOST_CHECK(nFounderPayment == 0);
    BOOST_CHECK(nLabPayment == 0);
    BOOST_CHECK(nBudgetPayment == 0);

    veil::Budget().GetBlockRewards(12 * nBlocksPerPeriod, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 40 * COIN);
    BOOST_CHECK(nFounderPayment == 8 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nLabPayment == 8 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nBudgetPayment == 24 * nBlocksPerPeriod * COIN);

    veil::Budget().GetBlockRewards(12 * nBlocksPerPeriod + 1, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 40 * COIN);
    BOOST_CHECK(nFounderPayment == 0);
    BOOST_CHECK(nLabPayment == 0);
    BOOST_CHECK(nBudgetPayment == 0);

    veil::Budget().GetBlockRewards(24 * nBlocksPerPeriod, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 30 * COIN);
    BOOST_CHECK(nFounderPayment == 6 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nLabPayment == 6 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nBudgetPayment == 18 * nBlocksPerPeriod * COIN);

    veil::Budget().GetBlockRewards(24 * nBlocksPerPeriod + 1, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 30 * COIN);
    BOOST_CHECK(nFounderPayment == 0);
    BOOST_CHECK(nLabPayment == 0);
    BOOST_CHECK(nBudgetPayment == 0);

    veil::Budget().GetBlockRewards(36 * nBlocksPerPeriod, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 20 * COIN);
    BOOST_CHECK(nFounderPayment == 4 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nLabPayment == 4 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nBudgetPayment == 12 * nBlocksPerPeriod * COIN);

    veil::Budget().GetBlockRewards(36 * nBlocksPerPeriod + 1, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 20 * COIN);
    BOOST_CHECK(nFounderPayment == 0);
    BOOST_CHECK(nLabPayment == 0);
    BOOST_CHECK(nBudgetPayment == 0);

    veil::Budget().GetBlockRewards(48 * nBlocksPerPeriod, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 10 * COIN);
    BOOST_CHECK(nFounderPayment == 2 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nLabPayment == 2 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nBudgetPayment == 6 * nBlocksPerPeriod * COIN);

    veil::Budget().GetBlockRewards(48 * nBlocksPerPeriod + 1, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 10 * COIN);
    BOOST_CHECK(nFounderPayment == 0);
    BOOST_CHECK(nLabPayment == 0);
    BOOST_CHECK(nBudgetPayment == 0);

    veil::Budget().GetBlockRewards(60 * nBlocksPerPeriod, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 10 * COIN);
    BOOST_CHECK(nFounderPayment == 0);
    BOOST_CHECK(nLabPayment == 2 * nBlocksPerPeriod * COIN);
    BOOST_CHECK(nBudgetPayment == 8 * nBlocksPerPeriod * COIN);

    veil::Budget().GetBlockRewards(60 * nBlocksPerPeriod + 1, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);

    BOOST_CHECK(nBlockReward == 10 * COIN);
    BOOST_CHECK(nFounderPayment == 0);
    BOOST_CHECK(nLabPayment == 0);
    BOOST_CHECK(nBudgetPayment == 0);

}

/**
 * Tests to make sure that in a 12 month period, 12 rewards have been
 * given
 */
BOOST_AUTO_TEST_CASE(countRwards)
{

    CAmount nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment = 0;
    int nBlockRewardsCount = 0;
    int nRewardsCount = 0;

    for(int i = 0; i <= 518399; i++) {
        veil::Budget().GetBlockRewards(i, nBlockReward, nFounderPayment, nLabPayment, nBudgetPayment);
        if(nFounderPayment > 0)
            nRewardsCount++;
        if(nBlockReward > 0)
            nBlockRewardsCount++;
    }

    BOOST_CHECK(nRewardsCount == 12);
    BOOST_CHECK(nBlockRewardsCount == 518399);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Interoperability helpers connecting internal math types and Eigen.
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_math
 */

#pragma once

#ifndef __cplusplus
#error "This header only usable from C++"
#endif

#include "math/m_api.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

/*!
 * @brief Wrap an internal quaternion struct in an Eigen type, const overload.
 *
 * Permits zero-overhead manipulation of `const xrt_quat&` by Eigen routines as
 * if it were a `const Eigen::Quaternionf&`.
 */
static inline Eigen::Map<const Eigen::Quaternionf>
map_quat(const struct xrt_quat &q)
{
	return Eigen::Map<const Eigen::Quaternionf>{&q.x};
}

/*!
 * @brief Wrap an internal quaternion struct in an Eigen type, non-const
 * overload.
 *
 * Permits zero-overhead manipulation of `xrt_quat&` by Eigen routines as if it
 * were a `Eigen::Quaternionf&`.
 */
static inline Eigen::Map<Eigen::Quaternionf>
map_quat(struct xrt_quat &q)
{
	return Eigen::Map<Eigen::Quaternionf>{&q.x};
}

/*!
 * @brief Wrap an internal 3D vector struct in an Eigen type, const overload.
 *
 * Permits zero-overhead manipulation of `const xrt_vec3&` by Eigen routines as
 * if it were a `const Eigen::Vector3f&`.
 */
static inline Eigen::Map<const Eigen::Vector3f>
map_vec3(const struct xrt_vec3 &v)
{
	return Eigen::Map<const Eigen::Vector3f>{&v.x};
}

/*!
 * @brief Wrap an internal 3D vector struct in an Eigen type, non-const
 * overload.
 *
 * Permits zero-overhead manipulation of `xrt_vec3&` by Eigen routines as
 * if it were a `Eigen::Vector3f&`.
 */
static inline Eigen::Map<Eigen::Vector3f>
map_vec3(struct xrt_vec3 &v)
{
	return Eigen::Map<Eigen::Vector3f>{&v.x};
}

/*!
 * @brief Wrap an internal 4x4 matrix struct in an Eigen type, non-const
 * overload.
 *
 * Permits zero-overhead manipulation of `xrt_matrix_4x4&` by Eigen routines as
 * if it were a `Eigen::Matrix4f&`.
 */
static inline Eigen::Map<Eigen::Matrix4f>
map_matrix_4x4(struct xrt_matrix_4x4 &m)
{
	return Eigen::Map<Eigen::Matrix4f>(m.v);
}

/*
 *
 * Pose deconstruction helpers.
 *
 */

/*!
 * Return a Eigen type wrapping a pose's orientation (const).
 */
static inline Eigen::Map<const Eigen::Quaternionf>
orientation(const struct xrt_pose &pose)
{
	return map_quat(pose.orientation);
}

/*!
 * Return a Eigen type wrapping a pose's orientation.
 */
static inline Eigen::Map<Eigen::Quaternionf>
orientation(struct xrt_pose &pose)
{
	return map_quat(pose.orientation);
}

/*!
 * Return a Eigen type wrapping a pose's position (const).
 */
static inline Eigen::Map<const Eigen::Vector3f>
position(const struct xrt_pose &pose)
{
	return map_vec3(pose.position);
}

/*!
 * Return a Eigen type wrapping a pose's position.
 */
static inline Eigen::Map<Eigen::Vector3f>
position(struct xrt_pose &pose)
{
	return map_vec3(pose.position);
}


#pragma once

#include <map>
#include <vector>
#include <iostream>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <Eigen/Dense>
#include <DemBones/DemBonesExt.h>
#include <DemBones/MatBlocks.h>
#include <Python.h>
#include "utils.h"

namespace py = pybind11;
using namespace std;
using namespace Eigen;
using namespace Dem;

class DemBonesModel : public DemBonesExt<double, float> {
public:
	int sF = 1001;
	int eF = 1010;
	vector<string> bonesMaya;
	vector<double> weightsMaya;
	map<string, MMatrix> bindMatricesMaya;
	map<string, map<int, MMatrix>> animMatricesMaya;
	map<string, MTransformationMatrix::RotationOrder> rotOrderMaya;

	double tolerance;
	int patience;

	DemBonesModel() : tolerance(1e-3), patience(3) { nIters = 30; clear(); }

	void cbIterBegin() {
		LOG("  iteration #" << iter << endl);
	}

	bool cbIterEnd() {
		double err = rmse();
		LOG("    rmse = " << err << endl);
		if ((err<prevErr*(1 + weightEps)) && ((prevErr - err)<tolerance*prevErr)) {
			np--;
			if (np == 0) {
				LOG("  convergence is reached" << endl);
				return true;
			}
		}
		else np = patience;
		prevErr = err;
		return false;
	}

	void cbInitSplitBegin() {}

	void cbInitSplitEnd() {}

	void cbWeightsBegin() {}

	void cbWeightsEnd() { LOG("    updated weights..." << endl);}

	void cbTranformationsBegin() {}

	void cbTransformationsEnd() { LOG("    updated transforms..." << endl); }

	bool cbTransformationsIterEnd() { return false; }

	bool cbWeightsIterEnd() { return false; }

	void extractSource(MDagPath& dag, MFnMesh& mesh) {
		MatrixXd wd(0, 0);
		MIntArray indices;
		MDoubleArray weights;
		MDagPath boneParentMaya;
		MDagPathArray bonesMaya;
		map<string, MatrixXd, less<string>, aligned_allocator<pair<const string, MatrixXd>>> mT;
		map<string, VectorXd, less<string>, aligned_allocator<pair<const string, VectorXd>>> wT;
		map<string, Matrix4d, less<string>, aligned_allocator<pair<const string, Matrix4d>>> bindMatrices;
		bool hasKeyFrame = false;

		time.setValue(sF);
		anim.setCurrentTime(time);

		// update model: bind vertex positions
		status = mesh.getPoints(points, MSpace::kWorld);
		CHECK_MSTATUS_AND_THROW(status);

		u.resize(3, nV);

		#pragma omp parallel for
		for (int i = 0; i < nV; i++) {
			MPoint point = points[i];
			u.col(i) << point.x, point.y, point.z;
		}

		// update model: face connections
		int nFV = mesh.numPolygons();
		fv.resize(nFV);

		for (int i = 0; i < nFV; i++) {
			mesh.getPolygonVertices(i, indices);
			int length = indices.length();
			for (int j = 0; j < length; j++)
				fv[i].push_back((int)indices[j]);
		}

		// update model: weights and skeleton
		MItDependencyGraph graphIter(dag.node(), MFn::kSkinClusterFilter, MItDependencyGraph::kUpstream);
		MObject rootNode = graphIter.currentItem(&status);

		if (MS::kSuccess == status) {
			// query bones
			MFnSkinCluster skinCluster(rootNode);
			nB = skinCluster.influenceObjects(bonesMaya, &status);
			CHECK_MSTATUS_AND_THROW(status);

			// get bones names
			boneName.resize(nB);
			for (int j = 0; j < nB; j++) {
				string name = bonesMaya[j].partialPathName().asUTF8();
				boneName[j] = name;
				boneIndex[name] = j;
			}

			// get bone weights
			for (int j = 0; j < nB; j++) {
				status = skinCluster.getWeights(dag, MObject::kNullObj, j, weights);
				CHECK_MSTATUS_AND_THROW(status);

				wT[boneName[j]] = VectorXd::Zero(nV);
				for (int k = 0; k < nV; k++)
					wT[boneName[j]](k) = weights[k];
			}
		}

		parent.resize(nB);
		bind.resize(nS * 4, nB * 4);
		preMulInv.resize(nS * 4, nB * 4);
		rotOrder.resize(nS * 3, nB);
		orient.resize(nS * 3, nB);
		lockM.resize(nB);
		lockW = VectorXd::Zero(nV);

		// update model: weights
		if (wT.size() != 0) {
			wd = MatrixXd::Zero(nB, nV);
			for (int j = 0; j < nB; j++)
				wd.row(j) = wT[boneName[j]].transpose();
		}

		// update model: weights locked
		MString colourSet = "demLock";
		if (mesh.hasColorChannels(colourSet)) {
			MColorArray colours;
			status = mesh.getVertexColors(colours, &colourSet);
			CHECK_MSTATUS_AND_THROW(status);

			for (int c = 0; c < (int)colours.length(); c++)
				lockW(c) = Conversion::toGrayScale(colours[c]);
		}

		// update model: skeleton
		for (int j = 0; j < nB; j++) {
			// get name
			string name = boneName[j];

			// get parent
			MObject boneObj = bonesMaya[j].node();
			MFnDagNode boneDagFn(boneObj, &status);
			CHECK_MSTATUS_AND_THROW(status);
			MObject boneParentObj = boneDagFn.parent(0);

			if (!boneParentObj.isNull() && boneParentObj.hasFn(MFn::kJoint)) {
				status = MDagPath::getAPathTo(boneParentObj, boneParentMaya);
				CHECK_MSTATUS_AND_THROW(status);

				string parentName = boneParentMaya.partialPathName().asUTF8();
				if (boneIndex.find(parentName) == boneIndex.end())
					parent(j) = -1;
				else
					parent(j) = boneIndex[parentName];
			}
			else {
				parent(j) = -1;
			}

			// get bind matrix
			mT[name].resize(nF * 4, nB * 4);
			Matrix4d bindMatrix = Conversion::toMatrix4D(bonesMaya[j].inclusiveMatrix());
			bind.blk4(0, j) = bindMatrix;
			bindMatrices[name] = bindMatrix;

			// get rotation order
			MPlug rotateOrderPlug = boneDagFn.findPlug("rotateOrder", &status);
			CHECK_MSTATUS_AND_THROW(status);

			int rotateOrder = rotateOrderPlug.asInt();
			switch (rotateOrder) {
				case 0: {
					rotOrder.vec3(0, j) = Vector3i(0, 1, 2);
					rotOrderMaya[name] = MTransformationMatrix::RotationOrder::kXYZ;
					break;
				}
				case 1: {
					rotOrder.vec3(0, j) = Vector3i(1, 2, 0);
					rotOrderMaya[name] = MTransformationMatrix::RotationOrder::kYZX;
					break;
				}
				case 2: {
					rotOrder.vec3(0, j) = Vector3i(2, 0, 1);
					rotOrderMaya[name] = MTransformationMatrix::RotationOrder::kZXY;
					break;
				}
				case 3: {
					rotOrder.vec3(0, j) = Vector3i(0, 2, 1);
					rotOrderMaya[name] = MTransformationMatrix::RotationOrder::kXZY;
					break;
				}
				case 4: {
					rotOrder.vec3(0, j) = Vector3i(1, 0, 2);
					rotOrderMaya[name] = MTransformationMatrix::RotationOrder::kYXZ;
					break;
				}
				case 5: {
					rotOrder.vec3(0, j) = Vector3i(2, 1, 0);
					rotOrderMaya[name] = MTransformationMatrix::RotationOrder::kZYX;
					break;
				}
			}

			// get joint orient
			MPlug jointOrientPlug = boneDagFn.findPlug("jointOrient", &status);
			CHECK_MSTATUS_AND_THROW(status);

			double jointOrientX = jointOrientPlug.child(0).asMAngle().asDegrees();
			double jointOrientY = jointOrientPlug.child(1).asMAngle().asDegrees();
			double jointOrientZ = jointOrientPlug.child(2).asMAngle().asDegrees();
			orient.vec3(0, j) = Vector3d(jointOrientX, jointOrientY, jointOrientZ);

			// get pre multiply inverse
			if (!boneParentObj.isNull() && parent(j) == -1) {
				status = MDagPath::getAPathTo(boneParentObj, boneParentMaya);
				CHECK_MSTATUS_AND_THROW(status);

				Matrix4d gp = Conversion::toMatrix4D(boneParentMaya.inclusiveMatrix());
				preMulInv.blk4(0, j) = gp.inverse();
			}
			else {
				preMulInv.blk4(0, j) = Matrix4d::Identity();
			}

			// get dem lock
			MPlug demLockPlug = boneDagFn.findPlug("demLock", &status);
			if (MS::kSuccess == status) lockM(j) = demLockPlug.asInt(); else lockM(j) = 0;
		}

		// update model: skeleton animation
		m.resize(nF * 4, nB * 4);
		for (int k = sF; k < eF + 1; k++) {
			time.setValue(k);
			anim.setCurrentTime(time);
			int num = k - sF;

			for (int j = 0; j < nB; j++) {
				// get name
				string name = boneName[j];

				// set matrix
				Matrix4d matrix = Conversion::toMatrix4D(bonesMaya[j].inclusiveMatrix());
				mT[name].blk4(num, 0) = matrix * bindMatrices[name].inverse();
			}
		}
		for (int j = 0; j < nB; j++) {
			m.block(0, j * 4, nF * 4, 4) = mT[boneName[j]];
		}

		// update model: animation state
		MString transformAttributes[9] = { "tx", "ty", "tz", "rx", "ry", "rz", "sx", "sy", "sz" };
		for (int j = 0; j < nB; j++) {
			string nj = boneName[j];

			for (int k = 0; k < 9; k++) {
				MFnDependencyNode node(bonesMaya[j].node(), &status);
				CHECK_MSTATUS_AND_THROW(status);

				MPlug plug = node.findPlug(transformAttributes[k], &status);
				CHECK_MSTATUS_AND_THROW(status);

				if (plug.isDestination()) {
					hasKeyFrame = true;
					break;
				}
			}
		}

		w = (wd / nS).sparseView(1, 1e-20);
		lockW = lockW /= (double)nS;
		if (!hasKeyFrame) m.resize(0, 0);

		// report
		LOG("extracted source" << endl);
		LOG("  " << nV << " vertices" << endl);
		if (nB != 0) LOG("  " << nB << " joints" << endl);
		if (hasKeyFrame) LOG("  keyframes found" << endl);
		if (w.size() != 0) LOG("  skinning found" << endl);
	}

	void extractTarget(MFnMesh& mesh) {
		// update model: animated vertex positions
		for (int k = sF; k < eF + 1; k++) {
			time.setValue(k);
			anim.setCurrentTime(time);

			int num = k - sF;
			fTime(num) = time.asUnits(MTime::kSeconds);
			status = mesh.getPoints(points, MSpace::kWorld);
			CHECK_MSTATUS_AND_THROW(status);

			#pragma omp parallel for
			for (int i = 0; i < nV; i++) {
				MPoint point = points[i];
				v.col(i).segment<3>(num * 3) << (float)point.x, (float)point.y, (float)point.z;
			}
		}

		// update model: subject ids
		subjectID.resize(nF);
		for (int s = 0; s < nS; s++)
			for (int k = fStart(s); k < fStart(s + 1); k++)
				subjectID(k) = s;

		LOG("extracted target" << endl);
	}

	void compute(string& source, string& target, int& startFrame, int& endFrame) {
		// log parameters
		LOG("parameters" << endl);
		LOG("  source                   = " << source << endl);
		LOG("  target                   = " << target << endl);
		LOG("  start_frame              = " << startFrame << endl);
		LOG("  end_frame                = " << endFrame << endl);
		LOG("  num_iterations           = " << nIters << endl);
		LOG("  patience                 = " << patience << endl);
		LOG("  tolerance                = " << tolerance << endl);
		LOG("  num_transform_iterations = " << nTransIters << endl);
		LOG("  num_weight_iterations    = " << nWeightsIters << endl);
		LOG("  translation_affine       = " << transAffine << endl);
		LOG("  translation_affine_norm  = " << transAffineNorm << endl);
		LOG("  max_influences           = " << nnz << endl);
		LOG("  weights_smooth           = " << weightsSmooth << endl);
		LOG("  weights_smooth_step      = " << weightsSmoothStep << endl);
		LOG("  weights_epsilon          = " << weightEps << endl);

		// variables
		prevErr = -1;
		np = patience;

		// get geometry
		MDagPath sourcePath = Conversion::toMDagPath(source, true);
		MDagPath targetPath = Conversion::toMDagPath(target, true);

		MFnMesh sourceMeshFn(sourcePath, &status);
		CHECK_MSTATUS_AND_THROW(status);
		MFnMesh targetMeshFn(targetPath, &status);
		CHECK_MSTATUS_AND_THROW(status);

		// update model
		nS = 1;
		sF = startFrame;
		eF = endFrame;
		nF = endFrame - startFrame + 1;
		nV = sourceMeshFn.numVertices();
		int cF = (int)anim.currentTime().value();

		if (sF >= eF)
			throw std::exception("Start frame is not allowed to be equal or larger than the end frame.");
		if (nV != sourceMeshFn.numVertices())
			throw std::exception("Vertex count between source and target do not match.");

		v.resize(3 * nF, nV);
		fTime.resize(nF);
		fStart.resize(nS + 1);
		fStart(0) = 0;
		fStart(1) = nF;

		// update model: source + target
		extractTarget(targetMeshFn);
		extractSource(sourcePath, sourceMeshFn);

		// initialize model
		if (nB == 0)
			throw std::exception("No influences found.");

		// compute model
		LOG("computing" << endl);
		DemBonesExt<double, float>::compute();

		// compute transformations + weights
		VectorXd tVal, rVal;
		MatrixXd lr, lt, gb, lbr, lbt;
		computeRTB(0, lr, lt, gb, lbr, lbt, false);

		for (int j = 0; j < nB; j++) {
			string name = boneName[j];
			bonesMaya.push_back(name);
			MVector translate = MVector(lbt(0, j), lbt(1, j), lbt(2, j));
			MVector rotate = MVector(lbr(0, j), lbr(1, j), lbr(2, j));
			bindMatricesMaya[name] = Conversion::toMMatrix(translate, rotate, rotOrderMaya[name]);

			tVal = lt.col(j);
			rVal = lr.col(j);

			for (int k = sF; k < eF + 1; k++) {
				int num = k - sF;
				MVector translate = MVector(tVal(num * 3), tVal((num * 3) + 1), tVal((num * 3) + 2));
				MVector rotate = MVector(rVal(num * 3), rVal((num * 3) + 1), rVal((num * 3) + 2));
				animMatricesMaya[name][k] = Conversion::toMMatrix(translate, rotate, rotOrderMaya[name]);
			}
		}

		weightsMaya.resize(nB * nV);
		Eigen::SparseMatrix<double> wT = w.transpose();
		for (int j = 0; j < nB; j++)
			for (Eigen::SparseMatrix<double>::InnerIterator it(wT, j); it; ++it)
				weightsMaya[((int)it.row() * nB) + j] = it.value();

		time.setValue(cF);
		anim.setCurrentTime(time);
	}

	array<double, 16> bindMatrix(string& bone) {
		if (bindMatricesMaya.find(bone) == bindMatricesMaya.end())
			throw std::exception("Provided influence is not valid.");

		return Conversion::toMatrixArray(bindMatricesMaya[bone]);
	}

	array<double, 16> animMatrix(string& bone, int& frame) {
		if (animMatricesMaya.find(bone) == animMatricesMaya.end())
			throw std::exception("Provided bone is not valid.");
		if (animMatricesMaya[bone].find(frame) == animMatricesMaya[bone].end())
			throw std::exception("Provided frame is not valid.");

		return Conversion::toMatrixArray(animMatricesMaya[bone][frame]);
	}

	void clear() {
		DemBonesExt<double, float>::clear();
		points.clear();
		boneIndex.clear();
	}

private:
	double prevErr;
	int np;

	MStatus status;
	MTime time;
	MAnimControl anim;
	MPointArray points;
	map<string, int> boneIndex;
} model;

PYBIND11_MODULE(_dem_bones, m) {
	py::class_<DemBonesModel>(m, "DemBones")
		.def(py::init<>())
		.def_readwrite("num_iterations", &DemBonesModel::nIters, "Number of global iterations, default = 30")
		.def_readwrite("num_transform_iterations", &DemBonesModel::nTransIters, "Number of bone transformations update iterations per global iteration, default = 5")
		.def_readwrite("translation_affine", &DemBonesModel::transAffine, "Translations affinity soft constraint, default = 10.0")
		.def_readwrite("translation_affine_norm", &DemBonesModel::transAffineNorm, "p-norm for bone translations affinity soft constraint, default = 4.0")
		.def_readwrite("num_weight_iterations", &DemBonesModel::nWeightsIters, "Number of weights update iterations per global iteration, default = 3")
		.def_readwrite("max_influences", &DemBonesModel::nnz, "Number of non-zero weights per vertex, default = 8")
		.def_readwrite("weights_smooth", &DemBonesModel::weightsSmooth, "Weights smoothness soft constraint, default = 1e-4")
		.def_readwrite("weights_smooth_step", &DemBonesModel::weightsSmoothStep, "Step size for the weights smoothness soft constraint, default = 1.0")
		.def_readwrite("weights_epsilon", &DemBonesModel::weightEps, "Epsilon for weights solver, default = 1e-15")
		.def_readonly("start_frame", &DemBonesModel::sF, "Start frame of solver")
		.def_readonly("end_frame", &DemBonesModel::eF, "End frame of solver")
		.def_readonly("influences", &DemBonesModel::bonesMaya, "List of all influences")
		.def_readonly("weights", &DemBonesModel::weightsMaya, "List of weights for all influences and vertices")
		.def("rmse", &DemBonesModel::rmse, "Root mean squared reconstruction error")
		.def("compute", &DemBonesModel::compute, "Skinning decomposition of alternative updating weights and bone transformations", py::arg("source"), py::arg("target"), py::arg("start_frame"), py::arg("end_frame"))
		.def("bind_matrix", &DemBonesModel::bindMatrix, "Get the bind matrix for the provided influence", py::arg("influence"))
		.def("anim_matrix", &DemBonesModel::animMatrix, "Get the animation matrix for the provided influence at the provided frame", py::arg("influence"), py::arg("frame"));
}


//  (C) Copyright John Maddock 2017.

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

#ifndef BOOST_MATH_COMMON_FACTOR_RT_HPP
#define BOOST_MATH_COMMON_FACTOR_RT_HPP

#include <boost/integer/common_factor_rt.hpp>
#include <boost/config/header_deprecated.hpp>

BOOST_HEADER_DEPRECATED("<boost/integer/common_factor_rt.hpp>");

namespace boost {
   namespace math {
      using boost::integer::gcd;
      using boost::integer::lcm;
      using boost::integer::gcd_range;
      using boost::integer::lcm_range;
      using boost::integer::gcd_evaluator;
      using boost::integer::lcm_evaluator;
   }
}

#endif  // BOOST_MATH_COMMON_FACTOR_RT_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_IEEE_FUNCTIONS_SCALAR_FAST_LDEXP_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_FAST_LDEXP_HPP_INCLUDED

#include <boost/simd/ieee/functions/fast_ldexp.hpp>
#include <boost/simd/include/functions/scalar/shift_left.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( fast_ldexp_, tag::cpu_
                                    , (A0)(A1)
                                    , (scalar_< integer_<A0> >)(scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(2)
    {
       return (a1>=0)?(a0<<a1):(a0>>a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( fast_ldexp_, tag::cpu_
                                    , (A0)(A1)
                                    , (scalar_< floating_<A0> >)(scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename meta::scalar_of<result_type>::type            sA0;
      typedef typename dispatch::meta::as_integer<A0>::type          iA0;
      iA0 ik =  a1+Maxexponent<A0>();
      ik = shl(ik, Nbmantissabits<sA0>());
      return a0*bitwise_cast<A0>(ik);
    }
  };
} } }

#endif


/**
 * @file DynamicsOptimizer.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-08
 */

#pragma once

#include <string>
#include <vector>
#include <iostream>
#include <Eigen/Eigen>

#include <solver/interface/Solver.hpp>
#include <momentumopt/utilities/Clock.hpp>
#include <momentumopt/setting/PlannerSetting.hpp>
#include <momentumopt/kinopt/KinematicsState.hpp>
#include <momentumopt/cntopt/ContactPlanInterface.hpp>

namespace momentumopt {

  /**
   * Helper class to define a linear approximation of a friction cone.
   */
  struct FrictionCone
  {
    typedef Eigen::Matrix< double, 4, 3> ConeMat;
    void getCone(double fcoeff, FrictionCone::ConeMat& cone_mat)
    {
      if (fcoeff <= 0.001) { fcoeff = 0.001; }
      Eigen::Vector3d fconevec = Eigen::Vector3d(0.5*sqrt(2.0), 0.5*sqrt(2.0), -fcoeff); fconevec.normalize();
      Eigen::Matrix3d fconemat = Eigen::Matrix3d::Identity();
      double angle = 2*M_PI/4;
      fconemat(0,0) = cos(angle);    fconemat(0,1) = -sin(angle);
      fconemat(1,0) = sin(angle);    fconemat(1,1) =  cos(angle);
      for (int i=0; i<4; i++) {
        cone_mat.row(i) = fconevec;
        fconevec = fconemat * fconevec;
      }
    }
  };

  /*! main class of the dynamics optimization problem  */
  class DynamicsOptimizer
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
      /*! default class constructor and destructor */
	  DynamicsOptimizer(){}
      ~DynamicsOptimizer(){}

      /**
       * function to initialize and configure the optimization
       * @param[in]  PlannerSetting               setting of planner configuration variables
       */
      void initialize(PlannerSetting& planner_setting);

      /**
       * function to parse equations and objective into optimization problem and attempt to find a solution
       * @param[in]  ini_state                    initial state of the robot
       * @param[in]  contact_plan                 container of contact sequence to be used for dynamics planning
       * @param[in]  kin_sequence                 kinematics sequence to be used as momentum tracking reference (can be zeros).
       * @param[in]  update_tracking_objective    changes weights from regulation to tracking of momentum in ref_sequence
       * @return     ExitCode                     flag that indicates the optimization result (for example: optimal, infeasible)
       */
      solver::ExitCode optimize(const DynamicsState& ini_state, ContactPlanInterface* contact_plan,
                                const KinematicsSequence& kin_sequence, bool update_tracking_objective = false);

      /**
       * this function gives access to the optimized motion plan
       * @return     DynamicsSequence             reference to dynamics sequence, which is a collection of dynamic states,
       *                                          each of which contains information about all variables, all end-effectors
       *                                          of the motion plan for one time step
       */
      DynamicsSequence& dynamicsSequence() { return dyn_sequence_; }
      const DynamicsSequence& dynamicsSequence() const { return dyn_sequence_; }

      /*! function to have access to time required to solve the optimization problem */
      const double& solveTime() const { return solve_time_; }

    private:
      /*! Getter and setter methods for getting the planner variables  */
      inline PlannerSetting& getSetting() { return *planner_setting_; }
      inline const PlannerSetting& getSetting() const { return *planner_setting_; }

      /*! helper functions for the optimization problem */
      const ContactType& contactType(int time_id, int eff_id) const;
      Eigen::Matrix3d contactRotation(int time_id, int eff_id) const;
      double contactLocation(int time_id, int eff_id, int axis_id) const;

      /**
       * function to initialize optimization variables: type [continuous or binary],
       * guess value [if any], upper and lower bounds for the variable
       */
      void initializeOptimizationVariables();

      /**
       * function to add each variable to the Model and assign a unique identifier
       * to it, to be used by the optimizer to construct the problem
       * @param[in]  opt_var                      helper optimization variable for model predictive control
       * @param[in]  model                        instance of solver interface to collect constraints, objective and solve problem
       * @param[in]  vars                         vector of optimization variables
       */
      void addVariableToModel(const solver::OptimizationVariable& opt_var, solver::Model& model, std::vector<solver::Var>& vars);

      /**
       * functions to update tracking objective for momentum from penalty to tracking
       * and attempt to find a solution to the optimization problem
       * @param[in]  ref_sequence                 dynamics sequence to be used as momentum tracking reference (can be zeros).
       * @param[in]  is_first_time                flag to indicate if this is the first time the solution is being constructed
       */
      void internalOptimize(const KinematicsSequence& kin_sequence, bool is_first_time = false);
      void updateTrackingObjective();

      /**
       * functions that transfers optimal solution from model to helper class OptimizationVariable,
       * from helper class OptimizationVariable to dynamics sequence to be accessed by the user, and
       * from dynamics sequence to a file.
       * @param[in]  opt_var                      helper optimization variable for model predictive control
       * @param[in]  ref_sequence                 dynamics sequence to be used as momentum tracking reference (can be zeros).
       */
      void saveSolution(solver::OptimizationVariable& opt_var);
      void storeSolution();
      void saveToFile(const KinematicsSequence& kin_sequence);

    private:
      /*! Variables required for optimization */
      PlannerSetting* planner_setting_;

      /**
       * class that stores all information about the problem, interfaces between high level definition
       * of the problem and its corresponding mathematical construction to solve it with a solver instance
       */
      solver::Model model_;

      /*! helper variables to construct linear and quadratic expressions */
      solver::LinExpr lin_cons_;
      solver::DCPQuadExpr quad_objective_, quad_cons_;

      /*! exit code of the optimization problem */
      solver::ExitCode exitcode_;

      /**
       * c++ vector containing all problem variables defined by the user. Does not include extra
       * variables required to write the problem in standard conic form.
       */
      std::vector<solver::Var> vars_;

      /**
       * initial configuration of the robot, including center of mass position, linear and angular momenta,
       * end-effectors configurations: activation, position, orientation
       */
      DynamicsState ini_state_;

      /*! simple helper class to build a linear approximation of a friction cone */
      FrictionCone friction_cone_;
      FrictionCone::ConeMat cone_matrix_;

      /**
       * dynamics sequence of dynamic states. This is the main interface between the user
       * and the planner. The user can find in this variable all the optimization results
       */
      DynamicsSequence dyn_sequence_;

      /**
       * helper class to store information about the contact plan, such as surfaces,
       * position and orientation of a sequence of contacts
       */
      ContactPlanInterface* contact_plan_;

      /*! clock for timing purposes */
      Clock timer_;

      /*! type for optimization variable: continuous 'C' or binary 'B' and type of heuristic */
      char variable_type_;

      /*! helper boolean variables for the optimization problem */
      bool has_converged_;

      /*! helper integer variables for the optimization problem */
      int size_, num_vars_;

      /*! helper double variables for the optimization problem */
      double solve_time_, convergence_err_, last_convergence_err_;

      /*! helper vector variables for the optimization problem */
      Eigen::Vector3d com_pos_goal_, weight_desired_com_tracking_;

      /*! helper optimization variables for the optimization problem */
      solver::OptimizationVariable dt_, com_, lmom_, amom_, lmomd_, amomd_;
      std::array<solver::OptimizationVariable, Problem::n_endeffs_> frc_world_, trq_local_, cop_local_, ub_var_, lb_var_;

      /*! helper matrices and vectors for the optimization problem */
      solver::OptimizationVariable::OptVector solution_;
      solver::OptimizationVariable::OptMatrix mat_lb_, mat_ub_, mat_guess_, com_guess_, lmom_guess_, amom_guess_;
  };

}


#include <iostream>
#include <Eigen/Dense>

using namespace std;

struct Point
{
    Point() { };
    Point(float x, float y) : x(x), y(y) { };
    ~Point() { };
    Point(const Point& other) : x(other.x), y(other.y) {};

    union {
        struct {
            float x, y;
        };

        float _data[2];
        Eigen::Vector2f _eigen;
    };

    float& operator[](size_t ii)
    {
        assert(ii >= 0 && ii < 2);
        return _data[ii];
    };

    const float& operator[](size_t ii) const
    {
        assert(ii >= 0 && ii < 2);
        return _data[ii];
    };

    Eigen::Vector2f& asEigen() { return _eigen; };
    const Eigen::Vector2f& asEigen() const { return _eigen; };

    friend std::ostream& operator<<(std::ostream& os, const Point& pt);
};

std::ostream& operator<<(std::ostream& os, const Point& pt)
{
    os << pt.asEigen().transpose();
    return os;
}

int main()
{
    Eigen::Vector2f hello;
    Point world;
    Point gar(13258, .3905);

    hello[0] = 1;
    hello[1] = 1;

    cerr << sizeof(hello) << endl;
    cerr << sizeof(world) << endl;

    cerr << hello << endl;

    world.x = 100;
    world.y = 101.123;

    cerr << world.x << ", " << world.y << endl;
    cerr << world._data[0] << ", " << world._data[1] << endl;
    cerr << world._eigen.transpose() << endl;

    cerr << gar << endl;

    cerr << gar.asEigen().transpose() * gar.asEigen() << endl;

    auto gardup = gar;
    cerr << gardup << endl;

    auto gartheft = std::move(gar);
    cerr << gartheft << endl;
}


#include <boost/shared_ptr.hpp>
#include <iostream>
#include <time.h>

#include "fastsim.hpp"
#include "mlp.hpp"

int main(int argc, char* argv[])
{
    using namespace fastsim;
    // if (argc != 35 + 2) {
    //     std::cerr << "This program cannot run: you need to provide a XML file (e.g.," << argv[0] << " worlds/example.xml) and 35 weights" << std::endl;
    //    exit(1);
    //  }
    fastsim::Settings settings(argv[1]);
    boost::shared_ptr<Map> map = settings.map();
    boost::shared_ptr<Robot> robot = settings.robot();
    int timesteps = boost::lexical_cast<int>(argv[2]);
    std::string coreID = boost::lexical_cast<std::string>(argv[3]);

    using namespace nn;
    Mlp<Neuron<PfWSum<>, AfTanhNoBias<> >, Connection<> > nn(4 + 12, 3, 2);
    std::vector<float> in(4 + 12);
    // Mlp<Neuron<PfWSum<>, AfTanhNoBias<> >, Connection<> > nn(4, 3, 2);
    // std::vector<float> in(4);

    // weights
    std::vector<float> w(nn.get_nb_connections());
    // std::cout<<"argc -2 :" << argc - 2 << " --- nb connections:" << nn.get_nb_connections() << std::endl;

    assert(argc - 4 == nn.get_nb_connections());
    //srand(time(0));
    for (size_t i = 4; i < argc; ++i)
      w[i - 4] = boost::lexical_cast<float>(argv[i]);
    //rand()/(RAND_MAX+1.0)*4-2.0;
    nn.set_all_weights(w);
    nn.init();
    //std::ofstream nn_ofs("nn.dot");
    //nn.write(nn_ofs);

    boost::shared_ptr<Display> d;
    if (settings.display())
        d = boost::shared_ptr<Display>(new Display(map, *robot));

    std::string trajFilename = "traj.dat" + coreID;
    std::ofstream traj(trajFilename.c_str());
    for (int k = 0; k < timesteps; ++k) {
        // lasers
        for (size_t j = 0; j < robot->get_lasers().size(); ++j) {
            float d = robot->get_lasers()[j].get_dist();
            float range = robot->get_lasers()[j].get_range();
            in[j] = (d == -1 ? -1 : 1 - 2 * d / range);
        }
        // radar
        for (size_t j = 0; j < 4; ++j)
            in[robot->get_lasers().size() + j + 1] = -1.0f;
        int s = robot->get_radars()[0].get_activated_slice();

        in[robot->get_lasers().size() + 1] = 1.0f;

        for (int j = 0; j < 3; ++j)
            nn.step(in);

        // move the robot
        float out1 = nn.get_outf(0);
        float out2 = nn.get_outf(1);
        robot->move(out1 * 2, out2 * 2, map);
        //std::cout<<"out:"<<out1<<" "<<out2<<std::endl;

        traj << robot->get_pos().x() << " "
             << robot->get_pos().y() << " "
             << robot->get_pos().theta() << std::endl;
        // update the display
        if (settings.display())
            d->update();

    }
    return 0;
}


/*
 * imu_orb_fusion.cpp
 *
 *  Created on: May 21, 2016
 *      Author: sujiwo
 */

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <vector>
#include <thread>
#include <mutex>
#include "test_tools/IMUPacket.h"
#include "ParticleFilter.h"
#include "LocalizationObserver.h"
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <cmath>

#define DEFAULT_NUM_PARTICLE 500
#define InitialParticleRandomizer 2.0

using namespace PF;

struct ImuState {
	Eigen::Vector3d position;
//	Eigen::Quaterniond orientation;
//	Eigen::Matrix<double, 6, 6> pcovariance;
//
//	Eigen::Vector3d twist_linear;
//	Eigen::Vector3d twist_angular;
//	Eigen::Matrix<double, 6, 6> tcovariance;
	double yaw;

	ImuState () :
		position (Eigen::Vector3d::Zero()),
		yaw (0.0)
	{
//		orientation = Eigen::Quaterniond::Identity();
//		pcovariance = Eigen::Matrix<double,6,6>::Identity();
//		twist_linear = Eigen::Vector3d::Zero();
//		twist_angular = Eigen::Vector3d::Zero();
//		tcovariance = Eigen::Matrix<double,6,6>::Identity();
	}
};

struct ImuMotionCtrl {
	double wx, wy, wz;
	short int wheel;
	double timestamp;
};

static const string
	odomParentFrameId = "world",
	odomFrameId = "CameraEye";

void convert (const tf::Transform &pose, Eigen::Vector3d &position, Eigen::Quaterniond &orientation)
{
	position.x() = pose.getOrigin().x();
	position.y() = pose.getOrigin().y();
	position.z() = pose.getOrigin().z();
	orientation.x() = pose.getRotation().x();
	orientation.y() = pose.getRotation().y();
	orientation.z() = pose.getRotation().z();
	orientation.w() = pose.getRotation().w();
}

void convert (const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation, tf::Transform &resPose)
{
	resPose.setOrigin(
		tf::Vector3(
			position.x(),
			position.y(),
			position.z()));
	resPose.setRotation(
		tf::Quaternion(
			orientation.x(),
			orientation.y(),
			orientation.z(),
			orientation.w()));
}

//void quaternionToEuler (const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw)
//{
//
//}

class ImuFusion :
	public VehicleBase <ImuState, tf::Transform, ImuMotionCtrl>,
	public LocalizationObserver
{
public:

	ImuFusion () :
		filter (DEFAULT_NUM_PARTICLE, *this),
		positionFound (false),
		prevTime (0), prevCount (0),
		statePtrList(DEFAULT_NUM_PARTICLE)
	{}

	ImuState initializeParticleState() const
	{
		ImuState cst;
		double roll, pitch, yaw;
		cst.position.x() = initialPose.getOrigin().x();
		cst.position.y() = initialPose.getOrigin().y();
		cst.position.z() = initialPose.getOrigin().z();
		quaternionToEuler (initialPose, roll, pitch, yaw);
//		convert (initialPose, cst.position, cst.orientation);

		// Randomize
		cst.position.x() += PF::nrand(InitialParticleRandomizer);
		cst.position.y() += PF::nrand(InitialParticleRandomizer);
		cst.position.z() += PF::nrand(InitialParticleRandomizer);

		return cst;
	}

	ImuState motionModel (const ImuState &vstate, const ImuMotionCtrl &ctrl) const
	{
		double v;
		double dt = ctrl.timestamp - prevTime;
		short diff = ctrl.wheel - prevCount;

		/*
		 * Why these numbers are not used directly ?
		 */
		if (dt > 1e-4)
			v = (75.0 / 3.e5) * diff / dt;
		else v = 0.0;
		v *= 2.94;
		double wx = (0.03125 * M_PI/180.0) * (short)ctrl.wx;
		double wy = -(0.03125 * M_PI/180.0) * (short)ctrl.wy;
		double wz = (0.03125 * M_PI/180.0) * (short)ctrl.wz;
		wy -= wyOffset;

		ImuState nstate = calcOdometry(vstate, v, wy, dt);
		return nstate;
	}

	double measurementModel (const ImuState &vstate, const vector<tf::Transform> &observations) const
	{
		vector<double> obsWeights;

		for (const auto &pose: observations) {

			double wo,
				xt = pose.getOrigin().x(),
				yt = pose.getOrigin().y(),
				xs = vstate.position.x(),
				ys = vstate.position.y();

				wo = exp (-(pow(xt-xs,2) / (2*orbError*orbError) +
							pow(yt-ys,2) / (2*orbError*orbError)
					));

			obsWeights.push_back(wo);
		}

		double w = *std::max_element(obsWeights.begin(), obsWeights.end());
		return max (w, 0.01);
	}

	void imuCallback (const test_tools::IMUPacket::ConstPtr &imuMsg)
	{
		return;
		vector<tf::Transform> observationList;
		collectObservations (observationList);

		ImuMotionCtrl motion;
		motion.wx = imuMsg->wx;
		motion.wy = imuMsg->wy;
		motion.wz = imuMsg->wz;
		motion.wheel = imuMsg->wheel;
		motion.timestamp = imuMsg->header.stamp.toSec();

		if (positionFound==false) {
			if (observationList.size() != 0) {
				initialPose = observationList[0];
				filter.initializeParticles();
				positionFound = true;
				prevTime = imuMsg->header.stamp.toSec();
				prevCount = imuMsg->wheel;
				cout << "Position initialized" << endl;
			}

			else {}

			return;
		}

		filter.update (motion, observationList);

		filter.getStates (statePtrList);
		tf::Transform curPose = summarizePF(statePtrList);
//		cout << curPose.getOrigin().x() << " " << curPose.getOrigin().y() << endl;
		camBr.sendTransform(tf::StampedTransform(curPose, imuMsg->header.stamp, odomParentFrameId, odomFrameId));

		prevTime = imuMsg->header.stamp.toSec();
		prevCount = imuMsg->wheel;

		// Debugging
		display (prevTime, observationList);
	}

protected:
	ParticleFilter<ImuState, tf::Transform, ImuMotionCtrl> filter;
	double prevTime;
	unsigned long int prevCount;
	bool positionFound;
	tf::Transform initialPose;
	tf::TransformBroadcaster camBr;
	vector<ImuState*> statePtrList;

	static constexpr double
		wyOffset = 0.0034,
		odomNoise1 = 0.00115,
		odomNoise2 = 0.00115,
		odomNoise3 = 0.00345,
		odomNoise4 = 0.00345,
		orbError = 0.5;

private:

	/*
	 * Create new state from previous state
	 * based on three parameters
	 */
	static ImuState calcOdometry (const ImuState &cstate, double v, double wy, double dt)
	{
		ImuState nstate;

		Eigen::Matrix<double, 3, 2> V;
		Eigen::Matrix<double, 2, 3> VT;
		Eigen::Matrix2d N;
		Eigen::Matrix3d Q, G, GT, SO, SN;

		double delD = v * dt,
			   delT = wy * dt;
		double roll, pitch, yaw;
		quaternionToEuler(cstate.orientation, roll, pitch, yaw);

		// renewal odometry
		nstate.position.x() = cstate.position.x() - delD * sin(yaw);
		nstate.position.y() = cstate.position.y() + delD * cos(yaw);
		yaw += 	delT;
//		eulerToQuaternion(roll, pitch, yaw, nstate)
//		nstate.position.x() = cstate.position.x() + delD * cos(cstate.orientation.z());
//		nstate.position.y() = cstate.position.y() + delD * sin(cstate.orientation.z());
//		nstate.orientation.z() = cstate.orientation.z() + delT;
//		nstate.orientation.w() = 1.0;
//		if (nstate.orientation.z() < -M_PI)
//			nstate.orientation.z() += 2.0 * M_PI;
//		else if (nstate.orientation.z() > M_PI)
//			nstate.orientation.z() -= 2.0 * M_PI;
//		nstate.twist_linear.x() = v * dt;
//		nstate.twist_angular.z() = wy * dt;

		return nstate;
	}

	static ImuState getAverage (const vector<ImuState> &states)
	{
		for (const ImuState &state: states) {

		}
	}

	static tf::Transform summarizePF (const vector<ImuState> &states)
	{
		tf::Transform avgPose;
		Eigen::Vector3d pos = Eigen::Vector3d::Zero();
		Eigen::Quaterniond ori = Eigen::Quaterniond::Identity();

		for (const auto &st: states) {
			pos.x() += st.position.x();
			pos.y() += st.position.y();
			pos.z() += st.position.z();
			// XXX: orientation not handled
		}

		pos.x() /= (double)states.size();
		pos.y() /= (double)states.size();
		pos.z() /= (double)states.size();

		convert(pos, ori, avgPose);
		return avgPose;
	}

	static tf::Transform summarizePF (const vector<ImuState*> &statePtr)
	{
		tf::Transform avgPose;
		Eigen::Vector3d pos = Eigen::Vector3d::Zero();
		Eigen::Quaterniond ori = Eigen::Quaterniond::Identity();

		for (const ImuState* st: statePtr) {
			pos.x() += st->position.x();
			pos.y() += st->position.y();
			pos.z() += st->position.z();
			// XXX: orientation not handled
		}

		pos.x() /= (double)statePtr.size();
		pos.y() /= (double)statePtr.size();
		pos.z() /= (double)statePtr.size();

		convert(pos, ori, avgPose);
		return avgPose;
	}

	static void convertVoltageToLinear ()
	{

	}

	void display (double timestamp, const vector<tf::Transform> &observations)
	{
		static FILE* gnuplot;
		FILE *particle_file, *realpos_file, *sens_file,*trajectory;
		int i;
		double mx, my, n;
		static int count;

		if (!gnuplot) {
			gnuplot = popen("/usr/bin/gnuplot", "w");
			fprintf (gnuplot, "set term x11\n");
		}

		sens_file = fopen("/tmp/sens_file", "w");
		if (observations.size()>0) {
			fprintf (sens_file, "%f %f 0 0 \n",
				observations[0].getOrigin().x(),
				observations[0].getOrigin().y());
		}
		if (observations.size()>1) {
			fprintf (sens_file, "0 0 %f %f \n",
				observations[1].getOrigin().x(),
				observations[1].getOrigin().y());
		}
		fclose(sens_file);

		particle_file = fopen("/tmp/particle_file", "w");
//		mx=my=n=0;
		for (i=0; i<filter.getNumberOfParticles(); i++) {
			const Particle<ImuState> &particle = filter.getParticle(i);
			fprintf (particle_file, "%f %f \n",
				particle.current.position.x(),
				particle.current.position.y());
//			mx += particle.current.position.x();
//			my += particle.current.position.y();
			n+=1;
		}
		fclose (particle_file);

//		trajectory = fopen ("trajectory", "a");
//		fprintf(particle_file, "%f %f %f\n",
//			timestamp,
//			mx/filter.getNumberOfParticles(),
//			my/filter.getNumberOfParticles());
//		fclose(trajectory);

		// Adjust from ground truth before running
		fprintf(gnuplot, "set xrange [%f:%f]\n",-600.0,1000.0);//mx/n-20,mx/n+20);
		fprintf(gnuplot, "set yrange [%f:%f]\n",-1500.0,1500.0);//my/n-15,my/n+15);

		fprintf(gnuplot, "plot '/tmp/particle_file' u 1:2 w d,");

		//  if(z->observed)
		fprintf(gnuplot, "'/tmp/sens_file' u 1:2, '/tmp/sens_file' u 3:4 \n");
		//  fprintf(gnuplot, "'realpos_file'  u 1:2\n");

		fflush(gnuplot);

	}

};

int main (int argc, char **argv)
{
	ros::init (argc, argv, "imu_fusion");
	ros::NodeHandle nh;

	ImuFusion prius;
	prius.addTfListener ("world", "orb1");
//	prius.addTfListener ("world", "orb2");

	ros::Subscriber priusImuSubscriber = nh.subscribe ("imu_wheel", 1, &ImuFusion::imuCallback, &prius);
	cout << "Subscribed" << endl;
	ros::spin();

	return 0;
}


#include <fstream>
#include <sstream>
#include <iostream>
#include <string>
#include <iomanip>
#include <optional>
#include <cctype>
#include <map>
#include <algorithm>
#include <vector>
#include <range/v3/view.hpp>
#include <range/v3/algorithm.hpp>
#include <boost/dynamic_bitset.hpp>
#include <complex>
using namespace std::literals::complex_literals;
#include "../aoc2020/matrix.h"
#include "../aoc2020/mypair.h"

namespace views = ranges::views;

class Game
{
  using Position = std::complex<int>;
  constexpr static inline int N = 2001;
  constexpr static inline int DirIndex[2] = { 1, -1 };
  int origin = N / 2;
  FixedMatrix<bool, N> m;
  Mypair cur_;
  Position dir_;
  int cnt_ = 0;

public:
  Game() : cur_(origin, origin), dir_(0, 1)
  {
  }
  int forward(int n)
  {
    for (int i = 0; i < n; ++i) {
      cnt_ += !m(cur_.x, cur_.y);
      turn(m(cur_.x, cur_.y));
      m(cur_.x, cur_.y) = !m(cur_.x, cur_.y);
      cur_.x -= dir_.imag();
      cur_.y += dir_.real();
    }
    return cnt_;
  }
  void turn(bool isinfected)
  {
    dir_ *= Position{ 0, DirIndex[isinfected] };
  }

  friend std::istream &operator>>(std::istream &is, Game &g)
  {
    std::string s;
    int N = 25;
    for (int i = g.origin - N / 2; std::getline(is, s); ++i) {
      std::vector<int> v(s.size());
      int N = v.size();

      std::transform(s.begin(), s.end(), v.begin(), [](char c) { return c == '#'; });
      for (int j = g.origin - N / 2; j < g.origin + N / 2 + 1; ++j) {
        g.m(i, j) = v[j - (g.origin - N / 2)];
      }
    }
    return is;
  }
};

int main(int argc, char **argv)
{
  if (argc > 1) {
    Game g;
    std::ifstream iss(argv[1]);
    iss >> g;
    std::cout << g.forward(7) << std::endl;
    std::cout << g.forward(63) << std::endl;
    std::cout << g.forward(9930) << std::endl;
  }
}