/**
 * @file activation_functions_test.cpp
 * @author Marcus Edel
 *
 * Tests for the various activation functions.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/activation_functions/logistic_function.hpp>
#include <mlpack/methods/ann/activation_functions/identity_function.hpp>
#include <mlpack/methods/ann/activation_functions/softsign_function.hpp>
#include <mlpack/methods/ann/activation_functions/tanh_function.hpp>
#include <mlpack/methods/ann/activation_functions/rectifier_function.hpp>

#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/optimizer/rmsprop.hpp>
#include <mlpack/methods/ann/performance_functions/mse_function.hpp>

#include <mlpack/methods/ann/layer/bias_layer.hpp>
#include <mlpack/methods/ann/layer/linear_layer.hpp>
#include <mlpack/methods/ann/layer/base_layer.hpp>
#include <mlpack/methods/ann/layer/binary_classification_layer.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(ActivationFunctionsTest);

// Be careful!  When writing new tests, always get the boolean value and store
// it in a temporary, because the Boost unit test macros do weird things and
// will cause bizarre problems.

// Generate dataset for activation function tests.
const arma::colvec activationData("-2 3.2 4.5 -100.2 1 -1 2 0");

/*
 * Implementation of the activation function test.
 *
 * @param input Input data used for evaluating the activation function.
 * @param target Target data used to evaluate the activation.
 *
 * @tparam ActivationFunction Activation function used for the check.
 */
template<class ActivationFunction>
void CheckActivationCorrect(const arma::colvec input, const arma::colvec target)
{
  // Test the activation function using a single value as input.
  for (size_t i = 0; i < target.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::fn(input.at(i)),
        target.at(i), 1e-3);
  }

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  ActivationFunction::fn(input, activations);
  for (size_t i = 0; i < activations.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

/*
 * Implementation of the activation function derivative test.
 *
 * @param input Input data used for evaluating the activation function.
 * @param target Target data used to evaluate the activation.
 *
 * @tparam ActivationFunction Activation function used for the check.
 */
template<class ActivationFunction>
void CheckDerivativeCorrect(const arma::colvec input, const arma::colvec target)
{
  // Test the calculation of the derivatives using a single value as input.
  for (size_t i = 0; i < target.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::deriv(input.at(i)),
        target.at(i), 1e-3);
  }

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;
  ActivationFunction::deriv(input, derivatives);
  for (size_t i = 0; i < derivatives.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

/*
 * Implementation of the activation function inverse test.
 *
 * @param input Input data used for evaluating the activation function.
 * @param target Target data used to evaluate the activation.
 *
 * @tparam ActivationFunction Activation function used for the check.
 */
template<class ActivationFunction>
void CheckInverseCorrect(const arma::colvec input)
{
    // Test the calculation of the inverse using a single value as input.
  for (size_t i = 0; i < input.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::inv(ActivationFunction::fn(
        input.at(i))), input.at(i), 1e-3);
  }

  // Test the calculation of the inverse using the entire vector as input.
  arma::colvec activations;
  ActivationFunction::fn(input, activations);
  ActivationFunction::inv(activations, activations);

  for (size_t i = 0; i < input.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), input.at(i), 1e-3);
  }
}

/**
 * Basic test of the tanh function.
 */
BOOST_AUTO_TEST_CASE(TanhFunctionTest)
{
  const arma::colvec desiredActivations("-0.96402758 0.9966824 0.99975321 -1 \
                                         0.76159416 -0.76159416 0.96402758 0");

  const arma::colvec desiredDerivatives("0.07065082 0.00662419 0.00049352 0 \
                                         0.41997434 0.41997434 0.07065082 1");

  CheckActivationCorrect<TanhFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<TanhFunction>(desiredActivations, desiredDerivatives);
  CheckInverseCorrect<TanhFunction>(desiredActivations);
}

/**
 * Basic test of the logistic function.
 */
BOOST_AUTO_TEST_CASE(LogisticFunctionTest)
{
  const arma::colvec desiredActivations("1.19202922e-01 9.60834277e-01 \
                                         9.89013057e-01 3.04574e-44 \
                                         7.31058579e-01 2.68941421e-01 \
                                         8.80797078e-01 0.5");

  const arma::colvec desiredDerivatives("0.10499359 0.03763177 0.01086623 \
                                         3.04574e-44 0.19661193 0.19661193 \
                                         0.10499359 0.25");

  CheckActivationCorrect<LogisticFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<LogisticFunction>(desiredActivations,
      desiredDerivatives);
  CheckInverseCorrect<LogisticFunction>(activationData);
}

/**
 * Basic test of the softsign function.
 */
BOOST_AUTO_TEST_CASE(SoftsignFunctionTest)
{
  const arma::colvec desiredActivations("-0.66666667 0.76190476 0.81818182 \
                                         -0.99011858 0.5 -0.5 0.66666667 0");

  const arma::colvec desiredDerivatives("0.11111111 0.05668934 0.03305785 \
                                         9.7642e-05 0.25 0.25 0.11111111 1");

  CheckActivationCorrect<SoftsignFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<SoftsignFunction>(desiredActivations,
      desiredDerivatives);
  CheckInverseCorrect<SoftsignFunction>(desiredActivations);
}

/**
 * Basic test of the identity function.
 */
BOOST_AUTO_TEST_CASE(IdentityFunctionTest)
{
  const arma::colvec desiredDerivatives = arma::ones<arma::colvec>(
      activationData.n_elem);

  CheckActivationCorrect<IdentityFunction>(activationData, activationData);
  CheckDerivativeCorrect<IdentityFunction>(activationData, desiredDerivatives);
}

/**
 * Basic test of the rectifier function.
 */
BOOST_AUTO_TEST_CASE(RectifierFunctionTest)
{
  const arma::colvec desiredActivations("0 3.2 4.5 0 1 0 2 0");

  const arma::colvec desiredDerivatives("0 1 1 0 1 0 1 0");

  CheckActivationCorrect<RectifierFunction>(activationData, desiredActivations);
  CheckDerivativeCorrect<RectifierFunction>(desiredActivations,
      desiredDerivatives);
}

/*
 * Implementation of the numerical gradient checking.
 *
 * @param input Input data used for evaluating the network.
 * @param target Target data used to calculate the network error.
 * @param perturbation Constant perturbation value.
 * @param threshold Threshold used as bounding check.
 *
 * @tparam ActivationFunction Activation function used for the gradient check.
 */
template<class ActivationFunction>
void CheckGradientNumericallyCorrect(const arma::mat input,
                                     const arma::mat target,
                                     const double perturbation,
                                     const double threshold)
{
  // Specify the structure of the feed forward neural network.
  RandomInitialization randInit(-0.5, 0.5);
  arma::mat error;

  // Number of hidden layer units.
  const size_t hiddenLayerSize = 4;

  LinearLayer<mlpack::ann::RMSPROP, RandomInitialization> linearLayer0(
        input.n_rows, hiddenLayerSize, randInit);
  BiasLayer<> biasLayer0(hiddenLayerSize);
  BaseLayer<ActivationFunction> baseLayer0;

  LinearLayer<mlpack::ann::RMSPROP, RandomInitialization> linearLayer1(
         hiddenLayerSize, hiddenLayerSize, randInit);
  BiasLayer<> biasLayer1(hiddenLayerSize);
  BaseLayer<ActivationFunction> baseLayer1;

  LinearLayer<mlpack::ann::RMSPROP, RandomInitialization> linearLayer2(
         hiddenLayerSize, target.n_rows, randInit);
  BiasLayer<> biasLayer2(target.n_rows);
  BaseLayer<ActivationFunction> baseLayer2;

  BinaryClassificationLayer classOutputLayer;

  auto modules = std::tie(linearLayer0, biasLayer0, baseLayer0,
                          linearLayer1, biasLayer1, baseLayer1,
                          linearLayer2, biasLayer2, baseLayer2);

  FFN<decltype(modules), decltype(classOutputLayer), MeanSquaredErrorFunction>
      net(modules, classOutputLayer);

  // Initialize the feed forward neural network.
  net.FeedForward(input, target, error);
  net.FeedBackward(input, error);

  std::vector<std::reference_wrapper<decltype(linearLayer0)> > layer {
         linearLayer0, linearLayer1, linearLayer2 };

  std::vector<arma::mat> gradient {linearLayer0.Gradient(),
                                   linearLayer1.Gradient(),
                                   linearLayer2.Gradient()};

  double weight, mLoss, pLoss, dW, e;

  for (size_t l = 0; l < layer.size(); ++l)
  {
    for (size_t i = 0; i < layer[l].get().Weights().n_rows; ++i)
    {
      for (size_t j = 0; j < layer[l].get().Weights().n_cols; ++j)
      {
        // Store original weight.
        weight = layer[l].get().Weights()(i, j);

        // Add negative perturbation and compute error.
        layer[l].get().Weights().at(i, j) -= perturbation;
        net.FeedForward(input, target, error);
        mLoss = arma::as_scalar(0.5 * arma::sum(arma::pow(error, 2)));

        // Add positive perturbation and compute error.
        layer[l].get().Weights().at(i, j) += (2 * perturbation);
        net.FeedForward(input, target, error);
        pLoss = arma::as_scalar(0.5 * arma::sum(arma::pow(error, 2)));

        // Compute symmetric difference.
        dW = (pLoss - mLoss) / (2 * perturbation);
        e = std::abs(dW - gradient[l].at(i, j));

        bool b = e < threshold;
        BOOST_REQUIRE_EQUAL(b, 1);

        // Restore original weight.
        layer[l].get().Weights().at(i, j) = weight;
      }
    }
  }
}

/**
 * The following test implements numerical gradient checking. It computes the
 * numerical gradient, a numerical approximation of the partial derivative of J
 * with respect to the i-th input argument, evaluated at g. The numerical
 * gradient should be approximately the partial derivative of J with respect to
 * g(i).
 *
 * Given a function g(\theta) that is supposedly computing:
 *
 * @f[
 * \frac{\partial}{\partial \theta} J(\theta)
 * @f]
 *
 * we can now numerically verify its correctness by checking:
 *
 * @f[
 * g(\theta) \approx \frac{J(\theta + eps) - J(\theta - eps)}{2 * eps}
 * @f]
 */
BOOST_AUTO_TEST_CASE(GradientNumericallyCorrect)
{
  // Initialize dataset.
  const arma::colvec input = arma::randu<arma::colvec>(10);
  const arma::colvec target("0 1;");

  // Perturbation and threshold constant.
  const double perturbation = 1e-6;
  const double threshold = 1e-5;

  CheckGradientNumericallyCorrect<LogisticFunction>(input, target,
      perturbation, threshold);

  CheckGradientNumericallyCorrect<IdentityFunction>(input, target,
      perturbation, threshold);

  CheckGradientNumericallyCorrect<RectifierFunction>(input, target,
      perturbation, threshold);

  CheckGradientNumericallyCorrect<SoftsignFunction>(input, target,
      perturbation, threshold);

  CheckGradientNumericallyCorrect<TanhFunction>(input, target,
      perturbation, threshold);
}

BOOST_AUTO_TEST_SUITE_END();


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

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

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

int main()
{
  pack_ft pf = { 5.0f, -5.0f, 3.0f,  5.0f };
  pack_ft qf = { 2.0f,  2.0f, 3.0f, -2.0f };

  std::cout
    <<  "---- simd" << '\n'
    << " <- pf =                             " << pf << '\n'
    << " <- qf =                             " << qf << '\n'
    << " -> bs::rem(pf, qf) =                " << bs::rem(pf, qf) << '\n'
    << " -> bs::rem(bs::fix, pf, qf) =       " << bs::rem(bs::fix, pf, qf) << '\n'
    << " -> bs::rem(bs::ceil, pf, qf) =      " << bs::rem(bs::ceil, pf, qf) << '\n'
    << " -> bs::rem(bs::floor, pf, qf) =     " << bs::rem(bs::floor, pf, qf) << '\n'
    << " -> bs::rem(bs::round, pf, qf) =     " << bs::rem(bs::round, pf, qf) << '\n'
    << " -> bs::rem(bs::nearbyint, pf, qf) = " << bs::rem(bs::nearbyint, pf, qf) << '\n';

  float xf = 5.0, yf = 2.0f;

  std::cout
    << "---- scalar"  << '\n'
    << " <- xf =                             " << xf << '\n'
    << " <- yf =                             " << yf << '\n'
    << " -> bs::rem( xf, yf) =               " << bs::rem(xf, yf) << '\n'
    << " -> bs::rem(bs::fix, xf, yf) =       " << bs::rem(bs::fix, xf, yf) << '\n'
    << " -> bs::rem(bs::ceil, xf, yf) =      " << bs::rem(bs::ceil, xf, yf) << '\n'
    << " -> bs::rem(bs::floor, xf, yf) =     " << bs::rem(bs::floor, xf, yf) << '\n'
    << " -> bs::rem(bs::round, xf, yf) =     " << bs::rem(bs::round, xf, yf) << '\n'
    << " -> bs::rem(bs::nearbyint, xf, yf) = " << bs::rem(bs::nearbyint, xf, yf) << '\n';
  return 0;
}
//! [rem]


#include "KD.h"
#include "Ray.h"
#include "Material.h"
#include "Triangle.h"

#include <algorithm>

#include <Eigen/Dense>

std::vector<KdNode> kd_tree;

bool AABB::intersects(const Ray& ray) const {
    auto p = ray.p;
    auto d = ray.d;

    auto d_inv = Eigen::Vector3f(1.0f/d[0], 1.0f/d[1], 1.0f/d[2]);

    float mins[3] = {min.x, min.y, min.z};
    float maxs[3] = {max.x, max.y, max.z};

    float t1 = (mins[0] - p[0]) * d_inv[0];
    float t2 = (maxs[0] - p[0]) * d_inv[0];

    float tmin = std::min(t1, t2);
    float tmax = std::max(t1, t2);

    for (int i = 1; i < 3; ++i) {
        t1 = (mins[i] - p[i]) * d_inv[i];
        t2 = (maxs[i] - p[i]) * d_inv[i];

        tmin = std::max(tmin, std::min(std::min(t1, t2), tmax));
        tmax = std::min(tmax, std::max(std::max(t1, t2), tmin));
    }

    return tmax > std::max(tmin, 0.0f);
}

