/*
 *
 * Copyright (c) 2014, Laurens van der Maaten (Delft University of Technology)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *    This product includes software developed by the Delft University of Technology.
 * 4. Neither the name of the Delft University of Technology nor the names of
 *    its contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY LAURENS VAN DER MAATEN ''AS IS'' AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
 * EVENT SHALL LAURENS VAN DER MAATEN BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
 * OF SUCH DAMAGE.
 *
 */

#include <cfloat>
#include <cmath>
#include <cstdlib>
#include <cstdio>
#include <cstring>
#include <ctime>
#include <chrono>
#include "vptree.h"
#include "sptree.h"
#include "bhtsne.h"
#include <iostream>
#include <map>
#include <vector>

#include <string>
#include <armadillo>

using namespace std;
using namespace arma;

// Fisher-Yates shuffle
static void randperm(ivec &selected, int N, int K) {
  ivec tmp(N);
  for (int i = 0; i < N; i++) {
    tmp(i) = i;
  }

  for (int i = 0; i <= min(N - 2, K); i++) {
    ivec r = randi(1, distr_param(i + 1, N - 1));

    // Swap
    int val = tmp(i);
    tmp(i) = tmp(r(0));
    tmp(r(0)) = val;
  }

  selected.set_size(K);
  for (int i = 0; i < K; i++) {
    selected(i) = tmp(i % N);
  }
}

static bool load_data(string infile, double **data, int &num_instances, int &num_features) {

  FILE *fp = fopen(infile.c_str(), "rb");
	if (fp == NULL) {
		cout << "Error: could not open data file " << infile << endl;
		return false;
	}

  uint64_t ret;
	ret = fread(&num_instances, sizeof(int), 1, fp);
	ret = fread(&num_features, sizeof(int), 1, fp);

	*data = (double *)malloc(num_instances * num_features * sizeof(double));
  if (*data == NULL) {
    cout << "Error: memory allocation of " << num_instances << " by "
         << num_features << " matrix failed" << endl;
    return false;
  }

  uint64_t nelem = (uint64_t)num_instances * num_features;

  size_t batch_size = 1e8;
  double *ptr = *data;
  ret = 0;
  for (uint64_t remaining = nelem; remaining > 0; remaining -= batch_size) {
    if (remaining < batch_size) {
      batch_size = remaining;
    }
    ret += fread(ptr, sizeof(double), batch_size, fp);
    ptr += batch_size;
  }

  if (ret != nelem) {
    cout << "Error: reading input returned incorrect number of elements (" << ret
         << ", expected " << nelem << ")" << endl;
    return false;
  }

	fclose(fp);

	return true;
}

static void truncate_data(double *X, int num_instances, int num_features, int target_dims) {
  size_t i_old = 0;
  size_t i_new = 0;
  for (int r = 0; r < num_instances; r++) {
    for (int c = 0; c < num_features; c++) {
      if (c < target_dims) {
        X[i_new++] = X[i_old];
      }
      i_old++;
    }
  }
}