bool kd_load(const char* fileName, std::vector<KdNode> &kdTree) {
    FILE* fp = fopen(fileName, "r+");
    if (!fp) {
        fprintf(stderr, "Could not load %s\n", fileName);
        return false;
    }
    char temp[256];
    int nodeId = -1;
    while (true) {
        *temp = '\0';
        nodeId++;
        int ignore = fscanf(fp, "%s{ ", temp);
        if (!ignore) {
            fprintf(stderr, "Something catastrophic went wrong\n");
            exit(EXIT_FAILURE);
        }
        if (strcmp(temp, "inner{") == 0) {
            KdNode kd;
            kd.nodeId = nodeId;
            kd.isLeaf = false;
            int ignore = fscanf(fp, "%f %f %f %f %f %f ; %d %d %d %f }",
                                &kd.boundingBox.min.x,
                                &kd.boundingBox.min.y,
                                &kd.boundingBox.min.z,
                                &kd.boundingBox.max.x,
                                &kd.boundingBox.max.y,
                                &kd.boundingBox.max.z,
                                &kd.leftChildId,
                                &kd.rightChildId,
                                &kd.splitAxis,
                                &kd.splitPosition);
            if (!ignore) {
                fprintf(stderr, "Something catastrophic went wrong\n");
                exit(EXIT_FAILURE);
            }
            kdTree.push_back(kd);
        } else if (strcmp(temp, "leaf{") == 0) {
            KdNode kd;
            kd.nodeId = nodeId;
            kd.isLeaf = true;
            int ignore = fscanf(fp, "%f %f %f %f %f %f ;",
                                &kd.boundingBox.min.x,
                                &kd.boundingBox.min.y,
                                &kd.boundingBox.min.z,
                                &kd.boundingBox.max.x,
                                &kd.boundingBox.max.y,
                                &kd.boundingBox.max.z);
            if (!ignore) {
                fprintf(stderr, "Something catastrophic went wrong\n");
                exit(EXIT_FAILURE);
            }
            char token[256];
            while (true) {
                int ignore = fscanf(fp, " %s", token);
                if (!ignore) {
                    fprintf(stderr, "Something catastrophic went wrong\n");
                    exit(EXIT_FAILURE);
                }
                if (strcmp(token, "}") == 0)
                    break;
                int triIndex = atoi(token);
                kd.triIndex.push_back(triIndex);
            }
            kdTree.push_back(kd);
        } else {
            break;
        }
    }
    return true;
}

SurfaceList kd_intersect(const Ray& ray, const std::vector<KdNode>& kd_tree, int id, const Material& mt) {
    auto node = kd_tree[id];
    if (node.isLeaf) {
        // Construct all leaf triangles
        SurfaceList leaves;
        for (int i : node.triIndex) {
            int k0 = gTriangles[i].indices[0];
            int k1 = gTriangles[i].indices[1];
            int k2 = gTriangles[i].indices[2];

            auto a = Eigen::Vector3f(gPositions[k0].x, gPositions[k0].y, gPositions[k0].z);
            auto b = Eigen::Vector3f(gPositions[k1].x, gPositions[k1].y, gPositions[k1].z);
            auto c = Eigen::Vector3f(gPositions[k2].x, gPositions[k2].y, gPositions[k2].z);

            auto n = ((b-a).cross(c-a)).normalized();

            auto triangle1 = std::unique_ptr<Triangle>(new Triangle(a,b,c,n,mt));
            auto triangle2 = std::unique_ptr<Triangle>(new Triangle(c,b,a,-n,mt));

            leaves.add(std::move(triangle1));
            leaves.add(std::move(triangle2));
        }
        return leaves;
    } else {
        SurfaceList surfaces;

        // Recurse on children whose bounding boxes intersect ray
        auto left_node  = kd_tree[node.leftChildId];
        auto right_node = kd_tree[node.rightChildId];
        if (left_node.boundingBox.intersects(ray)) {
            auto left_triangles = kd_intersect(ray, kd_tree, node.leftChildId, mt);
            surfaces.add(left_triangles);
        }
        if (right_node.boundingBox.intersects(ray)) {
            auto right_triangles = kd_intersect(ray, kd_tree, node.rightChildId, mt);
            surfaces.add(right_triangles);
        }

        return surfaces;
    }
}


#include "filter.h"
#include "../../modules/math/constants.h"
#include "../../synthesis/synthesis.h"
#include <complex>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

using namespace Eigen;

VectorXcf poly(const VectorXcf& z) {
    VectorXcf poly(z.size() + 1);
    poly.setOnes();

    poly(0) = 1.0;

    for (int j = 0; j < z.size(); ++j) {
        for (int i = j; i >= 0; --i) {
            poly(i + 1) -= z(j) * poly(i);
        }
    }

    return poly;
}

std::vector<std::complex<double>> poly(const std::vector<std::complex<double>>& z) {
    std::vector<std::complex<double>> poly(z.size() + 1, 1.0);

    poly[0] = 1.0;

    for (int j = 0; j < z.size(); ++j) {
        for (int i = j; i >= 0; --i) {
            poly[i + 1] -= z[j] * poly[i];
        }
    }

    return poly;
}

std::vector<std::array<double, 6>> Analysis::butterworthHighpass(int N, double fc, double fs)
{
    const double Wn = fc / (fs / 2.0);
    const double Wo = tanf(Wn * M_PI / 2.0);

    std::vector<std::complex<double>> p;

    // Step 1. Get Butterworth analog lowpass prototype.
    for (int i = 2 + N - 1; i <= 3 * N - 1; i += 2) {
        p.push_back(std::polar<double>(1, (M_PI * i) / (2.0 * N)));
    }

    // Step 2. Transform to high pass filter.
    std::complex<double> Sg = 1.0,
                        prodSp = 1.0,
                        prodSz = 1.0;

    std::vector<std::complex<double>> Sp(p.size()), Sz(p.size());

    for (int i = 0; i < p.size(); ++i) {
        Sg *= -p[i];
        Sp[i] = Wo / p[i];
        Sz[i] = 0.0;
        prodSp *= (1.0 - Sp[i]);
        prodSz *= (1.0 - Sz[i]);
    }
    Sg = 1.0 / Sg;

    // Step 3. Transform to digital filter.
    std::vector<std::complex<double>> P(Sp.size()), Z(Sp.size());

    double G = std::real(Sg * prodSz / prodSp);

    for (int i = 0; i < Sp.size(); ++i) {
        P[i] = (1.0 + Sp[i]) / (1.0 - Sp[i]);
        Z[i] = (1.0 + Sz[i]) / (1.0 - Sz[i]);
    }

    // Step 6. Convert to SOS.

    return zpk2sos(Z, P, G);
}

std::vector<std::array<double, 6>> Analysis::butterworthLowpass(int N, double fc, double fs)
{
    const double Wn = fc / (fs / 2.0);
    const double Wo = tanf(Wn * M_PI / 2.0);

    std::vector<std::complex<double>> p;

    // Step 1. Get Butterworth analog lowpass prototype.
    for (int i = 2 + N - 1; i <= 3 * N - 1; i += 2) {
        p.push_back(std::polar<double>(1, (M_PI * i) / (2.0 * N)));
    }

    // Step 2. Transform to low pass filter.
    std::complex<double> Sg = 1.0,
                        prodSp = 1.0;

    std::vector<std::complex<double>> Sp(p.size()), Sz(0);

    for (int i = 0; i < p.size(); ++i) {
        Sg *= Wo;
        Sp[i] = Wo * p[i];
        prodSp *= (1.0 - Sp[i]);
    }

    // Step 3. Transform to digital filter.
    std::vector<std::complex<double>> P(Sp.size()), Z(Sp.size(), -1);

    double G = std::real(Sg / prodSp);

    for (int i = 0; i < Sp.size(); ++i) {
        P[i] = (1.0 + Sp[i]) / (1.0 - Sp[i]);
    }

    // Step 6. Convert to SOS.

    return zpk2sos(Z, P, G);
}


/* btcTools.cxx */
/* ====================================================================
 * INCLUDE ALL BITCOIN RELATED FUNCTIONS FOR THIS PROJECT.
 * DO NOT USE DIRECTLY; ONLY USE BY INCLUDING.
 * ====================================================================
 *
 * FUNCTIONS EXPLAINED:
 *
 * #1 uint256_t urandom_256():
 *    Generate a random 256-bit unsigned integer which serves as a Bitcoin
 *    private key. Cryptographically secure URANDOM engine is needed
 *    for the function. Return the large number.
 *
 * #2 string priv_to_wif_comp(uint256_t):
 *    Convert a Bitcoin private key into Wallet Import Format-compressed
 *    (string starts with K or L). Return the encoded key string.
 *
 * #3 uint256_t wif_to_priv(string):
 *    Convert a WIF-compressed key into its raw format(256-bit integer).
 *    Can process WIF-uncompressed keys but not used for this project.
 *    Return the large number.
 *
 * #4 string wif_comp_to_pkh(string):
 *    Derive the public key from the WIF-compressed using elliptical curve
 *    cryptography specified in Bitcoin; then use one way hash functions
 *    and base58 check encoding to produce a standard Bitcoin address
 *    (string starts with 1). Return the address.
 *
 * #5 string wif_comp_to_cipher6p(string, string):
 *    Consume a WIF-compressed key and produce a encrypted cipher based
 *    roughly on the BIP-38 standard with a slight change in KDF; instead
 *    of using scrypt, SHA512 is chosen to generate the key for AES256
 *    encryption. Flag byte is set to be 0xF0 which causes the cipher to
 *    begin with "6Pb/6Pc". Return the cipher string.
 *
 * #6 uint256_t cipher6p_decrypt(string, string):
 *    Use a password to convert the cipher string back into the raw private key.
 *    This process does not automatically verify the passowrd in execution.
 *    Wrong password will produce a wrong private key. Return the large number.
 *
 * #7 int get_ecdsa_sig(unsigned char*, int, unsigned char*, unsigned char**):
 *    Use the given message array and the private key to produce a ECDSA
 *    signature which complies with the Bitcoin consensus rules. It is used
 *    to authorize the spending of Bitcoin available to this wallet. The
 *    signature data is in DER encoding. Return the length of the signature.
 *
 * #8 void get_output_serial(std::string, double, unsigned char*):
 *    Generate a serialized output data for a raw transaction.
 *    Locking script is a simple P2PKH script. Return nothing.
 *
 * #9 string create_raw_transaction(vector<UTXO>&, vector<string>&, string, string, double, double):
 *    Respond to a send Bitcoin request from the user by generating a raw
 *    transaction data chunk which can be directly mined by the network.
 *    UTXOs chosen to be used as inputs will be voided after the transaction is
 *    created. It only supports sending Bitcoin to one recipient's address and
 *    will automatically generate a change output to refund the user the
 *    remaining balance(minus fee). The change output will be credited to the
 *    wallet's balance with the user's consent.
 *
 * #
 *
 *
 */

#include "btcStructs.h"
#include "btcTools.h"
#include <boost/multiprecision/cpp_int.hpp>
#include <openssl/evp.h>
#include <openssl/sha.h>
#include <openssl/ripemd.h>
#include <openssl/bn.h>
#include <openssl/obj_mac.h>
#include <openssl/ec.h>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <cmath>
#include <iomanip>
#include <sstream>

using namespace std;
using namespace boost::multiprecision;

typedef vector<unsigned char>::iterator vbit;

uint256_t urandom_256() {

	uint256_t real_key;
	uint256_t part_1, part_2, part_3, part_4;
	long part;
	size_t part_size = sizeof(part);

	ifstream urandom("/dev/urandom", ios::in|ios::binary);
	if (urandom) {
		urandom.read(reinterpret_cast<char*>(&part_1), part_size);
		urandom.read(reinterpret_cast<char*>(&part_2), part_size);
		urandom.read(reinterpret_cast<char*>(&part_3), part_size);
		urandom.read(reinterpret_cast<char*>(&part_4), part_size);
		}
	urandom.close();

	real_key += part_4 << 192;
	real_key += part_3 << 128;
	real_key += part_2 << 64;
	real_key += part_1;

	return real_key;

	}

string priv_to_wif_comp(uint256_t t_raw_key) {

	vector<unsigned char> main_stream;
	main_stream.push_back(0x80);

	uint256_t chunk;
	unsigned char temp_byte = 0;
	for (int i = 31; i >= 0; --i) {
		chunk = (t_raw_key >> (i * 8)) & 0xFF;

		while (chunk > 0) {
			temp_byte += 1;
			--chunk;
			}

		main_stream.push_back(temp_byte);
		temp_byte = 0;
		}
	main_stream.push_back(0x01);

	unsigned char hash_one[SHA256_DIGEST_LENGTH];
	SHA256_CTX hash_state;
	SHA256_Init(&hash_state);
	for (vbit iit = main_stream.begin(); iit != main_stream.end(); ++iit) {
		SHA256_Update(&hash_state, &(*iit), 1);
		}
	SHA256_Final(hash_one, &hash_state);

	unsigned char hash_two[SHA256_DIGEST_LENGTH];
	SHA256_Init(&hash_state);
	for (int j = 0; j < 32; ++j) {
		SHA256_Update(&hash_state, &(hash_one[j]), 1);
		}
	SHA256_Final(hash_two, &hash_state);

	vector<unsigned char> check_sum;
	for (int k = 0; k < 4; ++k) {
		check_sum.push_back(hash_two[k]);
		main_stream.push_back(hash_two[k]);
		}

	const string encoder = "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";
	uint512_t long_key;
	uint512_t shifter;
	int sfcounter = 37;
	for (vbit kkt = main_stream.begin(); kkt != main_stream.end(); ++kkt) {
		shifter = *kkt;
		long_key += (shifter << (sfcounter * 8));
		sfcounter -= 1;
		}

	string final_code = "";
	vector<char> parts;
	uint512_t temp_modulo;
	int modulo = 0;

	while (long_key > 0) {
		temp_modulo = long_key % 58;
		long_key -= temp_modulo;
		long_key /= 58;

		while (temp_modulo > 0) {
			modulo += 1;
			temp_modulo -= 1;
			}

		parts.push_back(encoder[modulo]);
		modulo = 0;
		}

	for (int l = parts.size() - 1; l >= 0; --l) {
		final_code += parts[l];
		}

	return final_code;

	}

uint256_t wif_to_priv(string t_wif_key) {

	uint256_t priv_key;
	string const decoder = "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";

	bool comp;
	if (t_wif_key.find("5") == 0) {
		comp = false;
		} else {comp = true;}

	uint512_t long_key;
	char code;
	int index;
	int power;
	uint512_t chunk;
	for (size_t i = 0; i < t_wif_key.length(); ++i) {
		code = t_wif_key[i];
		index = decoder.find(code);
		power = (t_wif_key.length() - 1) - i;
		chunk = 58;
		chunk = pow(chunk, power) * index;
		long_key += chunk;
		}

	vector<unsigned char> long_stream;
	vector<unsigned char> checksum;
	unsigned char ibyte = 0;
	uint512_t trunc;
	for (int j = 36; j >= 0; --j) {
		trunc = (long_key >> (j * 8)) & 0xFF;

		while (trunc > 0) {
			ibyte += 1;
			trunc -= 1;
			}

		if (j < 4) {
			checksum.push_back(ibyte);
			}else {
				if (comp and j != 4) {long_stream.push_back(ibyte);}
				else if (not comp and j != 36) {long_stream.push_back(ibyte);}
				}
		ibyte = 0;
		}

	uint256_t key_part;
	int shifter = 31;
	for (vbit kkt = long_stream.begin(); kkt != long_stream.end(); ++kkt) {
		key_part = *kkt;
		key_part = (key_part << shifter * 8);
		priv_key += key_part;
		shifter -= 1;
		}

	return priv_key;

	}

string wif_comp_to_pkh(string t_wif_comp) {

	string final_address;
	uint256_t raw_key;
	raw_key = wif_to_priv(t_wif_comp);

	unsigned char priv_key[32];
	unsigned char tbt = 0;
	uint256_t tchunk;
	for (int i = 0; i < 32; ++i) {
		tchunk = (raw_key >> ((31 - i) * 8)) & 0xFF;

		while (tchunk > 0) {
			++tbt;
			--tchunk;
			}

		priv_key[i] = tbt;
		tbt = 0;
		}

	unsigned char pub_key[33];
	size_t klen = 33;

	EC_GROUP *ec256;
	ec256 = EC_GROUP_new_by_curve_name(NID_secp256k1);

	BN_CTX *ctx;
	ctx = BN_CTX_new();
	BN_CTX_start(ctx);

	BIGNUM *bn_priv;
	bn_priv = BN_new();
	BN_bin2bn(priv_key, 32, bn_priv);

	EC_POINT *pt_pub;
	pt_pub = EC_POINT_new(ec256);

	EC_POINT_mul(ec256, pt_pub, bn_priv, NULL, NULL, ctx);
	EC_POINT_point2oct(ec256, pt_pub, POINT_CONVERSION_COMPRESSED,
					   pub_key, klen, ctx);

	SHA256_CTX SH;
	SHA256_CTX *sha_state = &SH;
	unsigned char sha_one[SHA256_DIGEST_LENGTH];
	SHA256_Init(sha_state);
	for (int j = 0; j < 33; ++j) {
		SHA256_Update(sha_state, &pub_key[j], 1);
		}
	SHA256_Final(sha_one, sha_state);

	RIPEMD160_CTX RIP;
	RIPEMD160_CTX *rip_state = &RIP;
	unsigned char rip_one[RIPEMD160_DIGEST_LENGTH];
	RIPEMD160_Init(rip_state);
	for (int k = 0; k < 32; ++k) {
		RIPEMD160_Update(rip_state, &sha_one[k], 1);
		}
	RIPEMD160_Final(rip_one, rip_state);

	unsigned char ex_pub_key[25];
	ex_pub_key[0] = 0x00;

	unsigned char sha_two[32];
	unsigned char sha_three[32];
	SHA256_Init(sha_state);
	SHA256_Update(sha_state, &ex_pub_key[0], 1);
	for (int l = 0; l < 20; ++l) {
		SHA256_Update(sha_state, &rip_one[l], 1);
		}
	SHA256_Final(sha_two, sha_state);

	SHA256_Init(sha_state);
	for (int m = 0; m < 32; ++m) {
		SHA256_Update(sha_state, &sha_two[m], 1);
		}
	SHA256_Final(sha_three, sha_state);

	for (int n = 0; n < 20; ++n) {
		ex_pub_key[n + 1] = rip_one[n];
		}
	for (int o = 0; o < 4; ++o) {
		ex_pub_key[o + 21] = sha_three[o];
		}

	const string encoder = "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";
	int lead_zero_count = 0;
	while (ex_pub_key[lead_zero_count] == 0x00) {
		final_address += "1";
		lead_zero_count += 1;
		}

	uint256_t bn_ex_key;
	uint256_t tchunk2;
	for (int p = 0; p < 25; ++p) {
		tchunk2 = ex_pub_key[p];
		tchunk2 = tchunk2 << ((24 - p) * 8);
		bn_ex_key += tchunk2;
		}

	vector<char> address_char;
	int modu = 0;
	uint256_t mchunk;
	while (bn_ex_key > 0) {
		mchunk = bn_ex_key % 58;
		bn_ex_key -= mchunk;
		bn_ex_key /= 58;
		while (mchunk > 0) {
			modu += 1;
			mchunk -= 1;
			}
		address_char.push_back(encoder[modu]);
		modu = 0;
		}

	for (int q = address_char.size() - 1; q >= 0; --q) {
		final_address += address_char[q];
		}

	return final_address;

	}

string wif_comp_to_cipher6p(string t_wif_comp, string t_password) {

	string address;
	size_t addr_len;

	unsigned char pre1 = 0x01;
	unsigned char pre2 = 0x42;
	unsigned char flag = 0xF0;

	address = wif_comp_to_pkh(t_wif_comp);
	addr_len = address.length();
	unsigned char addr_bytes[addr_len];
	copy(address.begin(), address.end(), addr_bytes);

	unsigned char saltsha1[32];
	unsigned char saltsha2[32];
	SHA256_CTX sha_state;
	SHA256_Init(&sha_state);
	for (size_t i = 0; i < addr_len; ++i) {
		SHA256_Update(&sha_state, &addr_bytes[i], 1);
		}
	SHA256_Final(saltsha1, &sha_state);

	SHA256_Init(&sha_state);
	for (int j = 0; j < 32; ++j) {
		SHA256_Update(&sha_state, &saltsha1[j], 1);
		}
	SHA256_Final(saltsha2, &sha_state);

	const unsigned char addr_hash[4] = {saltsha2[0], saltsha2[1],
										saltsha2[2], saltsha2[3]};

	// !! USE SHA512 INSTEAD OF SCRYPT TO DERIVE THE KEY !!
	unsigned char keyout[SHA512_DIGEST_LENGTH];

	unsigned char dh1[32];
	unsigned char dh2[32];

	size_t pass_len = t_password.length();
	unsigned char password[pass_len];
	copy(t_password.begin(), t_password.end(), password);

	SHA512_CTX ss512;
	SHA512_Init(&ss512);
	SHA512_Update(&ss512, &addr_hash[0], 1);
	SHA512_Update(&ss512, &addr_hash[1], 1);
	SHA512_Update(&ss512, &addr_hash[2], 1);
	SHA512_Update(&ss512, &addr_hash[3], 1);
	for (size_t i = 0; i < pass_len; ++i) {
		SHA512_Update(&ss512, &password[i], 1);
		}
	SHA512_Final(keyout, &ss512);

	for (int j = 0; j < 32; ++j) {
		dh1[j] = keyout[j];
		dh2[j] = keyout[j + 32];
		}

	const unsigned char r_key2[32] = {
		dh2[0], dh2[1], dh2[2], dh2[3], dh2[4], dh2[5],
		dh2[6], dh2[7], dh2[8], dh2[9], dh2[10], dh2[11],
		dh2[12], dh2[13], dh2[14], dh2[15], dh2[16], dh2[17],
		dh2[18], dh2[19], dh2[20], dh2[21], dh2[22], dh2[23],
		dh2[24], dh2[25], dh2[26], dh2[27], dh2[28], dh2[29],
		dh2[30], dh2[31]};
	const unsigned char* key2 = r_key2;

	uint256_t raw_key = wif_to_priv(t_wif_comp);
	unsigned char priv_key[32];
	unsigned char tbt = 0;
	uint256_t tchunk;
	for (int i = 0; i < 32; ++i) {
		tchunk = (raw_key >> ((31 - i) * 8)) & 0xFF;
		while (tchunk > 0) {
			++tbt;
			--tchunk;}
		priv_key[i] = tbt;
		tbt = 0;}

	unsigned char tempb1[16];
	unsigned char tempb2[16];
	for (int j = 0; j < 16; ++j) {
		tempb1[j] = priv_key[j] ^ dh1[j];
		tempb2[j] = priv_key[j + 16] ^ dh1[j + 16];
		}
	const unsigned char r_block1[16] = {
		tempb1[0], tempb1[1], tempb1[2], tempb1[3], tempb1[4], tempb1[5],
		tempb1[6], tempb1[7], tempb1[8], tempb1[9], tempb1[10], tempb1[11],
		tempb1[12], tempb1[13], tempb1[14], tempb1[15]};
	const unsigned char r_block2[16] = {
		tempb2[0], tempb2[1], tempb2[2], tempb2[3], tempb2[4], tempb2[5],
		tempb2[6], tempb2[7], tempb2[8], tempb2[9], tempb2[10], tempb2[11],
		tempb2[12], tempb2[13], tempb2[14], tempb2[15]};

	const unsigned char* block1 = r_block1;
	const unsigned char* block2 = r_block2;
	int block_len = 16;

	EVP_CIPHER_CTX *ctx;
	ctx = EVP_CIPHER_CTX_new();
	EVP_CIPHER_CTX *ctx2;
	ctx2 = EVP_CIPHER_CTX_new();

	unsigned char eh1[16];
	unsigned char eh2[16];
	unsigned char* eout1 = eh1;
	unsigned char* eout2 = eh2;
	int eh_len = sizeof(eh1);

	EVP_EncryptInit(ctx, EVP_aes_256_cbc(), key2, NULL);
	EVP_EncryptUpdate(ctx, eout1, &eh_len, block1, block_len);
	EVP_CIPHER_CTX_free(ctx);

	EVP_EncryptInit(ctx2, EVP_aes_256_cbc(), key2, NULL);
	EVP_EncryptUpdate(ctx2, eout2, &eh_len, block2, block_len);
	EVP_CIPHER_CTX_free(ctx2);

	vector<unsigned char> cipher_stream;
	cipher_stream.push_back(pre1);
	cipher_stream.push_back(pre2);
	cipher_stream.push_back(flag);
	cipher_stream.push_back(addr_hash[0]);
	cipher_stream.push_back(addr_hash[1]);
	cipher_stream.push_back(addr_hash[2]);
	cipher_stream.push_back(addr_hash[3]);

	for (int i = 0; i < 16; ++i) {cipher_stream.push_back(eh1[i]);}
	for (int i = 0; i < 16; ++i) {cipher_stream.push_back(eh2[i]);}
	assert (cipher_stream.size() == 39);

	unsigned char csm[SHA256_DIGEST_LENGTH];
	SHA256_Init(&sha_state);
	for (size_t j = 0; j < cipher_stream.size(); ++j) {
		SHA256_Update(&sha_state, &cipher_stream[j], 1);}
	SHA256_Final(csm, &sha_state);

	cipher_stream.push_back(csm[0]);
	cipher_stream.push_back(csm[1]);
	cipher_stream.push_back(csm[2]);
	cipher_stream.push_back(csm[3]);
	assert (cipher_stream.size() == 43);

	const string encoder = "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";
	uint512_t long_key;
	uint512_t shifter;
	int sfcounter = 42;
	for (vbit iit = cipher_stream.begin(); iit != cipher_stream.end(); ++iit) {
		shifter = *iit;
		long_key += (shifter << (sfcounter * 8));
		sfcounter -= 1;
		}

	string final_cipher;
	vector<char> parts;
	uint512_t temp_modulo;
	int modulo = 0;

	while (long_key > 0) {
		temp_modulo = long_key % 58;
		long_key -= temp_modulo;
		long_key /= 58;

		while (temp_modulo > 0) {
			modulo += 1;
			temp_modulo -= 1;
			}

		parts.push_back(encoder[modulo]);
		modulo = 0;
		}

	for (int i = parts.size() - 1; i >= 0; --i) {final_cipher += parts[i];}

	return final_cipher;

	}