// Perform t-SNE
bool TSNE::run(int N, unsigned int *row_P, unsigned int *col_P, double *val_P,
               double* Y, int no_dims, double theta, int rand_seed,
               bool skip_random_init, int max_iter, int stop_lying_iter, int mom_switch_iter,
               double momentum, double final_momentum, double eta, boost::filesystem::path outdir) {

  // Set random seed
  if (!skip_random_init) {
    if (rand_seed >= 0) {
      printf("Using random seed: %d\n", rand_seed);
      srand((unsigned int)rand_seed);
    } else {
      printf("Using current time as random seed...\n");
      srand(time(NULL));
    }
  }

  printf("Using no_dims = %d and theta = %f\n", no_dims, theta);

  float total_time = .0;
  clock_t start, end;
  auto t_start = chrono::high_resolution_clock::now();
  auto t_end = chrono::high_resolution_clock::now();
  boost::filesystem::path newfile;

  // Allocate some memory
  double *dY    = (double *)malloc(N * no_dims * sizeof(double));
  double *uY    = (double *)malloc(N * no_dims * sizeof(double));
  double *gains = (double *)malloc(N * no_dims * sizeof(double));
  if (dY == NULL || uY == NULL || gains == NULL) {
    printf("Memory allocation failed!\n");
    return false;
  }
  for (int i = 0; i < N * no_dims; i++) {
    uY[i] = 0;
    gains[i] = 1;
  }

  if (!skip_random_init) {
    for (int i = 0; i < N * no_dims; i++) {
      Y[i] = randn() * 0.0001;
    }
  }

	// Perform main training loop
  printf("Learning embedding...\n");
  start = clock();
  t_start = chrono::high_resolution_clock::now();

  newfile = outdir;
  newfile /= "log.txt";
  ofstream logfs(newfile.c_str());
  logfs << "iter\tobjective\telapsed-time" << endl;

  double C0 = evaluateError(row_P, col_P, val_P, Y, N, no_dims, theta); // doing approximate computation here!
  iters.push_back(0);
  objectives.push_back(C0);
  elapsed_times.push_back(0);
  printf("Iteration 0: error is %f\n", C0);

  logfs << iters[0] << "\t" << objectives[0] << "\t" << elapsed_times[0] << endl;
  logfs.flush();

  int batch_N;

  if (BATCH_FLAG) {
    batch_N = (int)(N * BATCH_FRAC);
    cout << "batch N: " << batch_N << endl;
  }

	for(int iter = 0; iter < max_iter; iter++) {

    // Compute (approximate) gradient
    computeGradient(row_P, col_P, val_P, Y, N, no_dims, dY, theta, iter < stop_lying_iter);

    if (BATCH_FLAG) {

      ivec selected;
      randperm(selected, N, batch_N);

      // Perform gradient update (with momentum)
      for (int i = 0; i < batch_N; i++) {
        int ind = selected(i) * no_dims;
        for (int d = 0; d < no_dims; d++) {
          uY[ind + d] = momentum * uY[ind + d] - eta * dY[ind + d];
           Y[ind + d] += uY[ind + d];
        }
      }

    } else {

      // Update gains
      for (int i = 0; i < N * no_dims; i++) gains[i] = (sign(dY[i]) != sign(uY[i])) ? (gains[i] + .2) : (gains[i] * .8);
      for (int i = 0; i < N * no_dims; i++) if (gains[i] < .01) gains[i] = .01;

      // Perform gradient update (with momentum and gains)
      for (int i = 0; i < N * no_dims; i++) uY[i] = momentum * uY[i] - eta * gains[i] * dY[i];
		  for (int i = 0; i < N * no_dims; i++)  Y[i] = Y[i] + uY[i];

    }

    // Make solution zero-mean
		zeroMean(Y, N, no_dims);

    if (iter == mom_switch_iter) {
      momentum = final_momentum;
    }

    // Print out progress
    if (iter > 0 && ((iter + 1) % 50 == 0 || iter == max_iter - 1)) {
      end = clock();
      t_end = chrono::high_resolution_clock::now();

      double C = evaluateError(row_P, col_P, val_P, Y, N, no_dims, theta);
      float elapsed = chrono::duration<float, std::milli>(t_end - t_start).count() / 1000.0;

      total_time += elapsed;
      printf("Iteration %d: error is %f (50 iterations in %4.2f seconds)\n",
             iter + 1, C, (float)(end - start) / CLOCKS_PER_SEC);

      iters.push_back(iter + 1);
      objectives.push_back(C);
      elapsed_times.push_back((double)total_time);

      logfs << iters.back() << "\t" << objectives.back() << "\t" << elapsed_times.back() << endl;
      logfs.flush();

      if ((iter + 1) % CACHE_ITER == 0 || iter == max_iter - 1) {
        string filename;
        if (iter == max_iter - 1) {
          filename = "Y_final.txt";
        } else {
          filename = string("Y_") + to_string(iter + 1) + string(".txt");
        }
        newfile = outdir;
        newfile /= filename;

        //FILE *fp = fopen(newfile.string().c_str(), "wb");
        //fwrite(Y, sizeof(double), no_dims * N, fp);
        //fclose(fp);

        mat Y_tmp(Y, no_dims, N, true); // convert Y to armadillo matrix
        Y_tmp = Y_tmp.t();
        Y_tmp.save(newfile.string(), arma_ascii);
      }

		  start = clock();
      t_start = chrono::high_resolution_clock::now();
    }
  }

  logfs.close();

  free(dY);
  free(uY);
  free(gains);

  printf("Fitting performed in %4.2f seconds.\n", total_time);

  return true;
}