uint256_t cipher6p_decrypt(string t_cipher6p, string t_password) {

	int cp_len = t_cipher6p.length();

	const string decoder = "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";
	uint512_t bn_cipher, chunk = 58;
	int value;
	for (int i = cp_len - 1; i > -1; --i) {
		value = decoder.find(t_cipher6p[cp_len - 1 - i]);
		chunk = pow(chunk, i) * value;
		bn_cipher += chunk;
		chunk = 58;
		}

	vector<unsigned char> cipher_bytes;
	unsigned char cbyte = 0;
	chunk = 0;
	for (int j = 42; j > -1; --j) {
		chunk = (bn_cipher >> (j * 8)) & 0xFF;
		while (chunk > 0) {++cbyte; --chunk;}
		cipher_bytes.push_back(cbyte);
		cbyte = 0;
		}

	const unsigned char salt[4] = {cipher_bytes[3], cipher_bytes[4],
								   cipher_bytes[5], cipher_bytes[6]};

	unsigned char password[t_password.length()];
	copy(t_password.begin(), t_password.end(), password);

	const unsigned char eh1[16] = {
		cipher_bytes[7], cipher_bytes[8], cipher_bytes[9], cipher_bytes[10],
		cipher_bytes[11], cipher_bytes[12], cipher_bytes[13], cipher_bytes[14],
		cipher_bytes[15], cipher_bytes[16], cipher_bytes[17], cipher_bytes[18],
		cipher_bytes[19], cipher_bytes[20], cipher_bytes[21], cipher_bytes[22]};
	const unsigned char eh2[16] = {
		cipher_bytes[23], cipher_bytes[24], cipher_bytes[25], cipher_bytes[26],
		cipher_bytes[27], cipher_bytes[28], cipher_bytes[29], cipher_bytes[30],
		cipher_bytes[31], cipher_bytes[32], cipher_bytes[33], cipher_bytes[34],
		cipher_bytes[35], cipher_bytes[36], cipher_bytes[37], cipher_bytes[38]};
	const unsigned char* dblock1 = eh1;
	const unsigned char* dblock2 = eh2;

	unsigned char full_key[SHA512_DIGEST_LENGTH];
	unsigned char d1h1[16];
	unsigned char d1h2[16];
	SHA512_CTX ss512;
	SHA512_Init(&ss512);
	SHA512_Update(&ss512, &salt[0], 1);
	SHA512_Update(&ss512, &salt[1], 1);
	SHA512_Update(&ss512, &salt[2], 1);
	SHA512_Update(&ss512, &salt[3], 1);
	for (size_t i = 0; i < t_password.length(); ++i) {
		SHA512_Update(&ss512, &password[i], 1);}
	SHA512_Final(full_key, &ss512);

	for (int j = 0; j < 16; ++j) {
		d1h1[j] = full_key[j];
		d1h2[j] = full_key[j + 16];
		}

	const unsigned char d2h[32] = {
		full_key[32], full_key[33], full_key[34], full_key[35],
		full_key[36], full_key[37], full_key[38], full_key[39],
		full_key[40], full_key[41], full_key[42], full_key[43],
		full_key[44], full_key[45], full_key[46], full_key[47],
		full_key[48], full_key[49], full_key[50], full_key[51],
		full_key[52], full_key[53], full_key[54], full_key[55],
		full_key[56], full_key[57], full_key[58], full_key[59],
		full_key[60], full_key[61], full_key[62], full_key[63]
		};
	const unsigned char* dhkey = d2h;

	unsigned char result1[16];
	unsigned char result2[16];
	unsigned char* out1 = result1;
	unsigned char* out2 = result2;
	int out_len = 16;

	EVP_CIPHER_CTX *ctx;
	ctx = EVP_CIPHER_CTX_new();
	EVP_CIPHER_CTX *ctx2;
	ctx2 = EVP_CIPHER_CTX_new();

	EVP_DecryptInit(ctx, EVP_aes_256_cbc(), dhkey, NULL);
	EVP_DecryptUpdate(ctx, out1, &out_len, dblock1, 16);
	EVP_CIPHER_CTX_free(ctx);

	EVP_DecryptInit(ctx2, EVP_aes_256_cbc(), dhkey, NULL);
	EVP_DecryptUpdate(ctx2, out2, &out_len, dblock2, 16);
	EVP_CIPHER_CTX_free(ctx2);

	unsigned char pkh1[16];
	unsigned char pkh2[16];
	for (int i = 0; i < 16; ++i) {
		pkh1[i] = result1[i] ^ d1h1[i];
		pkh2[i] = result2[i] ^ d1h2[i];
		}

	uint256_t final_priv_key, chunk1, chunk2;
	for (int j = 15; j > -1; --j) {
		chunk1 = pkh1[15 - j];
		chunk2 = pkh2[15 - j];
		chunk1 = chunk1 << ((j + 16) * 8);
		chunk2 = chunk2 << (j * 8);
		final_priv_key += (chunk1 + chunk2);
		}

	// string final_wif_comp = priv_to_wif_comp(final_priv_key);
	// return final_wif_comp;

	return final_priv_key;

	}

int get_ecdsa_sig(unsigned char* t_digest, int t_dlen, unsigned char* t_secret, unsigned char** t_sigpp) {

	const EC_GROUP* ecgroup = EC_GROUP_new_by_curve_name(NID_secp256k1);
	EC_KEY* eckey = EC_KEY_new_by_curve_name(NID_secp256k1);
	EC_POINT* ecpoint = EC_POINT_new(ecgroup);

	BN_CTX* bnctx = BN_CTX_new();
	const BIGNUM* priv_bn = BN_bin2bn(t_secret, 32, NULL);

	EC_POINT_mul(ecgroup, ecpoint, priv_bn, NULL, NULL, bnctx);
	const EC_POINT* pubkey = EC_POINT_dup(ecpoint, ecgroup);
	EC_KEY_set_private_key(eckey, priv_bn);
	EC_KEY_set_public_key(eckey, pubkey);

	const ECDSA_SIG* ecwit = ECDSA_do_sign(t_digest, t_dlen, eckey);
	size_t siglen = i2d_ECDSA_SIG(ecwit, t_sigpp);
	*t_sigpp -= siglen;

	return siglen;

	}

void get_output_serial(string t_address, double t_amount, unsigned char* osout) {
	// output size = 8 + 4 + 20 + 2 = 34.

	unsigned char* rawpub = new unsigned char;

	const string decoder = "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";
	size_t adlen = t_address.length();
	uint256_t bigN, chunk, cbyte;
	int value;
	for (size_t i = 0; i < adlen; ++i) {
		value = decoder.find(t_address[i]);
		chunk = 58;
		chunk = pow(chunk, adlen - 1 - i) * value;
		bigN += chunk;
		}

	unsigned char ibyte;
	for (int j = 0; j < 20; ++j) {
		cbyte = (bigN >> (23 - j) * 8) & 0xFF;
		while (cbyte > 0) {++ibyte; --cbyte;}
		rawpub[j] = ibyte;
		ibyte = 0;
		}

	uint64_t satoshi = t_amount * 100000000;
	unsigned char abyte;
	for (int i = 0; i < 8; ++i) {
		abyte = (satoshi >> (i * 8)) & 0xFF;
		osout[i] = abyte;
		}

	osout[8] = 0x19;
	osout[9] = 0x76;
	osout[10] = 0xa9;
	osout[11] = 0x14;

	for (int i = 0; i < 20; ++i)
	{osout[12 + i] = rawpub[i];}

	osout[32] = 0x88;
	osout[33] = 0xac;

	}

string create_raw_transaction(vector<UTXO>& t_uts, vector<string>& t_wif_set, string t_recip, string t_chg, double t_pay, double t_fee) {

	vector<UTXO> utins;
	size_t allulen = t_uts.size();
	double wealth = 0, gross = t_pay + t_fee;
	for (size_t i = 0; i < allulen; ++i) {
		if (wealth < gross) {
			if (t_uts[i].good()) {
				utins.push_back(t_uts[i]);
				t_uts[i].destroy();
				wealth = wealth + t_uts[i].get_balance();}
			}
		else {break;}
		}

	double change = wealth - t_pay - t_fee;
	unsigned char ninp = (unsigned char)utins.size();
	unsigned char noup = 0x02;

	vector<unsigned char> tx_struct, tx_final;
	tx_struct.push_back(0x01); tx_final.push_back(0x01);
	tx_struct.push_back(0x00); tx_final.push_back(0x00);
	tx_struct.push_back(0x00); tx_final.push_back(0x00);
	tx_struct.push_back(0x00); tx_final.push_back(0x00);
	tx_struct.push_back(ninp); tx_final.push_back(ninp);	// branch off.

	for (size_t i = 0; i < utins.size(); ++i) {
		unsigned char* stc_input = new unsigned char[66];
		utins[i].input_serial(stc_input);
		for (int j = 0; j < 66; ++j)
		{tx_struct.push_back(stc_input[j]);}
		}

	tx_struct.push_back(noup);
	unsigned char* rec_out = new unsigned char[34];
	unsigned char* chg_out = new unsigned char[34];
	get_output_serial(t_recip, t_pay, rec_out);
	get_output_serial(t_chg, change, chg_out);

	for (int i = 0; i < 34; ++i)
	{tx_struct.push_back(rec_out[i]);}
	for (int i = 0; i < 34; ++i)
	{tx_struct.push_back(chg_out[i]);}
	tx_struct.push_back(0x00); tx_struct.push_back(0x00);
	tx_struct.push_back(0x00); tx_struct.push_back(0x00);
	tx_struct.push_back(0x01); tx_struct.push_back(0x00);
	tx_struct.push_back(0x00); tx_struct.push_back(0x00);

	// get signature.
	unsigned char* d1 = new unsigned char[32];
	unsigned char* digest_stc = new unsigned char[32];
	SHA256_CTX shactx;
	SHA256_Init(&shactx);
	for (size_t i = 0; i < tx_struct.size(); ++i)
	{SHA256_Update(&shactx, &tx_struct[i], 1);}
	SHA256_Final(d1, &shactx);
	SHA256_Init(&shactx);
	for (size_t i = 0; i < 32; ++i)
	{SHA256_Update(&shactx, &(d1[i]), 1);}
	SHA256_Final(digest_stc, &shactx);

	unsigned char tm;
	vector<unsigned char> all_sigs;
	vector<int> all_siglens;
	int wlen;
	for (size_t i = 0; i < utins.size(); ++i) {
		unsigned char* so = new unsigned char[1024];
		unsigned char** sigpp = &so;
		unsigned char* isecret = new unsigned char[32];
		utins[i].fetch_private(t_wif_set, isecret);
		wlen = get_ecdsa_sig(digest_stc, 32, isecret, sigpp);
		all_siglens.push_back(wlen);
		for (int j = 0; j < wlen; ++j) {
			tm = (*sigpp)[j];
			all_sigs.push_back(tm);
			}
		}

	// finalize the tx.
	unsigned char publen = 0x21;
	unsigned char ecslen;
	unsigned char sigplen;
	unsigned char fullsiglen;
	int sigmark = 0;
	for (size_t i = 0; i < utins.size(); ++i) {
		unsigned char* bcs = new unsigned char[36];
		utins[i].bc_partial_serial(bcs);
		for (int j = 0; j < 36; ++j)
		{tx_final.push_back(bcs[j]);}

		ecslen = (unsigned char)all_siglens[i];
		sigplen = ecslen + 0x01;
		fullsiglen = ecslen + 0x02 + 0x01 + publen;
		tx_final.push_back(fullsiglen);
		tx_final.push_back(sigplen);
		for (int j = 0; j < ecslen; ++j) {
			tm = all_sigs[j + sigmark];
			tx_final.push_back(tm);
			}
		sigmark += ecslen;
		tx_final.push_back(0x01);

		unsigned char* asecret = new unsigned char[32];
		unsigned char* acs = new unsigned char[38];
		utins[i].fetch_private(t_wif_set, asecret);
		utins[i].ac_partial_serial(asecret, acs);
		for (int j = 0; j < 38; ++j)
		{tx_final.push_back(acs[j]);}
		}

	tx_final.push_back(noup);
	unsigned char* nrec_out = new unsigned char[34];
	unsigned char* nchg_out = new unsigned char[34];
	get_output_serial(t_recip, t_pay, nrec_out);
	get_output_serial(t_chg, change, nchg_out);
	for (int i = 0; i < 34; ++i)
	{tx_final.push_back(nrec_out[i]);}
	for (int i = 0; i < 34; ++i)
	{tx_final.push_back(nchg_out[i]);}
	tx_final.push_back(0x00); tx_final.push_back(0x00);
	tx_final.push_back(0x00); tx_final.push_back(0x00);

	unsigned char* id1 = new unsigned char[32];
	unsigned char* txid = new unsigned char[32];
	SHA256_Init(&shactx);
	for (size_t i = 0; i < tx_final.size(); ++i)
	{SHA256_Update(&shactx, &(tx_final[i]), 1);}
	SHA256_Final(id1, &shactx);
	SHA256_Init(&shactx);
	for (int i = 0; i < 32; ++i)
	{SHA256_Update(&shactx, &(id1[i]), 1);}
	SHA256_Final(txid, &shactx);

	string final_id;
	string hex_table = "0123456789abcdef";
	int big, small;
	for (int i = 31; i >= 0; --i) {
		small = txid[i] % 16;
		big = (txid[i] - small) / 16;
		final_id += hex_table[big];
		final_id += hex_table[small];
		}

	cout << "\n---------- New Transaction ----------\n" << endl;
	cout << "Full TX Data: " << hex;
	for (size_t i = 0; i < tx_final.size(); ++i)
	{cout << setfill('0') << setw(2) << (int)tx_final[i];}
	cout << "     ----- End of Data\n" << endl;
	cout << "Data Size: " << dec << tx_final.size() << " Bytes" << endl;
	cout << "New TXID: " << final_id << endl;
	cout << "Change Paid Back: " << change << " BTC" << endl;
	cout << "Adding Outputs to UTXO Set......";
	t_uts.push_back(UTXO(final_id, t_recip, "NO", 0, t_pay));
	t_uts.push_back(UTXO(final_id, t_chg, "NO", 1, change));
	cout << " Finished.\n" << endl;
	cout << "---------- Operation Complete ----------\n" << endl;

	return final_id;

	}


/**
 * Copyright (c) 2015-present, Facebook, Inc.
 * All rights reserved.
 *
 * This source code is licensed under the BSD+Patents license found in the
 * LICENSE file in the root directory of this source tree.
 */

// Copyright 2004-present Facebook. All Rights Reserved

#include <cstdio>
#include <cstdlib>

#include <boost/test/unit_test.hpp>
#include "test-util.h"

#include <faiss/IndexIVFPQ.h>
#include <faiss/IndexFlat.h>
#include <faiss/index_io.h>

BOOST_AUTO_TEST_CASE(IVFPQ_index) {

    // dimension of the vectors to index
    int d = 64;

    // size of the database we plan to index
    size_t nb = 1000;

    // make a set of nt training vectors in the unit cube
    // (could be the database)
    size_t nt = 10000;

    // make the index object and train it
    faiss::IndexFlatL2 coarse_quantizer (d);

    // a reasonable number of cetroids to index nb vectors
    int ncentroids = 25;

    faiss::IndexIVFPQ index (&coarse_quantizer, d,
                             ncentroids, 16, 8);

    // index that gives the ground-truth
    faiss::IndexFlatL2 index_gt (d);

    //srand48 (35);
	int nSeed = 35;

    { // training

        std::vector <float> trainvecs (nt * d);
		generate_float_vector(d, trainvecs.data(), nt,nSeed);
        index.verbose = true;
        index.train (nt, trainvecs.data());
    }

    { // populating the database

        std::vector <float> database (nb * d);
		generate_float_vector(d, database.data(), nb, nSeed+1);

        index.add (nb, database.data());
        index_gt.add (nb, database.data());
    }

    int nq = 200;
    int n_ok;

    { // searching the database

        std::vector <float> queries (nq * d);
		generate_float_vector(d, queries.data(), nq, nSeed+2);

        std::vector<faiss::Index::idx_t> gt_nns (nq);
        std::vector<float>               gt_dis (nq);

        index_gt.search (nq, queries.data(), 1,
                         gt_dis.data(), gt_nns.data());

        index.nprobe = 5;
        int k = 5;
        std::vector<faiss::Index::idx_t> nns (k * nq);
        std::vector<float>               dis (k * nq);

        index.search (nq, queries.data(), k, dis.data(), nns.data());

        n_ok = 0;
        for (int q = 0; q < nq; q++) {

            for (int i = 0; i < k; i++)
                if (nns[q * k + i] == gt_nns[q])
                    n_ok++;
        }
		BOOST_TEST_MESSAGE("number of ok " << n_ok);
        BOOST_CHECK(n_ok>nq * 0.4);
    }

}


#include <iostream>
#include <mlpack/methods/ann/dists/bernoulli_distribution.hpp>
#include <armadillo>
#define PRINT 1

using namespace mlpack::ann;
using namespace std;
using namespace arma;

int main()
{
  // Using this only for demo purposes. This will dropout second channel always.
  arma_rng::set_seed(0);

  // User input
  arma::mat input(12, 1);
  input << 0.4963 << 0.0885 << 0.7682 << 0.1320 << 0.3074 << 0.4901 << 0.6341 << 0.8964 << 0.4556 << 0.3489 << 0.6323 << 0.4017 << endr;
  input = input.t();

  double ratio = 0.2;
  size_t size = 3; // input channels

  arma::mat output;
  output.zeros(arma::size(input));

  // Forward()
  size_t batchSize = input.n_cols;
  size_t inputSize = input.n_rows / size;
  double scale = 1.0 / (1.0 - ratio);
  arma::cube inputTemp(const_cast<arma::mat&>(input).memptr(), inputSize, size, batchSize, false, false);
  arma::cube outputTemp(const_cast<arma::mat&>(output).memptr(), inputSize, size, batchSize, false, false);
  arma::mat probabilities(1, size);
  arma::mat maskRow(1, size);
  arma::mat mask;
  probabilities.fill(ratio);
  BernoulliDistribution<> bernoulli_dist(probabilities, false);
  maskRow = bernoulli_dist.Sample();
  mask = arma::repmat(maskRow, inputSize, 1);

  for(size_t n = 0; n < batchSize; n++)
  {
    arma::mat& inputImage = inputTemp.slice(n);
    arma::mat& outputImage = outputTemp.slice(n);
    outputImage = inputImage % mask * scale;

    if(PRINT)
    {
      cout << "Image " << n << " calculations: " << endl;
      cout << "-----------------------------------" << endl;
      cout << "-----------------------------------" << endl;
      cout << "INPUT for Spatial Dropout: " << endl;
      cout << inputImage << endl;
      cout << "-----------------------------------" << endl;
      cout << "OUTPUT for Spatial Dropout: " << endl;
      cout << outputImage << endl;
      cout << "-----------------------------------" << endl;
    }
  }

  // this is gy for Spatial Dropout layer simulated as a tensor filled with an arbitrary sequence of values.
  arma::mat gy;
  gy << 1 << 3 << 2 << 4 << 5 << 7 << 6 << 8 << 9 << 11 << 10 << 12 << endr;
  gy = gy.t();

  // Backward()
  arma::mat g;
  g.zeros(arma::size(input));

  arma::cube gyTemp(const_cast<arma::mat&>(gy).memptr(), inputSize, size, batchSize, false, false);
  arma::cube gTemp(const_cast<arma::mat&>(g).memptr(), inputSize, size, batchSize, false, false);

  for(size_t n = 0; n < batchSize; n++)
  {
    arma::mat& gyImage = gyTemp.slice(n);
    arma::mat& gImage = gTemp.slice(n);

    gImage = gyImage % mask * scale;

    if(PRINT)
    {
      cout << "Image " << n << " calculations: " << endl;
      cout << "-----------------------------------" << endl;
      cout << "-----------------------------------" << endl;
      cout << "Hypothetical gy for Spatial Dropout: " << endl;
      cout << gyImage << endl;
      cout << "-----------------------------------" << endl;
      cout << "g for Spatial Dropout: " << endl;
      cout << gImage << endl;
      cout << "-----------------------------------" << endl;
    }
  }

  return 0;
}


#include <Engine/MeshEdit/ARAP.h>
#include <Engine/MeshEdit/CParameterize.h>
#include <Engine/Primitive/TriMesh.h>
#include <assert.h>
#include <Eigen/Sparse>

#include <Engine/MeshEdit/Paramaterize.h>

using namespace Ubpa;
using namespace std;
using namespace Eigen;

Ubpa::ARAP::ARAP(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
	texture_flag = false;
}

void Ubpa::ARAP::Clear()
{
	heMesh->Clear();

	triMesh = nullptr;
}

bool Ubpa::ARAP::Init(Ptr<TriMesh> triMesh)
{
	Clear();
	if (triMesh == nullptr)
		return true;
	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::ARAP::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	this->nT = triMesh->GetTriangles().size();
	this->nV = triMesh->GetPositions().size();

	// step1: init half-edge structure
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(this->nV);
	heMesh->Init(triangles);
	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}
	for (int i = 0; i < this->nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	this->triMesh = triMesh;

	// step2: initializing the points2d and cotan
	// (locally coordinates and cotangent of every angle for every triangle)
	CongruentMapping2D();

	// step3: locally parameterization with boundry
	auto parameterizeUniformCircle = ParameterizeUniformCircle::New(triMesh);
	if (parameterizeUniformCircle->Run()) {
		printf("paramaterizeUniformSquare done\n");
	}

	// step4: pick two anchor vertexes,
	// which can (a) remove ratation and displacement of the result (b) avoid  degradation solution.
	auto triangle = heMesh->Polygons().back();
	auto v1 = triangle->BoundaryVertice()[0];
	anchor_v1_idx = heMesh->Index(v1);
	auto v2 = triangle->BoundaryVertice()[1];
	anchor_v2_idx = heMesh->Index(v2);
	anchor_pos1 = pointf2(0, 0);
	anchor_pos2 = pointf2(2, 2);

	// step5: generate and compute the coefficient matrix A,
	// which is constant in all iterations, so it can be generated and computed only once.
	Generate_and_compute_A();

	return true;
}

bool Ubpa::ARAP::Run()
{
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::MinSurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	error = -1;
	ARAP_Pipeline(); // kernel iteration of the ARAP algorithm

	// Finally, half-edge structure -> triangle mesh, end.
	vector<pointf3> positions;
	vector<unsigned> indice;
	vector<pointf2> texcoords;

	this->nV = heMesh->NumVertices();
	const std::vector<Ptr<Ubpa::Triangle>>& triangles = triMesh->GetTriangles();
	this->nT = triangles.size();

	positions.reserve(this->nV);
	indice.reserve(3 * this->nT);
	texcoords.reserve(this->nV);

	for (auto v : heMesh->Vertices()) {
		positions.push_back(v->pos.cast_to<pointf3>());
		texcoords.push_back(v->pos.cast_to<pointf2>());
	}
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	if (this->texture_flag)
		this->triMesh->Update(texcoords);
	else
		this->triMesh->Update(positions);
	return true;
}

void Ubpa::ARAP::Add_texture()
{
	texture_flag = true;

}

void Ubpa::ARAP::ARAP_Pipeline()
{

	// stop condition
	size_t iter_n = 2;

	for (int i = 0; i < iter_n; i++)
	{
		Local_Phase();
		Global_Phase();
		cout << "iter:" << i << " error: " << error << endl;
	}
}

void Ubpa::ARAP::Global_Phase()
{
	// step1: set b
	setb(anchor_v1_idx, anchor_pos1, anchor_v2_idx, anchor_pos2);

	// Then solve the equation
	ARAP_solution = solver.solve(ARAP_mat_b);

	this->texCoor.clear();
	for (size_t i = 0; i < this->nV; i++)
	{
		double dist = pointf3::distance(pointf3(ARAP_solution(i, 0), ARAP_solution(i, 1), 0.0), heMesh->Vertices()[i]->pos.cast_to<pointf3>());
		if (error < 0)
		{
			error = dist;
		}
		else if (error < dist)
		{
			error = dist;
		}

		heMesh->Vertices()[i]->pos[0] = ARAP_solution(i, 0);
		heMesh->Vertices()[i]->pos[1] = ARAP_solution(i, 1);
		heMesh->Vertices()[i]->pos[2] = 0.0;
		// update tex coordinates
		texCoor.push_back(pointf2(ARAP_solution(i, 0), ARAP_solution(i, 1)));
	}
}

void Ubpa::ARAP::Local_Phase() // 使用SVD分解求解最优的Lt
{
	// Get newest u per iteration ,
	triangle_points.clear();	// remember to clear
	for (auto triangle : heMesh->Polygons())
	{
		if (triangle != nullptr)
		{
			this->triangle_points.push_back(triangle->BoundaryVertice());
		}
	}

	// 构造St(u)
	Lt_array.clear(); // 后面更新Lt
	for (size_t t = 0; t < this->nT; t++)
	{
		auto vec_u = triangle_points[t];
		auto mapped_u = points2d[t];
		Matrix2d St;
		St.setZero();

		// get St
		for (int i = 0; i < 3; i++)
		{
			V* u0 = vec_u[i];
			V* u1 = vec_u[(i + 1) % 3];
			MatrixXd delta_u(2, 1);
			delta_u <<
				u0->pos[0] - u1->pos[0], u0->pos[1] - u1->pos[1];

			pointf3 x0 = mapped_u[u0];
			pointf3 x1 = mapped_u[u1];
			MatrixXd delta_x(2, 1);
			delta_x <<
				x0[0] - x1[0], x0[1] - x1[1];

			double cot = getCotan(t, vec_u[(i + 2) % 3]);

			St += cot * delta_u * delta_x.transpose();
		}

		// Do SVD Composition on St
		JacobiSVD<MatrixXd> svd(St, ComputeThinU | ComputeThinV);
		Matrix2d Lt = svd.matrixU() * svd.matrixV().transpose(); // Lt = U * V^T

		Lt_array.push_back(Lt);

	}
}

void Ubpa::ARAP::Generate_and_compute_A()
{
	// Two Anchor points
	A_sparse.resize(this->nV, this->nV);
	ARAP_coeff.push_back(Eigen::Triplet<double>(anchor_v1_idx, anchor_v1_idx, 1));
	//ARAP_coeff.push_back(Eigen::Triplet<double>(anchor_v2_idx, anchor_v2_idx, 1));

	// Other non-anchor points
	for (size_t i = 0; i < nV; i++)
	{
		if (i != anchor_v1_idx /*&& i != anchor_v2_idx*/) {
			auto v = heMesh->Vertices()[i];
			double cotan_sum = 0.0;

			// traverse adjacent vertrices
			for (auto adj_v : v->AdjVertices())
			{
				size_t adj_idx = heMesh->Index(adj_v);

				// To get cotan
				auto e = v->EdgeWith(adj_v);
				auto he1 = e->HalfEdge();

				double cot1 = 0.0;
				if (he1->Polygon() != nullptr)
				{
					auto tri1_idx = heMesh->Index(he1->Polygon()); // get index of adjacent triangle
					try
					{
						auto tri_v1 = he1->Next()->End(); // get vertix of adjacent triangle
						assert(heMesh->Index(tri_v1) != heMesh->Index(v)
							&& heMesh->Index(tri_v1) != heMesh->Index(adj_v));

						cot1 = getCotan(tri1_idx, tri_v1);
					}
					catch (const std::exception& e)
					{
						cout << "cannot find cot 1 in map. " << endl;
					}
				}
				auto he2 = e->HalfEdge()->Pair();

				double cot2 = 0.0;
				if (he2->Polygon() != nullptr)
				{
					auto tri2_idx = heMesh->Index(he2->Polygon());
					try
					{
						auto tri_v2 = he2->Next()->End();
						assert(heMesh->Index(tri_v2) != heMesh->Index(v)
							&& heMesh->Index(tri_v2) != heMesh->Index(adj_v));
						cot2 = getCotan(tri2_idx, tri_v2);
					}
					catch (const std::exception& e)
					{
						cout << "cannot find cot 2 in map. " << endl;
					}
				}

				ARAP_coeff.push_back(Eigen::Triplet<double>(i, adj_idx, -(cot1 + cot2)));

				cotan_sum += (cot1 + cot2);
			}

			ARAP_coeff.push_back(Eigen::Triplet<double>(i, i, cotan_sum));
		}
	}
	A_sparse.setFromTriplets(ARAP_coeff.begin(), ARAP_coeff.end());
	// pre-computation
	solver.compute(A_sparse);
	if (solver.info() != Success) {
		qDebug() << "decomposition failed" << endl;
	}

}