// Compute gradient of the t-SNE cost function (using Barnes-Hut algorithm)
void TSNE::computeGradient(unsigned int* inp_row_P, unsigned int* inp_col_P, double* inp_val_P, double* Y, int N, int D, double* dC, double theta, bool early_exaggeration)
{

    // Construct space-partitioning tree on current map
    SPTree* tree = new SPTree(D, Y, N);

    // Compute all terms required for t-SNE gradient
    double sum_Q = .0;
    double* pos_f = (double*) calloc(N * D, sizeof(double));
    double* neg_f = (double*) calloc(N * D, sizeof(double));
    if(pos_f == NULL || neg_f == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    tree->computeEdgeForces(inp_row_P, inp_col_P, inp_val_P, N, pos_f);
    for(int n = 0; n < N; n++) tree->computeNonEdgeForces(n, theta, neg_f + n * D, &sum_Q);

    // Compute final t-SNE gradient
    for(int i = 0; i < N * D; i++) {
        if (early_exaggeration) {
          dC[i] = 12.0 * pos_f[i] - (neg_f[i] / sum_Q);
        } else {
          dC[i] = pos_f[i] - (neg_f[i] / sum_Q);
        }
    }
    free(pos_f);
    free(neg_f);
    delete tree;
}

// Evaluate t-SNE cost function (approximately)
double TSNE::evaluateError(unsigned int* row_P, unsigned int* col_P, double* val_P, double* Y, int N, int D, double theta)
{

    // Get estimate of normalization term
    SPTree* tree = new SPTree(D, Y, N);
    double* buff = (double*) calloc(D, sizeof(double));
    double sum_Q = .0;
    for(int n = 0; n < N; n++) tree->computeNonEdgeForces(n, theta, buff, &sum_Q);

    // Loop over all edges to compute t-SNE error
    int ind1, ind2;
    double C = .0, Q;
    for(int n = 0; n < N; n++) {
        ind1 = n * D;
        for(int i = row_P[n]; i < row_P[n + 1]; i++) {
            Q = .0;
            ind2 = col_P[i] * D;
            for(int d = 0; d < D; d++) buff[d]  = Y[ind1 + d];
            for(int d = 0; d < D; d++) buff[d] -= Y[ind2 + d];
            for(int d = 0; d < D; d++) Q += buff[d] * buff[d];
            Q = (1.0 / (1.0 + Q)) / sum_Q;
            C += val_P[i] * log((val_P[i] + FLT_MIN) / (Q + FLT_MIN));
        }
    }

    // Clean up memory
    free(buff);
    delete tree;
    return C;
}

// Makes data zero-mean
void TSNE::zeroMean(double* X, int N, int D) {

	// Compute data mean
	double* mean = (double*) calloc(D, sizeof(double));
    if(mean == NULL) { printf("Memory allocation failed!\n"); exit(1); }
    int nD = 0;
	for(int n = 0; n < N; n++) {
		for(int d = 0; d < D; d++) {
			mean[d] += X[nD + d];
		}
        nD += D;
	}
	for(int d = 0; d < D; d++) {
		mean[d] /= (double) N;
	}

	// Subtract data mean
    nD = 0;
	for(int n = 0; n < N; n++) {
		for(int d = 0; d < D; d++) {
			X[nD + d] -= mean[d];
		}
        nD += D;
	}
    free(mean); mean = NULL;
}

// Generates a Gaussian random number
double TSNE::randn() {
	double x, y, radius;
	do {
		x = 2 * (rand() / ((double) RAND_MAX + 1)) - 1;
		y = 2 * (rand() / ((double) RAND_MAX + 1)) - 1;
		radius = (x * x) + (y * y);
	} while((radius >= 1.0) || (radius == 0.0));
	radius = sqrt(-2 * log(radius) / radius);
	x *= radius;
	y *= radius;
	return x;
}

bool TSNE::load_P(string infile, int &N, unsigned int** row_P, unsigned int** col_P, double** val_P) {
  FILE *h;
	if((h = fopen(infile.c_str(), "rb")) == NULL) {
    return false;
	}

  size_t ret = 0;
  ret += fread(&N, sizeof(int), 1, h);
  *row_P = (unsigned int*)malloc((N+1) * sizeof(unsigned int));
  if (*row_P == NULL) {
    printf("Memory allocation error\n");
    exit(1);
  }
  ret += fread(*row_P, sizeof(unsigned int), N+1, h);
  *col_P = (unsigned int*)malloc((*row_P)[N] * sizeof(unsigned int));
  *val_P = (double*)malloc((*row_P)[N] * sizeof(double));
  if (*col_P == NULL || *val_P == NULL) {
    printf("Memory allocation error\n");
    exit(1);
  }
  ret += fread(*col_P, sizeof(unsigned int), (*row_P)[N], h);
  ret += fread(*val_P, sizeof(double), (*row_P)[N], h);
  fclose(h);

  printf("P successfully loaded\n");
  return true;
}


/**
 * @file
 * @brief Observation of a Finger robot.
 * @copyright 2020, Max Planck Gesellschaft. All rights reserved.
 * @license BSD 3-clause
 */
#pragma once

#include <string>
#include <vector>

#include <Eigen/Eigen>
#include <serialization_utils/cereal_eigen.hpp>

#include <robot_interfaces/loggable.hpp>

namespace robot_interfaces
{
/**
 * @brief Observation of a Finger robot.
 *
 *
 * Values like angular position, velocity or torque of the joints are
 * represented as a 1-dimensional vector with one element per joint.  The order
 * of the joints in these vectors is as follows:
 *
 *     0. Finger 1, upper joint
 *     1. Finger 1, middle joint
 *     2. Finger 1, lower joint
 *     3. Finger 2, upper joint
 *     4. Finger 2, middle joint
 *     5. Finger 2, lower joint
 *     ...
 *     #. Finger n, upper joint
 *     #. Finger n, middle joint
 *     #. Finger n, lower joint
 *
 * @tparam N_FINGERS  Number of fingers.
 */
template <size_t N_FINGERS>
struct NFingerObservation : public Loggable
{
    static constexpr size_t num_fingers = N_FINGERS;
    static constexpr size_t num_joints = N_FINGERS * 3;

    typedef Eigen::Matrix<double, num_joints, 1> JointVector;
    typedef Eigen::Matrix<double, num_fingers, 1> FingerVector;

    //! @brief Measured angular position of all joints in radian.
    JointVector position = JointVector::Zero();

    //! @brief Measured velocity of all joints in radian/second.
    JointVector velocity = JointVector::Zero();

    //! @brief Measured torques of all joints in Nm.
    JointVector torque = JointVector::Zero();

    /**
     * @brief Measurements of the pressure sensors at the finger tips.
     *
     * One per finger.  Ranges between 0 and 1 without specific unit.  Note that
     * not all fingers are equipped with an actual sensor!  For fingers without
     * sensor, this value is undefined.
     */
    FingerVector tip_force = FingerVector::Zero();

    template <class Archive>
    void serialize(Archive& archive)
    {
        archive(position, velocity, torque, tip_force);
    }

    std::vector<std::string> get_name() override
    {
        return {"position", "velocity", "torque", "tip_force"};
    }

    std::vector<std::vector<double>> get_data() override
    {
        typedef std::vector<double> vecd;

        std::vector<vecd> result = {vecd(position.size()),
                                    vecd(velocity.size()),
                                    vecd(torque.size()),
                                    vecd(tip_force.size())};

        JointVector::Map(&result[0][0], position.size()) = position;
        JointVector::Map(&result[1][0], velocity.size()) = velocity;
        JointVector::Map(&result[2][0], torque.size()) = torque;
        FingerVector::Map(&result[3][0], tip_force.size()) = tip_force;

        return result;
    }
};
}  // namespace robot_interfaces


/*
 * Copyright 2021 INRIA
 */

#include <Eigen/Core>
#include <eigenpy/eigenpy.hpp>

#include "pycppad/cppad.hpp"
#include "pycppad/ad.hpp"
#include "pycppad/independent.hpp"

BOOST_PYTHON_MODULE(pycppad)
{
  enum { Options = 0 };

  eigenpy::enableEigenPy();
  pycppad::enablePyCppAD();
};


//=======================================================================
// Copyright (c) 2015 Piotr Wygocki
//
// 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 "paal/lp/glp.hpp"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(lp_compilation_error)

//This macro helps checking that particular expression does not compile.
//Exactly one from EXPR1 and EXPR2 should compile
//one exactly one shouldn't.
//
#define CHECK_COMP_ERROR(UNIQUE_HELPER_FUN_NAME, EXPR1, EXPR2) \
template <typename Lp, typename Expr, typename Expr1, typename Expr2, typename XX, typename YY, typename Row> \
auto UNIQUE_HELPER_FUN_NAME(Lp & lp, Expr & expr, Expr1 & expr1, Expr2 & expr2, XX & X, YY & Y, Row & row) -> decltype(EXPR1) { return EXPR1; }\
\
template <typename Lp, typename Expr, typename Expr1, typename Expr2, typename XX, typename YY, typename Row> \
auto UNIQUE_HELPER_FUN_NAME(Lp & lp, Expr & expr, Expr1 & expr1, Expr2 & expr2, XX & X, YY & Y, Row & row) -> decltype(EXPR2) { return EXPR2; }\
\
BOOST_AUTO_TEST_CASE(UNIQUE_HELPER_FUN_NAME##_test) { \
    paal::lp::glp lp;\
    paal::lp::linear_expression expr, expr1, expr2;\
    auto X = lp.add_column();\
    auto Y = lp.add_column();\
    auto row = lp.add_row(X);\
    UNIQUE_HELPER_FUN_NAME(lp, expr, expr1, expr2, X, Y, row); \
}

CHECK_COMP_ERROR(F1, expr / X, expr + X)
CHECK_COMP_ERROR(F2, lp.add_row(1 == row), lp.add_row(1 == X))
CHECK_COMP_ERROR(F3, lp.add_row(1 >= row), lp.add_row(1 >= X))
CHECK_COMP_ERROR(F4, lp.add_row(1 <= row), lp.add_row(1 <= X))
CHECK_COMP_ERROR(F5, expr = 2 / expr1, expr = 2 * expr1)
CHECK_COMP_ERROR(F6, expr = 2 / X, expr = 2 * X)
CHECK_COMP_ERROR(F7, lp.add_row(2 >= (expr <= 1)), lp.add_row(2 >= (expr >= 1)))
CHECK_COMP_ERROR(F8, lp.add_row(2 <= (expr >= 1)), lp.add_row(2 <= (expr <= 1)))
CHECK_COMP_ERROR(F9, expr = 2 - X, expr = 2 * X)
CHECK_COMP_ERROR(F10, expr = 2 * row, expr = 2 * X)
CHECK_COMP_ERROR(F11, expr = 2 + X, expr = 2 * X)
CHECK_COMP_ERROR(F12, lp.add_row(3 >= expr >= 2 * Y), lp.add_row(3 >= expr))
CHECK_COMP_ERROR(F13, lp.add_row(3 >= expr >= 2 * Y), (lp.add_row(3 >= expr), lp.add_row(expr - 2 * Y >= 0)))
CHECK_COMP_ERROR(F14, lp.add_row(6 == expr >= 5), lp.add_row(6 >= expr >= 5))
CHECK_COMP_ERROR(F15, lp.add_row(6 == expr <= 7), lp.add_row(6 <= expr <= 7))
CHECK_COMP_ERROR(F16, lp.add_row(6 >= expr == 5), lp.add_row(6 >= expr >= 5))
CHECK_COMP_ERROR(F17, lp.add_row(6 >= expr <= 5), lp.add_row(6 >= expr >= 5))
CHECK_COMP_ERROR(F18, lp.add_row(6 <= expr == 7), lp.add_row(6 <= expr <= 7))
CHECK_COMP_ERROR(F19, lp.add_row(6 <= expr >= 5), lp.add_row(6 >= expr >= 5))
CHECK_COMP_ERROR(F20, expr = row, expr = X)
CHECK_COMP_ERROR(F21, expr /= expr1, expr -= expr1)
CHECK_COMP_ERROR(F22, expr /= X, expr /= 2)
CHECK_COMP_ERROR(F23, expr = expr1 / expr2, expr = expr1 + expr2)
CHECK_COMP_ERROR(F24, lp.add_row(expr == expr1), lp.add_row(expr - expr1 == 0))
CHECK_COMP_ERROR(F25, lp.add_row(expr >= 5 >= 6), lp.add_row(6 >= expr >= 5))
CHECK_COMP_ERROR(F26, lp.add_row(expr >= 5 <= 6), lp.add_row(6 >= expr >= 5))
CHECK_COMP_ERROR(F27, lp.add_row(expr >= expr1), lp.add_row(expr - expr1 >= 0))
CHECK_COMP_ERROR(F28, lp.add_row(expr <= 5 <= 6), lp.add_row(5 <= expr <= 6))
CHECK_COMP_ERROR(F29, lp.add_row(expr <= 6 >= 5), lp.add_row(5 <= expr <= 6))
CHECK_COMP_ERROR(F30, lp.add_row(expr <= expr1), lp.add_row(expr - expr1 <= 0))
CHECK_COMP_ERROR(F31, expr = expr1 * expr2, expr = expr1 + expr2)
CHECK_COMP_ERROR(F32, expr = expr1 * X, expr = expr1 + X)
CHECK_COMP_ERROR(F33, expr -= 1, expr -= X)
CHECK_COMP_ERROR(F34, expr -= row, expr -= X)
CHECK_COMP_ERROR(F35, expr *= expr1, expr += expr1)
CHECK_COMP_ERROR(F36, expr *= X,  expr *= 2)
CHECK_COMP_ERROR(F37, expr += 1, expr += X)
CHECK_COMP_ERROR(F38, expr += row, expr += X)
CHECK_COMP_ERROR(F39, lp.add_row(row == 1), lp.add_row(X == 1))
CHECK_COMP_ERROR(F40, lp.add_row(row >= 1), lp.add_row(X >= 1))
CHECK_COMP_ERROR(F41, lp.add_row(row <= 1), lp.add_row(X <= 1))
CHECK_COMP_ERROR(F42, expr = +X, expr = -X)
CHECK_COMP_ERROR(F43, expr = X / expr1, expr = X - expr1)
CHECK_COMP_ERROR(F44, expr = X / Y,  expr = X + Y)
CHECK_COMP_ERROR(F45, expr = X - 2, expr = X * 2)
CHECK_COMP_ERROR(F46, expr = X * expr1, expr = X + expr1)
CHECK_COMP_ERROR(F47, expr = X * X, expr = X + X)
CHECK_COMP_ERROR(F48, expr = X * Y, expr = X + Y)
CHECK_COMP_ERROR(F49, expr = X + 2, expr = X * 2)
CHECK_COMP_ERROR(F50, expr = X + row, expr = X + Y)

BOOST_AUTO_TEST_SUITE_END()


#ifndef ASLAM_BACKEND_SIMPLE_REPROJECTION_ERROR_HPP
#define ASLAM_BACKEND_SIMPLE_REPROJECTION_ERROR_HPP

#include <aslam/backend/ErrorTerm.hpp>
#include <aslam/backend/HomogeneousExpression.hpp>
#include <boost/shared_ptr.hpp>

namespace aslam {
namespace backend {

template <typename FRAME_T>
class SimpleReprojectionError : public ErrorTermFs<FRAME_T::KeypointDimension> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef FRAME_T frame_t;
    typedef typename frame_t::keypoint_t keypoint_t;
    typedef typename frame_t::camera_geometry_t camera_geometry_t;
    enum {
        // The dimension of the keypoint associated with this geometry policy
        KeypointDimension = frame_t::KeypointDimension
    };

    typedef Eigen::Matrix<double, KeypointDimension, 1> measurement_t;
    typedef Eigen::Matrix<double, KeypointDimension, KeypointDimension> inverse_covariance_t;
    typedef ErrorTermFs<KeypointDimension> parent_t;

    SimpleReprojectionError();

    /**
     * @brief Construct with frame and target point, the measurement and covariance matrix will be extracted from frame
     * by index
     * @param frame         Frame, include the measurement
     * @param keypointIndex Measurement point index in frame
     * @param point         Target point in camera frame, expressed in homogeneous coordinates
     */
    SimpleReprojectionError(const frame_t* frame, int keypointIndex, const HomogeneousExpression& point);

    /**
     * @brief Construct with measurement, target point and covariance matrix
     * @param y         Measurement, the point(corner) extract from image
     * @param invR      Point covariance matrix
     * @param point     Target point in camera frame, expressed in homogeneous coordinates
     * @param geometry  Camera geometry
     */
    SimpleReprojectionError(const measurement_t& y, const inverse_covariance_t& invR,
                            const HomogeneousExpression& point, const camera_geometry_t& geometry);

    virtual ~SimpleReprojectionError() = default;

  protected:
    /**
     * @brief Evaluate the error term
     * @return Error
     */
    virtual double evaluateErrorImplementation();

    /**
     * @brief Evaluate the jacobian
     * @param _jacobians  Jacobians
     */
    virtual void evaluateJacobiansImplementation(aslam::backend::JacobianContainer& _jacobians) const;

  protected:
    measurement_t _y;                    // measurement, the point in the frame(image)
    const camera_geometry_t* _geometry;  // camera geometry
    HomogeneousExpression _point;        // the homogeneous target point expressed in the camera frame
};

}  // namespace backend
}  // namespace aslam