void ARAP::CongruentMapping2D()
{
	this->points2d.clear();
	this->cot2d.clear();
	for (auto triangle : this->heMesh->Polygons())
	{
		if (triangle != nullptr) {
			map<V*, pointf3> map_v_2d;

			auto v0 = triangle->BoundaryVertice()[0];
			auto v1 = triangle->BoundaryVertice()[1];
			auto v2 = triangle->BoundaryVertice()[2];

			map_v_2d[v0] = pointf3(0, 0, 0); // set new v0 = (0,0,0)

			// get 2d coordinates
			double dist01 = pointf3::distance(v0->pos.cast_to<pointf3>(), v1->pos.cast_to<pointf3>());
			double dist02 = pointf3::distance(v0->pos.cast_to<pointf3>(), v2->pos.cast_to<pointf3>());
			double cos01_02 = vecf3::cos_theta((v1->pos - v0->pos), (v2->pos - v0->pos)); // angle of edge 01 and 02
			double sin01_02 = sqrt(1 - cos01_02 * cos01_02);

			map_v_2d[v1] = pointf3(dist01, 0, 0);   // set new v1 = (dist01,0,0)
			map_v_2d[v2] = pointf3(dist02 * cos01_02, dist02 * sin01_02, 0);  // set new v2 = (d02 * cos, d02 * sin, 0)

			this->points2d.push_back(map_v_2d); // triangle in points2d has the same sequence with heMesh->triangles()

			// get cot
			map<V*, double> cotan_2d;
			for (size_t i = 0; i < 3; i++)
			{
				double cos = vecf3::cos_theta((triangle->BoundaryVertice()[i]->pos - triangle->BoundaryVertice()[(i + 2) % 3]->pos),
					(triangle->BoundaryVertice()[(i + 1) % 3]->pos - triangle->BoundaryVertice()[(i + 2) % 3]->pos));
				double sin = sqrt(1 - cos * cos);
				cotan_2d[triangle->BoundaryVertice()[(i + 2) % 3]] = (cos / sin); // store the angle of edge i, i+1. idx is V_{(i+2) %3}
			}
			assert(cotan_2d.size() == 3);
			this->cot2d.push_back(cotan_2d);
		}
	}

	assert(this->points2d.size() == this->nT);
	assert(this->cot2d.size() == this->nT);

}

double ARAP::getCotan(int tri_idx, V* v)
{
	assert(this->cot2d.size() == this->nT);
	try
	{
		auto cotan = cot2d[tri_idx].at(v);
		return cotan;
	}
	catch (const std::exception& e)
	{
		cout << "[Error] Cannot find V in map. ";
	}
}
double ARAP::getCotan(V* v0, V* v1, V* v2)
{
	double cos = vecf3::cos_theta((v0->pos - v2->pos), (v1->pos - v2->pos));
	double sin = sqrt(1 - cos * cos);
	return cos / sin;
}

void ARAP::setb(size_t idx1, pointf2 pos1, size_t idx2, pointf2 pos2)
{
	ARAP_mat_b = MatrixXd(this->nV, 2);
	ARAP_mat_b.setZero();

	// two anchor points
	ARAP_mat_b(idx1, 0) = pos1[0];  // x
	ARAP_mat_b(idx1, 1) = pos1[1]; // y

	//ARAP_mat_b(idx2, 0) = pos2[0];  // x
	//ARAP_mat_b(idx2, 1) = pos2[1]; // y

	// Other non-anchor points
	for (size_t i = 0; i < this->nV; i++)
	{
		if (i != idx1/* && i != idx2*/) {
			auto v = heMesh->Vertices()[i];
			MatrixXd b(2, 1);
			b.setZero();
			//cout << "v idx: " << heMesh->Index(v) << endl<<endl;
			// traverse adjecent vertrices
			for (auto adj_v : v->AdjVertices())
			{
				size_t adj_idx = heMesh->Index(adj_v);  // get index of adj_v
				// get adjacent triangles
				auto e = v->EdgeWith(adj_v);
				auto he1 = e->HalfEdge();
				auto he2 = e->HalfEdge()->Pair();

				// get Lt and cot
				auto triangle1 = he1->Polygon();
				if (triangle1 != nullptr)
				{
					auto tri_v1 = he1->Next()->End();
					assert(heMesh->Index(tri_v1) != heMesh->Index(v)
						&& heMesh->Index(tri_v1) != heMesh->Index(adj_v));

					size_t tri_idx = heMesh->Index(triangle1);
					double cot1 = getCotan(tri_idx, tri_v1);
					map<V*, pointf3> mapped_v = this->points2d[tri_idx]; // congruent mapping of triangle1
					MatrixXd Lt = Lt_array[tri_idx];
					MatrixXd delta_x(2, 1);
					delta_x <<
						mapped_v[v][0] - mapped_v[adj_v][0],
						mapped_v[v][1] - mapped_v[adj_v][1];

					b += cot1 * Lt * delta_x;
				}

				auto triangle2 = he2->Polygon();
				if (triangle2 != nullptr)
				{
					auto tri_v2 = he2->Next()->End();
					assert(heMesh->Index(tri_v2) != heMesh->Index(v)
						&& heMesh->Index(tri_v2) != heMesh->Index(adj_v));

					size_t tri_idx = heMesh->Index(triangle2);
					double cot2 = getCotan(tri_idx, tri_v2);
					map<V*, pointf3> mapped_v = this->points2d[tri_idx]; // congruent mapping of triangle1
					MatrixXd Lt = Lt_array[tri_idx];
					MatrixXd delta_x(2, 1);
					delta_x <<
						mapped_v[v][0] - mapped_v[adj_v][0],
						mapped_v[v][1] - mapped_v[adj_v][1];
					b += cot2 * Lt * delta_x;
				}
			}
			ARAP_mat_b(i, 0) = b(0); // x
			ARAP_mat_b(i, 1) = b(1); // y
		}
	}
}


#ifndef _MLES_LAYER_HPP_
#define _MLES_LAYER_HPP_

#include <memory>
#include <Eigen/Dense>
#include "Activation.hpp"
#include <fstream>

namespace mles
{
    class Layer
    {
        private:
            unsigned int id;
            bool isOutput;
            Eigen::MatrixXd weight;
            ActivationPtr activation;

            Eigen::MatrixXd z, dz, A, delta;

        public:
            Layer();
            Layer(unsigned int id, unsigned int inputSize, unsigned int outputSize, ActivationPtr activation, bool isOutput);
            virtual ~Layer();

            double error(const Eigen::MatrixXd& y);
            Eigen::MatrixXd forward(const Eigen::MatrixXd& x);
            Eigen::MatrixXd backward(const Eigen::MatrixXd& y, Layer *layer);
            Eigen::MatrixXd update(double learningRate, const Eigen::MatrixXd& a);

            void load(std::ifstream& f);
            void write(std::ofstream& f);
            void print();

            void reset();

    };
}

#endif


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

#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>

using namespace boost::numeric::ublas;

int main() {
        // Simple vector fill
    vector<double> a(3);
    a <<= 0, 1, 2;
    std::cout << a << std::endl;
    // [ 0 1 2]

    // Vector from vector
    vector<double> b(7);
    b <<= a, 10, a;
    std::cout << b << std::endl;
    // [ 0 1 2 10 0 1 2]

    // Simple matrix fill
    matrix<double> A(3,3);
    A <<= 0, 1, 2,
         3, 4, 5,
         6, 7, 8;
    std::cout << A << std::endl;
    // [ 0 1 2 ]
    // [ 3 4 5 ]
    // [ 6 7 8 ]

    // Matrix from vector
    A <<= 0, 1, 2,
         3, 4, 5,
         a;
    std::cout << A << std::endl;
    // [ 0 1 2 ]
    // [ 3 4 5 ]
    // [ 0 1 2 ]

    // Matrix from vector - column assignment
    A <<= move(0,2), traverse_policy::by_column(),
         a;
    std::cout << A << std::endl;
    // [ 0 1 0 ]
    // [ 3 4 1 ]
    // [ 0 1 2 ]

    // Another matrix from vector example (watch the wraping);
    vector<double> c(9); c <<= 1, 2, 3, 4, 5, 6, 7, 8, 9;
    A <<= c;
    std::cout << A << std::endl;
    // [ 1 2 3 ]
    // [ 4 5 6 ]
    // [ 7 8 9 ]

    // If for performance(Benchmarks are not definite about that) or consistency reasons you need to disable wraping:
    static next_row_manip endr; //This can be defined globally
    A <<= traverse_policy::by_row_no_wrap(),
            1, 2, 3, endr,
            4, 5, 6, endr,
            7, 8, 9, endr;
    // [ 1 2 3 ]
    // [ 4 5 6 ]
    // [ 7 8 9 ]
    // If by default you need to disable wraping define
    // BOOST_UBLAS_DEFAULT_NO_WRAP_POLICY, in the compilation options,
    // so that you avoid typing the "traverse_policy::by_row_no_wrap()".

    //  Plus and minus assign:
    A <<= fill_policy::index_plus_assign(),
         3,2,1;
    std::cout << A << std::endl;
    // [ 4 4 4 ]
    // [ 4 5 6 ]
    // [ 7 8 9 ]

    // Matrix from proxy
    A <<= 0, 1, 2,
         project(b, range(3,6)),
         a;
    std::cout << A << std::endl;
    // [ 0 1 2 ]
    // [10 0 1 ]
    // [ 6 7 8 ]

    // Matrix from matrix
    matrix<double> B(6,6);
    B <<= A, A,
         A, A;
    std::cout << B << std::endl;
    // [ A A ]
    // [ A A ]

    // Matrix range (vector is similar)
    B = zero_matrix<double>(6,6);
    matrix_range<matrix<double> > mrB (B, range (1, 4), range (1, 4));
    mrB <<= 1,2,3,4,5,6,7,8,9;
    std::cout << B << std::endl;
    // [ 0 0 0 0 0 0]
    // [ 0 1 2 3 0 0]
    // [ 0 4 5 6 0 0]
    // [ 0 0 0 0 0 0]
    // [ 0 0 0 0 0 0]
    // [ 0 0 0 0 0 0]

    // Horizontal concatenation can be achieved using this trick:
    matrix<double> BH(3,9);
    BH <<= A, A, A;
    std::cout << BH << std::endl;
    // [ A A A]

    // Vertical concatenation can be achieved using this trick:
    matrix<double> BV(9,3);
    BV <<= A,
          A,
          A;
    std::cout << BV << std::endl;
    // [ A ]
    // [ A ]
    // [ A ]

    // Watch the difference when assigning matrices for different traverse policies:
    matrix<double> BR(9,9, 0);
    BR <<= traverse_policy::by_row(), // This is the default, so this might as well be omitted.
          A, A, A;
    std::cout << BR << std::endl;
    // [ A A A]
    // [ 0 0 0]
    // [ 0 0 0]

    matrix<double> BC(9,9, 0);
    BC <<= traverse_policy::by_column(),
          A, A, A;
    std::cout << BC << std::endl;
    // [ A 0 0]
    // [ A 0 0]
    // [ A 0 0]

    // The following will throw a run-time exception in debug mode (matrix mid-assignment wrap is not allowed) :
    // matrix<double> C(7,7);
    // C <<= A, A, A;

    // Matrix from matrix with index manipulators
    matrix<double> C(6,6,0);
    C <<= A, move(3,0), A;
    // [ A 0 ]
    // [ 0 A ]

    // A faster way for to construct this dense matrix.
    matrix<double> D(6,6);
    D <<= A, zero_matrix<double>(3,3),
         zero_matrix<double>(3,3), A;
    // [ A 0 ]
    // [ 0 A ]

    // The next_row and next_column index manipulators:
    // note: next_row and next_column functions return
    // a next_row_manip and and next_column_manip object.
    // This is the manipulator we used earlier when we disabled
    // wrapping.
    matrix<double> E(2,4,0);
    E <<= 1, 2, next_row(),
         3, 4, next_column(),5;
    std::cout << E << std::endl;
    // [ 1 2 0 5 ]
    // [ 3 4 0 0 ]

    // The begin1 (moves to the begining of the column) index manipulator, begin2 does the same for the row:
    matrix<double> F(2,4,0);
    F <<= 1, 2, next_row(),
         3, 4, begin1(),5;
    std::cout << F << std::endl;
    // [ 1 2 5 0 ]
    // [ 3 4 0 0 ]

    // The move (relative) and move_to(absolute) index manipulators (probably the most useful manipulators):
    matrix<double> G(2,4,0);
    G <<= 1, 2, move(0,1), 3,
         move_to(1,3), 4;
    std::cout << G << std::endl;
    // [ 1 2 0 3 ]
    // [ 0 0 0 4 ]

    // Static equivallents (faster) when sizes are known at compile time:
    matrix<double> Gs(2,4,0);
    Gs <<= 1, 2, move<0,1>(), 3,
         move_to<1,3>(), 4;
    std::cout << Gs << std::endl;
    // [ 1 2 0 3 ]
    // [ 0 0 0 4 ]

    // Choice of traverse policy (default is "row by row" traverse):

    matrix<double> H(2,4,0);
    H <<= 1, 2, 3, 4,
         5, 6, 7, 8;
    std::cout << H << std::endl;
    // [ 1 2 3 4 ]
    // [ 5 6 7 8 ]

    H <<= traverse_policy::by_column(),
        1, 2, 3, 4,
        5, 6, 7, 8;
    std::cout << H << std::endl;
    // [ 1 3 5 7 ]
    // [ 2 4 6 8 ]

    // traverse policy can be changed mid assignment if desired.
     matrix<double> H1(4,4,0);
     H1 <<= 1, 2, 3, traverse_policy::by_column(), 1, 2, 3;

    std::cout << H << std::endl;
    // [1 2 3 1]
    // [0 0 0 2]
    // [0 0 0 3]
    // [0 0 0 0]

    // note: fill_policy and traverse_policy are namespaces, so you can use them
    // by a using statement.

    // For compressed and coordinate matrix types a push_back or insert fill policy can be chosen for faster assginment:
    compressed_matrix<double> I(2, 2);
    I <<=    fill_policy::sparse_push_back(),
            0, 1, 2, 3;
    std::cout << I << std::endl;
    // [ 0 1 ]
    // [ 2 3 ]

    coordinate_matrix<double> J(2,2);
    J<<=fill_policy::sparse_insert(),
        1, 2, 3, 4;
    std::cout << J << std::endl;
    // [ 1 2 ]
    // [ 3 4 ]

    // A sparse matrix from another matrix works as with other types.
    coordinate_matrix<double> K(3,3);
    K<<=fill_policy::sparse_insert(),
        J;
    std::cout << K << std::endl;
    // [ 1 2 0 ]
    // [ 3 4 0 ]
    // [ 0 0 0 ]

    // Be careful this will not work:
    //compressed_matrix<double> J2(4,4);
    //J2<<=fill_policy::sparse_push_back(),
     //   J,J;
    // That's because the second J2's elements
    // are attempted to be assigned at positions
    // that come before the elements already pushed.
    // Unfortunatelly that's the only thing you can do in this case
    // (or of course make a custom agorithm):
    compressed_matrix<double> J2(4,4);
    J2<<=fill_policy::sparse_push_back(),
        J, fill_policy::sparse_insert(),
        J;

    std::cout << J2 << std::endl;
    // [  J   J  ]
    // [ 0 0 0 0 ]
    // [ 0 0 0 0 ]

    // A different traverse policy doesn't change the result, only they order it is been assigned.
    coordinate_matrix<double> L(3,3);
    L<<=fill_policy::sparse_insert(), traverse_policy::by_column(),
        J;
    std::cout << L << std::endl;
    // (same as previous)
    // [ 1 2 0 ]
    // [ 3 4 0 ]
    // [ 0 0 0 ]

    typedef coordinate_matrix<double>::size_type cmst;
    const cmst size = 30;
    //typedef fill_policy::sparse_push_back spb;
    // Although the above could have been used the following is may be faster if
    //  you use the policy often and for relatively small containers.
    static fill_policy::sparse_push_back spb;

    // A block diagonal sparse using a loop:
    compressed_matrix<double> M(size, size, 4*15);
    for (cmst i=0; i!=size; i+=J.size1())
        M <<= spb, move_to(i,i), J;

    // If typedef was used above the last expression should start
    // with M <<= spb()...

    // Displaying so that blocks can be easily seen:
    for (unsigned int i=0; i!=M.size1(); i++) {
        std::cout << M(i,0);
        for (unsigned int j=1; j!=M.size2(); j++) std::cout << ", " << M(i,j);
        std::cout << "\n";
    }
    // [ J 0 0 0 ... 0]
    // [ 0 J 0 0 ... 0]
    // [ 0 . . . ... 0]
    // [ 0 0 ... 0 0 J]

    // A "repeat" trasverser may by provided so that this becomes faster and an on-liner like:
    // M <<= spb, repeat(0, size, J.size1(), 0, size, J.size1()), J;
    // An alternate would be to create a :repeater" matrix and vector expression that can be used in other places as well. The latter is probably better,
    return 0;
}


#include <iostream>
#include <algorithm>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <numeric>
#include <strings.h>
#include <assert.h>

#include <dirent.h>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <fstream>
#include <sstream>
BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > Polygon;

namespace py = pybind11;
using namespace std;

struct box{
  float ry;
  float l;
  float w;
  float h;
  float x;
  float z;
  float y;
  int cls_num;
  float is_obj;
  float wx0;
  float wx1;
  float wx2;
  float wx3;
  float wy0;
  float wy1;
  float wy2;
  float wy3;

};

void compute_4_points(vector<box> &boxes)
{
  for(int i=0;i<boxes.size();i++)
  {
    float ry = boxes[i].ry;
    float x = boxes[i].x;
    float z = boxes[i].z;
    float l = boxes[i].l;
    float w = boxes[i].w;
    using namespace boost::numeric::ublas;
    using namespace boost::geometry;
    matrix<double> mref(2, 2);
    mref(0, 0) = cos(ry); mref(0, 1) = sin(ry);
    mref(1, 0) = -sin(ry); mref(1, 1) = cos(ry);

    matrix<double> corners(2, 4);
    double data[] = {l / 2, l / 2, -l / 2, -l / 2,
                     w / 2, -w / 2, -w / 2, w / 2};
    std::copy(data, data + 8, corners.data().begin());
    matrix<double> gc = prod(mref, corners);
    for (int i = 0; i < 4; ++i) {
        gc(0, i) += x;
        gc(1, i) += z;
    }
    boxes[i].wx0 = gc(0, 0); boxes[i].wy0=gc(1, 0);
    boxes[i].wx1 = gc(0, 1); boxes[i].wy1=gc(1, 1);
    boxes[i].wx2 = gc(0, 2); boxes[i].wy2=gc(1, 2);
    boxes[i].wx3 = gc(0, 3); boxes[i].wy3=gc(1, 3);
  }

}

Polygon toPolygon_py(double ry,double l,double w,double x,double z)
{
    using namespace boost::numeric::ublas;
    using namespace boost::geometry;
    matrix<double> mref(2, 2);
    mref(0, 0) = cos(ry); mref(0, 1) = sin(ry);
    mref(1, 0) = -sin(ry); mref(1, 1) = cos(ry);

    static int count = 0;
    matrix<double> corners(2, 4);
    double data[] = {l / 2, l / 2, -l / 2, -l / 2,
                     w / 2, -w / 2, -w / 2, w / 2};
    std::copy(data, data + 8, corners.data().begin());
    matrix<double> gc = prod(mref, corners);
    for (int i = 0; i < 4; ++i) {
        gc(0, i) += x;
        gc(1, i) += z;
    }

    double points[][2] = {{gc(0, 0), gc(1, 0)},{gc(0, 1), gc(1, 1)},{gc(0, 2), gc(1, 2)},{gc(0, 3), gc(1, 3)},{gc(0, 0), gc(1, 0)}};
    Polygon poly;
    append(poly, points);
    return poly;
}
double groundBoxOverlap_py(double ry1,double l1,double w1,double x1,double z1,\
  double ry2,double l2,double w2,double x2,double z2, int criterion = -1)
{
  using namespace boost::geometry;
  Polygon gp = toPolygon_py(ry1,l1,w1,x1,z1);
  Polygon dp = toPolygon_py(ry2,l2,w2,x2,z2);

  std::vector<Polygon> in, un;
  intersection(gp, dp, in);
  union_(gp, dp, un);

  double inter_area = in.empty() ? 0 : area(in.front());
  double union_area = area(un.front());
  double o;
  if(criterion==-1)     // union
      o = inter_area / union_area;
  else if(criterion==0) // bbox_a
      o = inter_area / area(dp);
  else if(criterion==1) // bbox_b
      o = inter_area / area(gp);

  return o;
}

Polygon toPolygon(box a)
{
    using namespace boost::numeric::ublas;
    using namespace boost::geometry;
    matrix<double> mref(2, 2);
    mref(0, 0) = cos(a.ry); mref(0, 1) = sin(a.ry);
    mref(1, 0) = -sin(a.ry); mref(1, 1) = cos(a.ry);

    static int count = 0;
    matrix<double> corners(2, 4);
    double data[] = {a.l / 2, a.l / 2, -a.l / 2, -a.l / 2,
                     a.w / 2, -a.w / 2, -a.w / 2, a.w / 2};
    std::copy(data, data + 8, corners.data().begin());
    matrix<double> gc = prod(mref, corners);
    for (int i = 0; i < 4; ++i) {
        gc(0, i) += a.x;
        gc(1, i) += a.z;
    }

    double points[][2] = {{gc(0, 0), gc(1, 0)},{gc(0, 1), gc(1, 1)},{gc(0, 2), gc(1, 2)},{gc(0, 3), gc(1, 3)},{gc(0, 0), gc(1, 0)}};
    Polygon poly;
    append(poly, points);
    return poly;
}

float compute_iou_ground(box a,box b, int criterion = -1)
{
  using namespace boost::geometry;
  Polygon gp = toPolygon(a);
  Polygon dp = toPolygon(b);

  std::vector<Polygon> in, un;
  intersection(gp, dp, in);
  union_(gp, dp, un);

  double inter_area = in.empty() ? 0 : area(in.front());
  double union_area = area(un.front());
  double o;
  if(criterion==-1)     // union
      o = inter_area / union_area;
  else if(criterion==0) // bbox_a
      o = inter_area / area(dp);
  else if(criterion==1) // bbox_b
      o = inter_area / area(gp);

  return o;
}

float compute_iou_rect(box rectA,box rectB)
{
  float xa1 = max(max(max(rectA.wx0,rectA.wx1),rectA.wx2),rectA.wx3);
  float xa0 = min(min(min(rectA.wx0,rectA.wx1),rectA.wx2),rectA.wx3);
  float ya1 = max(max(max(rectA.wy0,rectA.wy1),rectA.wy2),rectA.wy3);
  float ya0 = min(min(min(rectA.wy0,rectA.wy1),rectA.wy2),rectA.wy3);

  float xb1 = max(max(max(rectB.wx0,rectB.wx1),rectB.wx2),rectB.wx3);
  float xb0 = min(min(min(rectB.wx0,rectB.wx1),rectB.wx2),rectB.wx3);
  float yb1 = max(max(max(rectB.wy0,rectB.wy1),rectB.wy2),rectB.wy3);
  float yb0 = min(min(min(rectB.wy0,rectB.wy1),rectB.wy2),rectB.wy3);

  if (xa0 > xb1) { return 0.; }
	if (ya0 > yb1) { return 0.; }
	if ((xa1) < xb0) { return 0.; }
	if ((ya1) < yb0) { return 0.; }
	float colInt = min(xa1, xb1) - max(xa0, xb0);
	float rowInt = min(ya1, yb1) - max(ya0, yb0);
	float intersection = colInt * rowInt;
	float areaA = (xa1-xa0) * (ya1-ya0);
	float areaB = (xb1-xb0) * (yb1-yb0);
	float intersectionPercent = intersection / (areaA + areaB - intersection);
	return intersectionPercent;
}

float sigmoid(float x)
{
  float s = 1.0 / (1.0 + exp(-x));
  return s;
}

void nms2(
        const std::vector<box>& srcRects,
        const std::vector<float>& scores,
        std::vector<box>& resRects,
        float thresh,
        int neighbors = 0,
        float minScoresSum = 0.f
        )
{
    resRects.clear();

    const size_t size = srcRects.size();
    if (!size)
    {
        return;
    }

    assert(srcRects.size() == scores.size());

    std::multimap<float, size_t> idxs;
    for (size_t i = 0; i < size; ++i)
    {
        idxs.insert(std::pair<float, size_t>(scores[i], i));
    }

    while (idxs.size() > 0)
    {
        auto lastElem = --std::end(idxs);
        box rect1 = srcRects[lastElem->second];

        int neigborsCount = 0;
        float score = lastElem->first;
        float scoresSum = lastElem->first;

        idxs.erase(lastElem);

        for (auto pos = std::begin(idxs); pos != std::end(idxs); )
        {
            box rect2 = srcRects[pos->second];
            float distance2 = (rect1.x-rect2.x)*(rect1.x-rect2.x)\
              +(rect1.z-rect2.z)*(rect1.z-rect2.z);
            if(distance2>15*15)
            {
              ++pos;
              continue;
            }

            float overlap=0;
            if((abs(rect1.ry)<70.0*3.14158/180 && abs(rect1.ry)>20.0*3.14158/180) || \
              (abs(rect2.ry)<70.0*3.14158/180 && abs(rect2.ry)>20.0*3.14158/180))
            {
              overlap  = compute_iou_ground(rect1,rect2);
            }
            {
              overlap  = compute_iou_rect(rect1,rect2);
            }
            if (overlap > thresh)
            {
                scoresSum += pos->first;
                pos = idxs.erase(pos);
                ++neigborsCount;
            }
            else
            {
                ++pos;
            }
        }
        if (neigborsCount >= neighbors &&
                scoresSum >= minScoresSum)
        {
            resRects.push_back(rect1);
        }
    }
}