#include "implementation/SimpleReprojectionError.hpp"

#endif /* ASLAM_BACKEND_SIMPLE_REPROJECTION_ERROR_HPP */


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  Permission to use, copy, modify, distribute and sell this software
//  and its documentation for any purpose is hereby granted without fee,
//  provided that the above copyright notice appear in all copies and
//  that both that copyright notice and this permission notice appear
//  in supporting documentation.  The authors make no representations
//  about the suitability of this software for any purpose.
//  It is provided "as is" without express or implied warranty.
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifdef BOOST_MSVC

#pragma warning (disable: 4355)
#pragma warning (disable: 4503)
#pragma warning (disable: 4786)

#endif

#include <iostream>

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

#include <boost/numeric/ublas/blas.hpp>

#include "test2.hpp"

#ifdef BOOST_MSVC
// Standard new handler is not standard compliant.
#include <new.h>
int __cdecl std_new_handler (unsigned) {
    throw std::bad_alloc ();
}
#endif

int main () {
#ifdef BOOST_MSVC
    _set_new_handler (std_new_handler);
#endif

    std::cout << "test_blas_1" << std::endl;

#ifdef USE_FLOAT
    std::cout << "float" << std::endl;
    test_blas_1<ublas::vector<float>, 3> () ();
#endif

#ifdef USE_DOUBLE
    std::cout << "double" << std::endl;
    test_blas_1<ublas::vector<double>, 3> () ();
#endif

#ifdef USE_STD_COMPLEX
#ifdef USE_FLOAT
    std::cout << "std::complex<float>" << std::endl;
    test_blas_1<ublas::vector<std::complex<float> >, 3> () ();
#endif

#ifdef USE_DOUBLE
    std::cout << "std::complex<double>" << std::endl;
    test_blas_1<ublas::vector<std::complex<double> >, 3> () ();
#endif
#endif

    std::cout << "test_blas_2" << std::endl;

#ifdef USE_FLOAT
    std::cout << "float" << std::endl;
    test_blas_2<ublas::vector<float>, ublas::matrix<float>, 3> () ();
#endif

#ifdef USE_DOUBLE
    std::cout << "double" << std::endl;
    test_blas_2<ublas::vector<double>, ublas::matrix<double>, 3> () ();
#endif

#ifdef USE_STD_COMPLEX
#ifdef USE_FLOAT
    std::cout << "std::complex<float>" << std::endl;
    test_blas_2<ublas::vector<std::complex<float> >, ublas::matrix<std::complex<float> >, 3> () ();
#endif

#ifdef USE_DOUBLE
    std::cout << "std::complex<double>" << std::endl;
    test_blas_2<ublas::vector<std::complex<double> >, ublas::matrix<std::complex<double> >, 3> () ();
#endif
#endif

    std::cout << "test_blas_3" << std::endl;

#ifdef USE_FLOAT
    std::cout << "float" << std::endl;
    test_blas_3<ublas::matrix<float>, 3> () ();
#endif

#ifdef USE_DOUBLE
    std::cout << "double" << std::endl;
    test_blas_3<ublas::matrix<double>, 3> () ();
#endif

#ifdef USE_STD_COMPLEX
#ifdef USE_FLOAT
    std::cout << "std::complex<float>" << std::endl;
    test_blas_3<ublas::matrix<std::complex<float> >, 3> () ();
#endif

#ifdef USE_DOUBLE
    std::cout << "std::complex<double>" << std::endl;
    test_blas_3<ublas::matrix<std::complex<double> >, 3> () ();
#endif
#endif

    return 0;
}


#include <boost/test/unit_test.hpp>
#include "algorithms/math/hamming_weight.hpp"

BOOST_AUTO_TEST_SUITE(TestHammingWeight)

BOOST_AUTO_TEST_CASE(str_empty_string)
{
    HammingWeight::Solution solution;
    BOOST_CHECK(0 == solution.HammingWeightForString(std::string()));
}

BOOST_AUTO_TEST_CASE(str_non_empty_string)
{
    HammingWeight::Solution solution;
    BOOST_CHECK(1 == solution.HammingWeightForString("1"));
    BOOST_CHECK(1 == solution.HammingWeightForString("0"));
    BOOST_CHECK(1 == solution.HammingWeightForString("y"));
    BOOST_CHECK(1 == solution.HammingWeightForString("!"));
    BOOST_CHECK(7 == solution.HammingWeightForString("rty1:,."));
}

BOOST_AUTO_TEST_CASE(str_zero_string)
{
    HammingWeight::Solution solution;
    BOOST_CHECK(0 == solution.HammingWeightForString(std::string(2, char(0))));
}

BOOST_AUTO_TEST_CASE(int_null)
{
    HammingWeight::Solution solution;
    BOOST_CHECK(0 == solution.HammingWeightForInt(0));
}

BOOST_AUTO_TEST_CASE(int_diff_values)
{
    HammingWeight::Solution solution;
    BOOST_CHECK(1 == solution.HammingWeightForInt(1));
    BOOST_CHECK(1 == solution.HammingWeightForInt(2));
    BOOST_CHECK(5 == solution.HammingWeightForInt(143));
    BOOST_CHECK(28 == solution.HammingWeightForInt(-143));
}

BOOST_AUTO_TEST_SUITE_END()