py::array_t<float> cal_result(py::array_t<float> &feature_out,\
  float obj_th,float OVERLAP,float Z_MIN, int img_height,int img_width,float DX,float DY,float DZ,\
  float nms_th)
{

  auto feature_map = feature_out.unchecked<3>();

  int feature_height = img_height/8+0.5;
  int feature_width = img_width/8+0.5;
  int grid_height = img_height/feature_height+0.5;
  int grid_width = img_width/feature_width+0.5;

  std::vector<box> objs;
  std::vector<float> scores;
  objs.clear();
  scores.clear();

  float cut_dis = OVERLAP-Z_MIN;

  for(int height_i=0;height_i<feature_height;height_i++)
  {
    for(int width_i=0;width_i<feature_width;width_i++)
    {
      float is_obj = sigmoid(feature_map(height_i,width_i,0));

      float reg_dy = feature_map(height_i,width_i,13);
      reg_dy = reg_dy*grid_height;
      float center_y = height_i*grid_height+reg_dy;
      float m_y = (center_y*DY);
      float reg_dx = feature_map(height_i,width_i,11);
      reg_dx = reg_dx*grid_width;
      float center_x = width_i*grid_width+reg_dx;
      float m_x = (center_x-img_width/2)*DX;
      float sin_theta = feature_map(height_i,width_i,7);
      float cos_theta = feature_map(height_i,width_i,9);
      float theta = atan2(sin_theta,cos_theta)/2;
      float reg_ln_l = feature_map(height_i,width_i,17);
      float reg_l = exp(reg_ln_l);
      float reg_ln_h = feature_map(height_i,width_i,21);
      float reg_h = exp(reg_ln_h);
      if(m_y>cut_dis)
      {
        m_y=m_y-cut_dis-OVERLAP;
        if(m_y-reg_l/2<-10)
          is_obj*=0.2;
      }
      else
      {
        m_x*=-1;
        m_y=-1*(m_y-OVERLAP);
        if(m_y+reg_l/2>10)
          is_obj*=0.2;
      }

      if(m_y>100 && abs(theta)<45*3.14158/180)
      {
        is_obj*=0.2;
      }

      float m_obj_th = obj_th;
      if(m_y>100)
        m_obj_th=obj_th*0.5;
      if(m_y>150)
        m_obj_th=obj_th*0.4;
      if(m_y>180)
        m_obj_th=obj_th*0.3;

      if(is_obj>m_obj_th)
      {
        int cls_num=0;
        float is_cls0=feature_map(height_i,width_i,2);
        float is_cls1=feature_map(height_i,width_i,3);
        float is_cls2=feature_map(height_i,width_i,4);
        float reg_ln_w = feature_map(height_i,width_i,15);
        float reg_w = exp(reg_ln_w);
        float m_z = feature_map(height_i,width_i,19);
        if(is_cls0>is_cls1 && is_cls0>is_cls2)
        {
          cls_num = 0;
        }
        else if(is_cls1>is_cls0 && is_cls1>is_cls2)
        {
          cls_num = 1;
          if (is_obj<0.88 && m_y<100 && abs(m_x)<40 && abs(m_x)>5 && m_y>10)
            continue;
        }
        else
        {
          cls_num = 2;
          if (is_obj<0.88 && m_y<100 && abs(m_x)<40 && abs(m_x)>5 && m_y>10)
            continue;
        }
        box one_obj;
        one_obj.ry = theta;
        one_obj.l = reg_l;
        one_obj.w = reg_w;
        one_obj.x = m_x;
        one_obj.z = m_y;
        one_obj.cls_num=cls_num;
        one_obj.is_obj=is_obj;
        one_obj.h = reg_h;
        one_obj.y = m_z;
        objs.push_back(one_obj);

        scores.push_back(is_obj);

      }

      is_obj= sigmoid(feature_map(height_i,width_i,1));
      if(is_obj>obj_th)
      {
        int cls_num=0;
        float is_cls3=feature_map(height_i,width_i,5);
        float is_cls4=feature_map(height_i,width_i,6);
        if(is_cls3>is_cls4)
        {
          cls_num = 3;
        }
        else
        {
          cls_num = 4;
        }

        float sin_theta = feature_map(height_i,width_i,8);
        float cos_theta = feature_map(height_i,width_i,10);
        float reg_dx = feature_map(height_i,width_i,12);
        float reg_dy = feature_map(height_i,width_i,14);
        float reg_ln_w = feature_map(height_i,width_i,16);
        float reg_ln_l = feature_map(height_i,width_i,18);
        float m_z = feature_map(height_i,width_i,20);
        float reg_ln_h = feature_map(height_i,width_i,22);
        float theta = atan2(sin_theta,cos_theta)/2;
        reg_dx = reg_dx*grid_width;
        reg_dy = reg_dy*grid_height;
        float center_x = width_i*grid_width+reg_dx;
        float center_y = height_i*grid_height+reg_dy;
        float m_x = (center_x-img_width/2)*DX;
        float m_y = (center_y*DY);
        float reg_w = exp(reg_ln_w);
        float reg_l = exp(reg_ln_l);
        float reg_h = exp(reg_ln_h);

        if(m_y>cut_dis)
        {
          m_y=m_y-cut_dis-OVERLAP;
        }
        else
        {
          m_x*=-1;
          m_y=-1*(m_y-OVERLAP);
        }

        box one_obj;
        one_obj.ry = theta;
        one_obj.l = reg_l;
        one_obj.w = reg_w;
        one_obj.x = m_x;
        one_obj.z = m_y;
        one_obj.cls_num=cls_num;
        one_obj.is_obj=is_obj;
        one_obj.h = reg_h;
        one_obj.y = m_z;
        objs.push_back(one_obj);

        scores.push_back(is_obj);

      }
    }
  }

  std::vector<box> results;
  compute_4_points(objs);
  nms2(objs,scores,results,nms_th);

  int obj_num = results.size();

  auto result = py::array_t<float>(obj_num*9);
  result.resize({obj_num,9});
  py::buffer_info buf_result = result.request();
  float* ptr_result = (float*)buf_result.ptr;

  for(int i=0;i<obj_num;i++)
  {
    ptr_result[i*9 + 0] = results[i].is_obj;
    ptr_result[i*9 + 1] = results[i].cls_num;
    ptr_result[i*9 + 2] = results[i].ry;
    ptr_result[i*9 + 3] = results[i].l;
    ptr_result[i*9 + 4] = results[i].w;
    ptr_result[i*9 + 5] = results[i].x;
    ptr_result[i*9 + 6] = results[i].z;
    ptr_result[i*9 + 7] = results[i].h;
    ptr_result[i*9 + 8] = results[i].y;
  }

  return result;
}

PYBIND11_MODULE(lib_cpp, m)
{
    m.def("cal_result", &cal_result);
}

int32_t main() {

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// CollapsePreventionEnergy.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Sheet material energy density that prevents elements from collapsing into
//  degenerate configurations (which will break the bending energy...) with
//  an infinite energy barrier:
//      (-log((det(C) - activationThreshold) / activationThreshold + 1))^2
//  for det(C) < activationThreshold, 0 otherwise
//
//  This energy term is C1. We could make it C2 (to avoid the single point
//  where the Hessian is undefined) by raising the power from 2 to 3--at the
//  expense of a faster ramp-up (greater nonlinearity).
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  05/30/2019 17:23:18
////////////////////////////////////////////////////////////////////////////////
#ifndef COLLAPSEPREVENTIONENERGY_HH
#define COLLAPSEPREVENTIONENERGY_HH
#include <cmath>

#include <Eigen/Dense>
#include <MeshFEM/EnergyDensities/Tensor.hh>
#include "SVDSensitivity.hh"
#include "InflatableSheet.hh"

struct BarrierFuncLogSq {
    using Real = InflatableSheet::Real;

    constexpr static Real inf = std::numeric_limits<double>::infinity();

    static Real   b(Real x) { if (x <= 0) return inf; if (x >= 1.0) return 0.0; return 0.5 * std::pow(log(x), 2); }
    static Real  db(Real x) { if (x <= 0) return inf; if (x >= 1.0) return 0.0; return log(x) / x; }
    static Real d2b(Real x) { if (x <= 0) return inf; if (x >= 1.0) return 0.0; return (1 - log(x)) / (x * x); }
};

template<class BarrierFunc>
struct NormalizedBarrierFunction {
    using Real = typename BarrierFunc::Real;
    using BF = BarrierFunc;

    void setActivationThreshold(Real val) { m_a = val; }
    Real activationThreshold() const { return m_a; }

    Real   b(Real x) const { return BF::  b(x / m_a); }
    Real  db(Real x) const { return BF:: db(x / m_a) / m_a; }
    Real d2b(Real x) const { return BF::d2b(x / m_a) / (m_a * m_a); }

protected:
    Real m_a = 1.0;
};

template<class BarrierFunc>
struct CollapsePreventionDet : public NormalizedBarrierFunction<BarrierFunc> {
    using BF = NormalizedBarrierFunction<BarrierFunc>;
    using M2d  = InflatableSheet::M2d;
    using Real = InflatableSheet::Real;

    template<typename Derived>
    void setMatrix(const Eigen::MatrixBase<Derived> &C) {
        static_assert((Derived::RowsAtCompileTime == 2) && (Derived::ColsAtCompileTime == 2), "Only 2x2 supported for now");
        m_det = C.determinant();
        m_grad_det <<  C(1, 1), -C(1, 0),
                      -C(0, 1),  C(0, 0);
    }

    Real energy() const { return BF::b(m_det); }
    M2d denergy() const { return BF::db(m_det) * m_grad_det; }

    template<typename Derived>
    M2d delta_denergy(const Eigen::MatrixBase<Derived> &dC) const {
        static_assert((Derived::RowsAtCompileTime == 2) && (Derived::ColsAtCompileTime == 2), "Only 2x2 supported for now");

        M2d delta_grad_det;
        delta_grad_det <<  dC(1, 1), -dC(1, 0),
                          -dC(0, 1),  dC(0, 0);

        return ((BF::d2b(m_det) * doubleContract(m_grad_det, dC.template cast<Real>()))) * m_grad_det
               + BF:: db(m_det) * delta_grad_det;
    }

    // For debugging scalar function of det + its derivatives
    void setDet(Real det) { m_det = det; }
    Real det() const { return m_det; }
    Real normalizedDet()  const { return m_det / BF::m_a; }
    Real denergy_ddet()   const { return BF::db(m_det);  }
    Real d2energy_d2det() const { return BF::d2b(m_det); }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    Real m_det;
    M2d m_grad_det;
};

template<class BarrierFunc>
struct CollapsePreventionSingularValues : public NormalizedBarrierFunction<BarrierFunc> {
    using BFRaw = BarrierFunc;
    using BF    = NormalizedBarrierFunction<BarrierFunc>;
    using V2d   = InflatableSheet::V2d;
    using M2d   = InflatableSheet::M2d;
    using Real  = InflatableSheet::Real;

    // The activation threshold for the singular value barriers should be the
    // square root of the area barrier activation threshold.
    void setActivationThreshold(Real val) { BF::setActivationThreshold(std::sqrt(val)); }
    Real activationThreshold() const { return std::pow(BF::activationThreshold(), 2); }

    template<typename Derived>
    void setMatrix(const Eigen::MatrixBase<Derived> &F) { m_svd.setMatrix(F); m_det = F.determinant(); }

    Real energy() const {
        if (m_det < 0.0) return std::numeric_limits<double>::infinity();
        Real result = BF::b(m_svd.sigma(0)) + BF::b(m_svd.sigma(1));
        if (applyStretchBarrier) {
            Real scale = 1.0 / (stretchBarrierLimit - stretchBarrierActivation);
            result += BFRaw::b(scale * (stretchBarrierLimit - m_svd.sigma(0)))
                   +  BFRaw::b(scale * (stretchBarrierLimit - m_svd.sigma(1)));
        }
        return result;
    }

    M2d denergy() const {
        if (m_det < 0.0) {
            M2d result;
            result.setConstant(std::numeric_limits<double>::infinity());
        }
        V2d dE_dsigma(BF::db(m_svd.sigma(0)),
                      BF::db(m_svd.sigma(1)));
        if (applyStretchBarrier) {
            Real scale = 1.0 / (stretchBarrierLimit - stretchBarrierActivation);
            dE_dsigma -= scale * V2d(BFRaw::db(scale * (stretchBarrierLimit - m_svd.sigma(0))),
                                     BFRaw::db(scale * (stretchBarrierLimit - m_svd.sigma(1))));
        }

        return m_svd.U() * (dE_dsigma.asDiagonal() * m_svd.V().transpose());
    }

    const SVDSensitivity &svd() const { return m_svd; }
    Real det() const { return m_det; }

    // Second derivatives blow up when sigma_0 == sigma_1!!!
    template<typename Derived>
    M2d delta_denergy(const Eigen::MatrixBase<Derived> &/* dF */) const {
        static_assert((Derived::RowsAtCompileTime == 2) && (Derived::ColsAtCompileTime == 2), "Only 2x2 supported for now");
        throw std::runtime_error("Second derivative of SVD collapse prevention unsupported; will blow up at sigma_0 == sigma_2.");
    #if 0
        // SVDSensitivity doesn't yet implement delta_dsigma...
        return (BF::d2b(m_svd.sigma(0)) * m_svd.dsigma(0, dF)) * m_svd.dsigma(0) +
               (BF::d2b(m_svd.sigma(1)) * m_svd.dsigma(1, dF)) * m_svd.dsigma(1) +
                BF:: db(m_svd.sigma(0)) * m_svd.delta_dsigma(0, dF) +
                BF:: db(m_svd.sigma(1)) * m_svd.delta_dsigma(1, dF);
    #endif
    }

    bool applyStretchBarrier = false;
    Real stretchBarrierActivation = 1.75; // threshold below which barrier term is smoothly deactivated
    Real stretchBarrierLimit      = 2.25; // placement of the infinite barrier

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    SVDSensitivity m_svd;
    Real m_det;
};

using CollapsePreventionEnergyDet = CollapsePreventionDet<BarrierFuncLogSq>;
using CollapsePreventionEnergySV  = CollapsePreventionSingularValues<BarrierFuncLogSq>;

#endif /* end of include guard: COLLAPSEPREVENTIONENERGY_HH */


// Including SDKDDKVer.h defines the highest available Windows platform.
// If you wish to build your application for a previous Windows platform, include WinSDKVer.h and
// set the _WIN32_WINNT macro to the platform you wish to support before including SDKDDKVer.h.
#include <SDKDDKVer.h>

#include "CppUnitTest.h"

using namespace Microsoft::VisualStudio::CppUnitTestFramework;

#include "ComputeRigidTransform.h"
#include <Eigen/Dense>
#include <time.h>
#include <vector>

namespace TestComputeRigidTransform
{
	TEST_CLASS(UnitTestRigidTransform)
	{
	public:

		TEST_METHOD(TestNoTransform)
		{
			typedef double Type;
			srand(time(NULL));

			std::vector<Eigen::Matrix<Type, 3, 1>> vertices;

			for (int i = 0; i < 100; ++i)
			{
				const Eigen::Matrix<Type, 3, 1> v(rand(), rand(), rand());
				vertices.push_back(v);
			}

			Eigen::Matrix<Type, 3, 3> R = Eigen::Matrix<Type, 3, 3>::Zero();
			Eigen::Matrix<Type, 3, 1> t = Eigen::Matrix<Type, 3, 1>::Zero();
			ComputeRigidTransform(vertices, vertices, R, t);

			Assert::IsTrue(R.isIdentity(0.00001), L"\n<Rotation matrix is not identity>\n", LINE_INFO());
			Assert::IsTrue(t.isZero(0.00001), L"\n<Translation vector is not zero>\n", LINE_INFO());
		}

	};

}