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

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

#define DADEPT_FLOATING_POINT_TYPE float
#include <adept_source.h>
#include <adept_arrays.h>
using adept::adouble;
using adept::aMatrix;
using adept::aVector;

using adept::Vector;
using adept::adouble;
using adept::aReal;

//extern "C" {
// from https://github.com/AndrewCarterUK/mnist-neural-network-plain-c

#define MNIST_LABEL_MAGIC 0x00000801
#define MNIST_IMAGE_MAGIC 0x00000803
#define MNIST_IMAGE_WIDTH 28
#define MNIST_IMAGE_HEIGHT 28
#define MNIST_IMAGE_SIZE MNIST_IMAGE_WIDTH * MNIST_IMAGE_HEIGHT
#define MNIST_LABELS 10

typedef struct mnist_label_file_header_t_ {
    uint32_t magic_number;
    uint32_t number_of_labels;
} __attribute__((packed)) mnist_label_file_header_t;

typedef struct mnist_image_file_header_t_ {
    uint32_t magic_number;
    uint32_t number_of_images;
    uint32_t number_of_rows;
    uint32_t number_of_columns;
} __attribute__((packed)) mnist_image_file_header_t;

typedef struct mnist_image_t_ {
    uint8_t pixels[MNIST_IMAGE_SIZE];
} __attribute__((packed)) mnist_image_t;

typedef struct mnist_dataset_t_ {
    mnist_image_t * images;
    uint8_t * labels;
    uint32_t size;
} mnist_dataset_t;

typedef struct neural_network_t_ {
    float b[MNIST_LABELS];
    float W[MNIST_LABELS][MNIST_IMAGE_SIZE];
} neural_network_t;

typedef struct aneural_network_t_ {
    adept::FixedArray<float,true,MNIST_LABELS> b;
    adept::FixedArray<float,true,MNIST_LABELS,MNIST_IMAGE_SIZE> W;
} aneural_network_t;

typedef struct neural_network_gradient_t_ {
    float b_grad[MNIST_LABELS];
    float W_grad[MNIST_LABELS][MNIST_IMAGE_SIZE];
} neural_network_gradient_t;

/**
 * Convert from the big endian format in the dataset if we're on a little endian
 * machine.
 */
uint32_t map_uint32(uint32_t in)
{
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
    return (
        ((in & 0xFF000000) >> 24) |
        ((in & 0x00FF0000) >>  8) |
        ((in & 0x0000FF00) <<  8) |
        ((in & 0x000000FF) << 24)
    );
#else
    return in;
#endif
}

/**
 * Read labels from file.
 *
 * File format: http://yann.lecun.com/exdb/mnist/
 */
uint8_t * get_labels(const char * path, uint32_t * number_of_labels)
{
    FILE * stream;
    mnist_label_file_header_t header;
    uint8_t * labels;

    stream = fopen(path, "rb");

    if (NULL == stream) {
        fprintf(stderr, "Could not open file: %s\n", path);
        return NULL;
    }

    if (1 != fread(&header, sizeof(mnist_label_file_header_t), 1, stream)) {
        fprintf(stderr, "Could not read label file header from: %s\n", path);
        fclose(stream);
        return NULL;
    }

    header.magic_number = map_uint32(header.magic_number);
    header.number_of_labels = map_uint32(header.number_of_labels);

    if (MNIST_LABEL_MAGIC != header.magic_number) {
        fprintf(stderr, "Invalid header read from label file: %s (%08X not %08X)\n", path, header.magic_number, MNIST_LABEL_MAGIC);
        fclose(stream);
        return NULL;
    }

    *number_of_labels = header.number_of_labels;

    labels = (uint8_t*)malloc(*number_of_labels * sizeof(uint8_t));

    if (labels == NULL) {
        fprintf(stderr, "Could not allocated memory for %d labels\n", *number_of_labels);
        fclose(stream);
        return NULL;
    }

    if (*number_of_labels != fread(labels, 1, *number_of_labels, stream)) {
        fprintf(stderr, "Could not read %d labels from: %s\n", *number_of_labels, path);
        free(labels);
        fclose(stream);
        return NULL;
    }

    fclose(stream);

    return labels;
}

/**
 * Read images from file.
 *
 * File format: http://yann.lecun.com/exdb/mnist/
 */
mnist_image_t * get_images(const char * path, uint32_t * number_of_images)
{
    FILE * stream;
    mnist_image_file_header_t header;
    mnist_image_t * images;

    stream = fopen(path, "rb");

    if (NULL == stream) {
        fprintf(stderr, "Could not open file: %s\n", path);
        return NULL;
    }

    if (1 != fread(&header, sizeof(mnist_image_file_header_t), 1, stream)) {
        fprintf(stderr, "Could not read image file header from: %s\n", path);
        fclose(stream);
        return NULL;
    }

    header.magic_number = map_uint32(header.magic_number);
    header.number_of_images = map_uint32(header.number_of_images);
    header.number_of_rows = map_uint32(header.number_of_rows);
    header.number_of_columns = map_uint32(header.number_of_columns);

    if (MNIST_IMAGE_MAGIC != header.magic_number) {
        fprintf(stderr, "Invalid header read from image file: %s (%08X not %08X)\n", path, header.magic_number, MNIST_IMAGE_MAGIC);
        fclose(stream);
        return NULL;
    }

    if (MNIST_IMAGE_WIDTH != header.number_of_rows) {
        fprintf(stderr, "Invalid number of image rows in image file %s (%d not %d)\n", path, header.number_of_rows, MNIST_IMAGE_WIDTH);
    }

    if (MNIST_IMAGE_HEIGHT != header.number_of_columns) {
        fprintf(stderr, "Invalid number of image columns in image file %s (%d not %d)\n", path, header.number_of_columns, MNIST_IMAGE_HEIGHT);
    }

    *number_of_images = header.number_of_images;
    images = (mnist_image_t*)malloc(*number_of_images * sizeof(mnist_image_t));

    if (images == NULL) {
        fprintf(stderr, "Could not allocated memory for %d images\n", *number_of_images);
        fclose(stream);
        return NULL;
    }

    if (*number_of_images != fread(images, sizeof(mnist_image_t), *number_of_images, stream)) {
        fprintf(stderr, "Could not read %d images from: %s\n", *number_of_images, path);
        free(images);
        fclose(stream);
        return NULL;
    }

    fclose(stream);

    return images;
}

/**
 * Free all the memory allocated in a dataset. This should not be used on a
 * batched dataset as the memory is allocated to the parent.
 */
void mnist_free_dataset(mnist_dataset_t * dataset)
{
    free(dataset->images);
    free(dataset->labels);
    free(dataset);
}

mnist_dataset_t * mnist_get_dataset(const char * image_path, const char * label_path)
{
    mnist_dataset_t * dataset;
    uint32_t number_of_images, number_of_labels;

    dataset = (mnist_dataset_t*)calloc(1, sizeof(mnist_dataset_t));

    if (NULL == dataset) {
        return NULL;
    }

    dataset->images = get_images(image_path, &number_of_images);

    if (NULL == dataset->images) {
        mnist_free_dataset(dataset);
        return NULL;
    }

    dataset->labels = get_labels(label_path, &number_of_labels);

    if (NULL == dataset->labels) {
        mnist_free_dataset(dataset);
        return NULL;
    }

    if (number_of_images != number_of_labels) {
        fprintf(stderr, "Number of images does not match number of labels (%d != %d)\n", number_of_images, number_of_labels);
        mnist_free_dataset(dataset);
        return NULL;
    }

    dataset->size = number_of_images;

    return dataset;
}

/**
 * Fills the batch dataset with a subset of the parent dataset.
 */
int mnist_batch(mnist_dataset_t * dataset, mnist_dataset_t * batch, int size, int number)
{
    int start_offset;

    start_offset = size * number;

    if (start_offset >= dataset->size) {
        return 0;
    }

    batch->images = &dataset->images[start_offset];
    batch->labels = &dataset->labels[start_offset];
    batch->size = size;

    if (start_offset + batch->size > dataset->size) {
        batch->size = dataset->size - start_offset;
    }

    return 1;
}

#define STEPS 1000
#define BATCH_SIZE 100

#include <stdlib.h>
#include <string.h>
#include <math.h>

// Convert a pixel value from 0-255 to one from 0 to 1
#define PIXEL_SCALE(x) (((float) (x)) / 255.0f)

// Returns a random value between 0 and 1
#define RAND_FLOAT() (((float) rand()) / ((float) RAND_MAX))

/**
 * Initialise the weights and bias vectors with values between 0 and 1
 */
void neural_network_random_weights(neural_network_t * network)
{
    int i, j;

    for (i = 0; i < MNIST_LABELS; i++) {
        network->b[i] = RAND_FLOAT();

        for (j = 0; j < MNIST_IMAGE_SIZE; j++) {
            network->W[i][j] = RAND_FLOAT();
        }
    }
}

/**
 * Calculate the softmax vector from the activations. This uses a more
 * numerically stable algorithm that normalises the activations to prevent
 * large exponents.
 */
void neural_network_softmax(float * activations, int length)
{
    int i;
    float sum, max;

    for (i = 1, max = activations[0]; i < length; i++) {
        if (activations[i] > max) {
            max = activations[i];
        }
    }

    for (i = 0, sum = 0; i < length; i++) {
        activations[i] = exp(activations[i] - max);
        sum += activations[i];
    }

    for (i = 0; i < length; i++) {
        activations[i] /= sum;
    }
}

/**
 * Calculate the softmax vector from the activations. This uses a more
 * numerically stable algorithm that normalises the activations to prevent
 * large exponents.
 */
void aneural_network_softmax(adept::FixedArray<float,true,MNIST_LABELS> &activations, int length)
{
    int i;
    aReal sum, max;

    for (i = 1, max = activations[0]; i < length; i++) {
        if (activations[i] > max) {
            max = activations[i];
        }
    }

    for (i = 0, sum = 0; i < length; i++) {
        activations[i] = exp(activations[i] - max);
        sum += activations[i];
    }

    activations /= sum;
    for (i = 0; i < length; i++) {
        activations[i] /= sum;
    }
}
static double maxval(const float *activations, int length) {
    float max = activations[0];

    for (int i = 1; i < length; i++) {
        if (activations[i] > max) {
            max = activations[i];
        }
    }
    return max;
}

static double sumval(const float *activations, int length) {
    float sum = 0;

    for (int i = 0; i < length; i++) {
        sum += activations[i];
    }
    return sum;
}

static    void makeexps(float* exps, const float* activations, int length, double max) {
    for (int i = 0; i < length; i++) {
        exps[i] = exp(activations[i] - max);
    }

}
/**
 * Calculate the softmax vector from the activations. This uses a more
 * numerically stable algorithm that normalises the activations to prevent
 * large exponents.
 */
static void neural_network_softmax_v2(const float * activations, float* outp, int length)
{
    int i;
    float sum, max;

    for (i = 1, max = activations[0]; i < length; i++) {
        if (activations[i] > max) {
            max = activations[i];
        }
    }

    for (i = 0, sum = 0; i < length; i++) {
        sum += exp(activations[i] - max);
    }

    for (i = 0; i < length; i++) {
        outp[i] = exp(activations[i] - max) / sum;
    }
#if 0
    float max = maxval(activations, length);
    float exps[length];
    makeexps(exps,activations, length, max);
    float sum = sumval(exps,length);
    /*
    for (int i = 0; i < length; i++) {
        double tmp = exps[i];//exp(activations[i] - max);
        sum += tmp;
    }
    */

    for (int i = 0; i < length; i++) {
        outp[i] = exps[i] / sum;
    }
#endif
}
/**
 * Use the weights and bias vector to forward propogate through the neural
 * network and calculate the activations.
 */
void neural_network_hypothesis(const mnist_image_t * image, const neural_network_t * network, float activations[MNIST_LABELS])
{
    int i, j;

    for (i = 0; i < MNIST_LABELS; i++) {
        activations[i] = network->b[i];

        for (j = 0; j < MNIST_IMAGE_SIZE; j++) {
            activations[i] += network->W[i][j] * PIXEL_SCALE(image->pixels[j]);
        }
    }

    neural_network_softmax(activations, MNIST_LABELS);
}

/**
 * Use the weights and bias vector to forward propogate through the neural
 * network and calculate the activations.
 */
static float neural_network_hypothesis_v2(const mnist_image_t * image, const neural_network_t * network, uint8_t label)
{
    float activations[MNIST_LABELS] = {0};
    int i, j;

    for (i = 0; i < MNIST_LABELS; i++) {
        activations[i] = network->b[i];

        for (j = 0; j < MNIST_IMAGE_SIZE; j++) {
            activations[i] += network->W[i][j] * PIXEL_SCALE(image->pixels[j]);
        }
    }

    float activations2[MNIST_LABELS] = { 0 };
    neural_network_softmax_v2(activations, activations2, MNIST_LABELS);
    return -log(activations2[label]);
}

static aReal neural_network_hypothesis_adept(const mnist_image_t * image, const aneural_network_t * network, uint8_t label)
{
    adept::FixedArray<float,true,MNIST_LABELS> activations = network->b;
    int i, j;

    for (i = 0; i < MNIST_LABELS; i++) {
        for (j = 0; j < MNIST_IMAGE_SIZE; j++) {
            activations(i) += network->W(i,j) * PIXEL_SCALE(image->pixels[j]);
        }
    }

    aneural_network_softmax(activations, MNIST_LABELS);
    return -log(activations[label]);
}

static void calculateDerivatives_adept(mnist_image_t * image, bool run, adept::Stack& stack, const aneural_network_t * anetwork, neural_network_t* gradient, uint8_t label) {

    if (!run) {
        stack.new_recording();
    //    run = true;
    } else
        stack.continue_recording();
    auto resa = neural_network_hypothesis_adept(image, anetwork, label);
    resa.set_gradient(1.0);
    stack.reverse();
    stack.pause_recording();

    for (int i = 0; i < MNIST_LABELS; i++) {
        gradient->b[i] = anetwork->b(i).get_gradient();
        for (int j = 0; j < MNIST_IMAGE_SIZE; j++) {
            gradient->W[i][j] = anetwork->W(i,j).get_gradient();
        }
    }
}

extern "C" {
#include <adBuffer.h>
}

void neural_network_softmax_b(float *activations, float *activationsb, int
        length) {
    float sum, max;
    float sumb, maxb;
    int branch;
    max = activations[0];
    for (int i = 1; i < length; ++i)
        if (activations[i] > max) {
            max = activations[i];
            pushControl1b(1);
        } else
            pushControl1b(0);
    sum = 0;
    for (int i = 0; i < length; ++i) {
        pushReal4(activations[i]);
        activations[i] = (float)exp(activations[i] - max);
        sum = sum + activations[i];
    }
    for (int i = 0; i < length; ++i) {
        pushReal4(activations[i]);
        activations[i] = activations[i]/sum;
    }
    sumb = 0.0;
    for (int i = length-1; i > -1; --i) {
        popReal4(&(activations[i]));
        sumb = sumb - activations[i]*activationsb[i]/(sum*sum);
        activationsb[i] = activationsb[i]/sum;
    }
    {
      float tempb;
      maxb = 0.0;
      for (int i = length-1; i > -1; --i) {
          activationsb[i] = activationsb[i] + sumb;
          popReal4(&(activations[i]));
          tempb = exp(activations[i]-max)*activationsb[i];
          maxb = maxb - tempb;
          activationsb[i] = tempb;
      }
    }
    for (int i = length-1; i > 0; --i) {
        popControl1b(&branch);
        if (branch != 0) {
            activationsb[i] = activationsb[i] + maxb;
            maxb = 0.0;
        }
    }
    activationsb[0] = activationsb[0] + maxb;
}

/**
 * Calculate the softmax vector from the activations. This uses a more
 * numerically stable algorithm that normalises the activations to prevent
 * large exponents.
 */
// Convert a pixel value from 0-255 to one from 0 to 1
// Returns a random value between 0 and 1
void neural_network_softmax_c(float *activations, int length) {
    float sum, max;
    max = activations[0];
	int i;
    for (i = 1; i < length; ++i)
        if (activations[i] > max)
            max = activations[i];
    sum = 0;
    for (i = 0; i < length; ++i) {
        activations[i] = (float)exp(activations[i] - max);
        sum += activations[i];
    }
    for (i = 0; i < length; ++i)
        activations[i] /= sum;
}

/*
  Differentiation of neural_network_hypothesis_tapenadesource in reverse (adjoint) mode:
   gradient     of useful results: neural_network_hypothesis_tapenadesource
                *network.b[0:10-1] *network.W[0:10-1][0:28*28-1]
   with respect to varying inputs: *network.b[0:10-1] *network.W[0:10-1][0:28*28-1]
   RW status of diff variables: neural_network_hypothesis_tapenadesource:in-killed
                *network.b[0:10-1]:incr *network.W[0:10-1][0:28*28-1]:incr
   Plus diff mem management of: network:in *network.b:in *network.W:in
                *network.W[0:10-1]:in
*/
static void neural_network_hypothesis_tapenadesource_b(const mnist_image_t *
        image, const neural_network_t *network, neural_network_t *networkb,
        uint8_t label, float neural_network_hypothesis_tapenadesourceb) {
    float activations[10];
    float activationsb[10];
    int ii1;
    float neural_network_hypothesis_tapenadesource;
    for (int i = 0; i < 10; ++i) {
        activations[i] = network->b[i];
        for (int j = 0; j < 784; ++j)
            activations[i] = activations[i] + network->W[i][j]*((float)image->
                pixels[j]/255.0f);
    }
    pushReal4Array(activations, 10);
    neural_network_softmax_c(activations, 10);
    for (ii1 = 0; ii1 < 10; ++ii1)
        activationsb[ii1] = 0.0;
    activationsb[(int)label] = activationsb[(int)label] -
        neural_network_hypothesis_tapenadesourceb/activations[(int)label];
    popReal4Array(activations, 10);
    neural_network_softmax_b(activations, activationsb, 10);
    for (int i = 9; i > -1; --i) {
        for (int j = 783; j > -1; --j)
            networkb->W[i][j] = networkb->W[i][j] + (float)image->pixels[j]*
                activationsb[i]/255.0f;
        networkb->b[i] = networkb->b[i] + activationsb[i];
        activationsb[i] = 0.0;
    }
}

/**
 * Update the gradients for this step of gradient descent using the gradient
 * contributions from a single training example (image).
 *
 * This function returns the loss ontribution from this training example.
 */
float neural_network_gradient_update(mnist_image_t * image, const neural_network_t * network, neural_network_gradient_t * gradient, uint8_t label)
{
    float activations[MNIST_LABELS];
    float b_grad, W_grad;
    int i, j;

    // First forward propagate through the network to calculate activations
    neural_network_hypothesis(image, network, activations);

    for (i = 0; i < MNIST_LABELS; i++) {
        // This is the gradient for a softmax bias input
        b_grad = (i == label) ? activations[i] - 1 : activations[i];

        for (j = 0; j < MNIST_IMAGE_SIZE; j++) {
            // The gradient for the neuron weight is the bias multiplied by the input weight
            W_grad = b_grad * PIXEL_SCALE(image->pixels[j]);

            // Update the weight gradient
            gradient->W_grad[i][j] += W_grad;
        }

        // Update the bias gradient
        gradient->b_grad[i] += b_grad;
    }

    // Cross entropy loss
    return 0.0f - log(activations[label]);
}

extern int enzyme_const;
template<typename Return, typename... T>
Return __enzyme_autodiff(T...);

static void calculateDerivatives(mnist_image_t * image, const neural_network_t * network, neural_network_t* gradient, uint8_t label) {
    __enzyme_autodiff<void>(neural_network_hypothesis_v2, enzyme_const, image, network, gradient, enzyme_const, label);
}

/**
 * Run one step of gradient descent and update the neural network.
 */
float neural_network_training_step(mnist_dataset_t * dataset, neural_network_t * network, float learning_rate)
{
    neural_network_t gradient = {0};
    neural_network_t gradient2 = {0};

    /*
    adept::Stack stack;
    aneural_network_t anetwork;

    for (int i = 0; i < MNIST_LABELS; i++) {
        anetwork.b[i] = network->b[i];
        for (int j = 0; j < MNIST_IMAGE_SIZE; j++) {
            anetwork.W[i][j] = network->W[i][j];
        }
    }*/

    float total_loss;
    int i, j;

    // Calculate the gradient and the loss by looping through the training set
    for (i = 0, total_loss = 0; i < dataset->size; i++) {
		mnist_image_t* image = &dataset->images[i];
		uint8_t label = dataset->labels[i];

    	// First forward propagate through the network to calculate activations

        //calculateDerivatives(image, network, &gradient, label);
        //calculateDerivatives_adept(image, i != 0, stack, &anetwork, &gradient, label);
		//neural_network_hypothesis_tapenadesource_b(image, network, &gradient, label, 1.0);

        total_loss +=neural_network_gradient_update(image, network, (neural_network_gradient_t*)&gradient, label);

        //total_loss +=neural_network_gradient_update(image, network, (neural_network_gradient_t*)&gradient2, label);

	    //float activations[MNIST_LABELS];
        //neural_network_hypothesis(image, network, activations);
    	//total_loss -= log(activations[label]);

    }

    // Apply gradient descent to the network
    for (i = 0; i < MNIST_LABELS; i++) {
        //printf("b'[i] %f %f\n", gradient.b[i], gradient2.b[i]);
        network->b[i] -= learning_rate * gradient.b[i] / ((float) dataset->size);

        for (j = 0; j < MNIST_IMAGE_SIZE + 1; j++) {
            network->W[i][j] -= learning_rate * gradient.W[i][j] / ((float) dataset->size);
        }
    }

    return total_loss;
}

/**
 * Run one step of gradient descent and update the neural network.
 */
float neural_network_training_step_enzyme(mnist_dataset_t * dataset, neural_network_t * network, float learning_rate)
{
    neural_network_t gradient = {0};
    neural_network_t gradient2 = {0};

    float total_loss;
    int i, j;

    // Calculate the gradient and the loss by looping through the training set
    for (i = 0, total_loss = 0; i < dataset->size; i++) {
		mnist_image_t* image = &dataset->images[i];
		uint8_t label = dataset->labels[i];

    	// First forward propagate through the network to calculate activations

        calculateDerivatives(image, network, &gradient, label);
        //calculateDerivatives_adept(image, i != 0, stack, &anetwork, &gradient, label);
		//neural_network_hypothesis_tapenadesource_b(image, network, &gradient, label, 1.0);

        //total_loss +=neural_network_gradient_update(image, network, (neural_network_gradient_t*)&gradient2, label);

	    float activations[MNIST_LABELS];
        neural_network_hypothesis(image, network, activations);
    	total_loss -= log(activations[label]);

    }

    // Apply gradient descent to the network
    for (i = 0; i < MNIST_LABELS; i++) {
        //printf("b'[i] %f %f\n", gradient.b[i], gradient2.b[i]);
        network->b[i] -= learning_rate * gradient.b[i] / ((float) dataset->size);

        for (j = 0; j < MNIST_IMAGE_SIZE + 1; j++) {
            network->W[i][j] -= learning_rate * gradient.W[i][j] / ((float) dataset->size);
        }
    }

    return total_loss;
}

/**
 * Run one step of gradient descent and update the neural network.
 */
float neural_network_training_step_adept(mnist_dataset_t * dataset, neural_network_t * network, float learning_rate)
{
    neural_network_t gradient = {0};
    neural_network_t gradient2 = {0};

    adept::Stack stack;
    aneural_network_t anetwork;

    for (int i = 0; i < MNIST_LABELS; i++) {
        anetwork.b[i] = network->b[i];
        for (int j = 0; j < MNIST_IMAGE_SIZE; j++) {
            anetwork.W[i][j] = network->W[i][j];
        }
    }

    float total_loss;
    int i, j;

    // Calculate the gradient and the loss by looping through the training set
    for (i = 0, total_loss = 0; i < dataset->size; i++) {
		mnist_image_t* image = &dataset->images[i];
		uint8_t label = dataset->labels[i];

    	// First forward propagate through the network to calculate activations

        calculateDerivatives_adept(image, i != 0, stack, &anetwork, &gradient, label);

        //total_loss +=neural_network_gradient_update(image, network, (neural_network_gradient_t*)&gradient2, label);

	    float activations[MNIST_LABELS];
        neural_network_hypothesis(image, network, activations);
    	total_loss -= log(activations[label]);

    }

    // Apply gradient descent to the network
    for (i = 0; i < MNIST_LABELS; i++) {
        //printf("b'[i] %f %f\n", gradient.b[i], gradient2.b[i]);
        network->b[i] -= learning_rate * gradient.b[i] / ((float) dataset->size);

        for (j = 0; j < MNIST_IMAGE_SIZE + 1; j++) {
            network->W[i][j] -= learning_rate * gradient.W[i][j] / ((float) dataset->size);
        }
    }

    return total_loss;
}

/**
 * Run one step of gradient descent and update the neural network.
 */
float neural_network_training_step_tapenade(mnist_dataset_t * dataset, neural_network_t * network, float learning_rate)
{
    neural_network_t gradient = {0};
    neural_network_t gradient2 = {0};

    float total_loss;
    int i, j;

    // Calculate the gradient and the loss by looping through the training set
    for (i = 0, total_loss = 0; i < dataset->size; i++) {
		mnist_image_t* image = &dataset->images[i];
		uint8_t label = dataset->labels[i];

    	// First forward propagate through the network to calculate activations

		neural_network_hypothesis_tapenadesource_b(image, network, &gradient, label, 1.0);
        //total_loss +=neural_network_gradient_update(image, network, (neural_network_gradient_t*)&gradient2, label);

	    float activations[MNIST_LABELS];
        neural_network_hypothesis(image, network, activations);
    	total_loss -= log(activations[label]);
    }

    // Apply gradient descent to the network
    for (i = 0; i < MNIST_LABELS; i++) {
        //printf("b'[i] %f %f\n", gradient.b[i], gradient2.b[i]);
        network->b[i] -= learning_rate * gradient.b[i] / ((float) dataset->size);

        for (j = 0; j < MNIST_IMAGE_SIZE + 1; j++) {
            network->W[i][j] -= learning_rate * gradient.W[i][j] / ((float) dataset->size);
        }
    }

    return total_loss;
}

/**
 * Downloaded from: http://yann.lecun.com/exdb/mnist/
 */
const char * train_images_file = "data/train-images-idx3-ubyte";
const char * train_labels_file = "data/train-labels-idx1-ubyte";
const char * test_images_file = "data/t10k-images-idx3-ubyte";
const char * test_labels_file = "data/t10k-labels-idx1-ubyte";

/**
 * Calculate the accuracy of the predictions of a neural network on a dataset.
 */
float calculate_accuracy(mnist_dataset_t * dataset, neural_network_t * network) {
    float activations[MNIST_LABELS], max_activation;
    int i, j, correct, predict;

    // Loop through the dataset
    for (i = 0, correct = 0; i < dataset->size; i++) {
        // Calculate the activations for each image using the neural network
        neural_network_hypothesis(&dataset->images[i], network, activations);

        // Set predict to the index of the greatest activation
        for (j = 0, predict = 0, max_activation = activations[0]; j < MNIST_LABELS; j++) {
            if (max_activation < activations[j]) {
                max_activation = activations[j];
                predict = j;
            }
        }

        // Increment the correct count if we predicted the right label
        if (predict == dataset->labels[i]) {
            correct++;
        }
    }

    // Return the percentage we predicted correctly as the accuracy
    return ((float) correct) / ((float) dataset->size);
}

#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>

float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

void run(float (*fn)(mnist_dataset_t*, neural_network_t*, float)) {
    mnist_dataset_t * train_dataset, * test_dataset;
    mnist_dataset_t batch;
    neural_network_t network;
    float loss, accuracy;
    int i, batches;

    // Read the datasets from the files
    train_dataset = mnist_get_dataset(train_images_file, train_labels_file);
    test_dataset = mnist_get_dataset(test_images_file, test_labels_file);

    // Initialise weights and biases with random values
    neural_network_random_weights(&network);

    // Calculate how many batches (so we know when to wrap around)
    batches = train_dataset->size / BATCH_SIZE;

    struct timeval start, end;
    gettimeofday(&start, NULL);

    for (i = 0; i < STEPS; i++) {
        // Initialise a new batch
        mnist_batch(train_dataset, &batch, 100, i % batches);

        // Run one step of gradient descent and calculate the loss
        loss = fn(&batch, &network, 0.5);

        // Calculate the accuracy using the whole test dataset
        accuracy = calculate_accuracy(test_dataset, &network);

        printf("Step %04d\tAverage Loss: %.2f\tAccuracy: %.3f\n", i, loss / batch.size, accuracy);
    }

    gettimeofday(&end, NULL);
    printf("%0.6f\n", tdiff(&start, &end));

    // Cleanup
    mnist_free_dataset(train_dataset);
    mnist_free_dataset(test_dataset);

}

int main(int argc, char *argv[])
{
    printf("Regular\n");
    run(neural_network_training_step);
    printf("Enzyme\n");
    run(neural_network_training_step_enzyme);
    printf("Adept\n");
    run(neural_network_training_step_adept);
    printf("Tapenade\n");
    run(neural_network_training_step_tapenade);
    return 0;
}


//==================================================================================================
/*!
  @file

    @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SINCPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINCPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the Computes the sinpi cardinal
    value of its parameter, i.e. \f$sin(\pi x)/(\pi x) \f$.

    @par Header <boost/simd/function/sincpi.hpp>

    @see sin, sinc, sinhc

    @par Example:

      @snippet sincpi.cpp sincpi

    @par Possible output:

      @snippet sincpi.txt sincpi

  **/
  IEEEValue sincpi(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sincpi.hpp>
#include <boost/simd/function/simd/sincpi.hpp>

#endif


/*
 * Sampling_functions.hpp
 *
 *  Created on: Dec 21, 2018
 *      Author: thanasis
 */
#include <Eigen/Core>
using namespace Eigen;

#ifndef SAMPLING_FUNCTIONS_HPP_
#define SAMPLING_FUNCTIONS_HPP_

double sample_mu(int N, double Esigma2,const VectorXd& Y,const MatrixXd& X,const VectorXd& beta);
double sample_psi2_chisq(const VectorXd& beta,int NZ,double v0B,double s0B);
double sample_sigma_chisq(int N,const VectorXd& epsilon,double v0E,double s0E);
double sample_w(int M,int NZ);

#endif /* SAMPLING_FUNCTIONS_HPP_ */


/*****************************************************************************
 * simple.cpp        Some simple array operations
 *****************************************************************************/

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    Array<double,1> x(100);
    x = tensor::i;          // x = [ 0, 1, 2, ..., 99 ]

    Array<double,1> z(x + 150);
    Array<double,1> v(z + x * 2);

    cout << v << endl;
}


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * 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. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 *          Erik Nelson            ( eanelson@eecs.berkeley.edu )
 */

#include <Eigen/Core>
#include <Eigen/LU>
#include <gflags/gflags.h>

#include <geometry/rotation.h>
#include <math/random_generator.h>

#include <gtest/gtest.h>

namespace bsfm {

using Eigen::Matrix3d;
using Eigen::Vector3d;

TEST(Rotation, TestEulerAnglesAndMatrices) {
  // Randomly generate euler angles, convert to a matrix, then convert back.
  // Check that (1) the rotation matrix has det(R)==1, and (2), that we get the
  // same angles back.
  math::RandomGenerator rng(0);
  for (int ii = 0; ii < 1000; ++ii) {
    Vector3d e1;
    e1.setRandom();

    // Converting from rotation matrices to euler angles is only valid when phi,
    // theta, and psi are all < 0.5*PI. Otherwise the problem has multiple
    // solutions, and we can only return one of them with our function.
    e1 *= 0.5 * M_PI;

    Matrix3d R = EulerAnglesToMatrix(e1);
    EXPECT_NEAR(1.0, R.determinant(), 1e-8);

    Vector3d e2 = MatrixToEulerAngles(R);
    EXPECT_NEAR(0.0, S1Distance(e1(0), e2(0)), 1e-8);
    EXPECT_NEAR(0.0, S1Distance(e1(1), e2(1)), 1e-8);
    EXPECT_NEAR(0.0, S1Distance(e1(2), e2(2)), 1e-8);

    EXPECT_TRUE(R.isApprox(EulerAnglesToMatrix(e2), 1e-4));
  }
}

}  //\namespace bsfm


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

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/tuple.hpp>
using namespace boost::hana;

int main() {

{

//! [sequence]
BOOST_HANA_CONSTEXPR_CHECK(
    sequence<Maybe>(make<Tuple>(just(1), just('2'), just(3.3))) ==
    just(make<Tuple>(1, '2', 3.3))
);

BOOST_HANA_CONSTANT_CHECK(
    sequence<Maybe>(make<Tuple>(just(1), nothing, just(3.3))) == nothing
);

// This is a generalized Cartesian product.
BOOST_HANA_CONSTEXPR_CHECK(
    sequence<Tuple>(make<Tuple>(make<Tuple>(1, 2, 3),
                                make<Tuple>(4),
                                make<Tuple>(5, 6)))
    ==
    make<Tuple>(
        make<Tuple>(1, 4, 5), make<Tuple>(1, 4, 6),
        make<Tuple>(2, 4, 5), make<Tuple>(2, 4, 6),
        make<Tuple>(3, 4, 5), make<Tuple>(3, 4, 6)
    )
);
//! [sequence]

}{

//! [traverse]
BOOST_HANA_CONSTEXPR_LAMBDA auto half = [](auto x) {
    return if_(x % int_<2> == int_<0>,
        just(x / int_<2>),
        nothing
    );
};

BOOST_HANA_CONSTANT_CHECK(
    traverse<Maybe>(make<Tuple>(int_<2>, int_<4>, int_<6>), half)
    ==
    just(make<Tuple>(int_<1>, int_<2>, int_<3>))
);

BOOST_HANA_CONSTANT_CHECK(
    traverse<Maybe>(make<Tuple>(int_<2>, int_<3>, int_<6>), half)
    ==
    nothing
);

BOOST_HANA_CONSTEXPR_LAMBDA auto twice = [](auto x) {
    return make<Tuple>(x, x);
};

BOOST_HANA_CONSTEXPR_CHECK(
    traverse<Tuple>(just('x'), twice) == make<Tuple>(just('x'), just('x'))
);

BOOST_HANA_CONSTANT_CHECK(
    traverse<Tuple>(nothing, twice) == make<Tuple>(nothing)
);
//! [traverse]

}

}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2015-2016 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
// #define EIGEN_DONT_VECTORIZE
// #define EIGEN_MAX_ALIGN_BYTES 0
#include "sparse_solver.h"
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/IterativeSolvers>

template<typename T, typename I_> void test_incomplete_cholesky_T()
{
  typedef SparseMatrix<T,0,I_> SparseMatrixType;
  ConjugateGradient<SparseMatrixType, Lower, IncompleteCholesky<T, Lower, AMDOrdering<I_> > >        cg_illt_lower_amd;
  ConjugateGradient<SparseMatrixType, Lower, IncompleteCholesky<T, Lower, NaturalOrdering<I_> > >    cg_illt_lower_nat;
  ConjugateGradient<SparseMatrixType, Upper, IncompleteCholesky<T, Upper, AMDOrdering<I_> > >        cg_illt_upper_amd;
  ConjugateGradient<SparseMatrixType, Upper, IncompleteCholesky<T, Upper, NaturalOrdering<I_> > >    cg_illt_upper_nat;
  ConjugateGradient<SparseMatrixType, Upper|Lower, IncompleteCholesky<T, Lower, AMDOrdering<I_> > >  cg_illt_uplo_amd;

  CALL_SUBTEST( check_sparse_spd_solving(cg_illt_lower_amd) );
  CALL_SUBTEST( check_sparse_spd_solving(cg_illt_lower_nat) );
  CALL_SUBTEST( check_sparse_spd_solving(cg_illt_upper_amd) );
  CALL_SUBTEST( check_sparse_spd_solving(cg_illt_upper_nat) );
  CALL_SUBTEST( check_sparse_spd_solving(cg_illt_uplo_amd) );
}

template<int>
void bug1150()
{
  // regression for bug 1150
  for(int N = 1; N<20; ++N)
  {
    Eigen::MatrixXd b( N, N );
    b.setOnes();

    Eigen::SparseMatrix<double> m( N, N );
    m.reserve(Eigen::VectorXi::Constant(N,4));
    for( int i = 0; i < N; ++i )
    {
        m.insert( i, i ) = 1;
        m.coeffRef( i, i / 2 ) = 2;
        m.coeffRef( i, i / 3 ) = 2;
        m.coeffRef( i, i / 4 ) = 2;
    }

    Eigen::SparseMatrix<double> A;
    A = m * m.transpose();

    Eigen::ConjugateGradient<Eigen::SparseMatrix<double>,
        Eigen::Lower | Eigen::Upper,
        Eigen::IncompleteCholesky<double> > solver( A );
    VERIFY(solver.preconditioner().info() == Eigen::Success);
    VERIFY(solver.info() == Eigen::Success);
  }
}

EIGEN_DECLARE_TEST(incomplete_cholesky)
{
  CALL_SUBTEST_1(( test_incomplete_cholesky_T<double,int>() ));
  CALL_SUBTEST_2(( test_incomplete_cholesky_T<std::complex<double>, int>() ));
  CALL_SUBTEST_3(( test_incomplete_cholesky_T<double,long int>() ));

  CALL_SUBTEST_1(( bug1150<0>() ));
}


/**
 * Computing transforms.
 * \todo should be smarter about dimensions...what if 2d data, what if 3d+time?
 */
#include <stdlib.h>
#include <stdint.h>
#include "src/aabb.h"
#include "nd.h"
#include <Eigen/Core>
#include <Eigen/LU>
using namespace Eigen;
#include <iostream>
using namespace std;

//#define DEBUG

#ifndef restrict
  #define restrict __restrict
#endif

// output transform should be T s.t. rs=T*rd
// so, if r=Ts*rs=Td*rd
//   T = inv(Ts)*Td

/**
 * \param[in,out] out   Must be preallocated with at least (ndim+1)*(ndim+1)
 *                      elements
 */
extern "C"
void compose(float *restrict out,
             aabb_t bbox,
             float sx, float sy, float sz, // scale in aabb units
             float *restrict transform, unsigned ndim)
{
  MatrixXf                         dst2world(ndim+1,ndim+1);
  Map<Matrix<float,Dynamic,Dynamic,RowMajor> > src2world(transform,ndim+1,ndim+1);
  Map<Matrix<float,Dynamic,Dynamic,RowMajor> > T(out,ndim+1,ndim+1);

  int64_t      *o;
  AABBGet(bbox,0,&o,0);
  dst2world.setIdentity();
  dst2world.block<3,1>(0,ndim)<<(float)o[0],(float)o[1],(float)o[2];
  dst2world.block<3,3>(0,0).diagonal()<<sx,sy,sz;

  T=(src2world.inverse()*dst2world).eval();
#ifdef DEBUG
  #define show(e) cout<<#e" is "<<endl<<e<<endl<<endl
  show(dst2world);
  show(src2world);
  show(src2world.inverse());
  show(T);
  #undef show
#endif
}

extern "C"
void box2box(float *restrict out,
             nd_t dst, aabb_t dstbox,
             nd_t src, aabb_t srcbox)
{ size_t d;
  int64_t *ori,*shape;

  AABBGet(srcbox,0,&ori,&shape);
  d=ndndim(src);
  MatrixXf src2world(d+1,d+1);
  src2world.setIdentity().block<3,3>(0,0).diagonal()
    << (float)shape[0]/(float)ndshape(src)[0],
       (float)shape[1]/(float)ndshape(src)[1],
       (float)shape[2]/(float)ndshape(src)[2];
  src2world.block<3,1>(0,d)
    << (float)ori[0],(float)ori[1],(float)ori[2];

  AABBGet(dstbox,0,&ori,&shape);
  d=ndndim(dst);
  MatrixXf dst2world(d+1,d+1);
  dst2world.setIdentity().block<3,3>(0,0).diagonal()
    << (float)shape[0]/(float)ndshape(dst)[0],
       (float)shape[1]/(float)ndshape(dst)[1],
       (float)shape[2]/(float)ndshape(dst)[2];
  dst2world.block<3,1>(0,d)
    << (float)ori[0],(float)ori[1],(float)ori[2];

  Map<Matrix<float,Dynamic,Dynamic,RowMajor> > T(out,ndndim(src)+1,ndndim(dst)+1);
  T=src2world.inverse()*dst2world;
#ifdef DEBUG
  #define show(e) cout<<#e" is "<<endl<<e<<endl<<endl
  show(dst2world);
  show(src2world);
  show(src2world.inverse());
  show(T);
  #undef show
#endif
  //  [s,r_s] x [r_d,d]; r_s == r_d
}


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MODULES_GEOMETRY_COMMONS_HPP_
#define MODULES_GEOMETRY_COMMONS_HPP_

#include <Eigen/Core>
#include <string>
#include <vector>
#include <cmath>
#include <sstream>
#include <memory>
#include <iostream>
#include <algorithm>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>

namespace modules {
namespace geometry {

//! using boost geometry
namespace bg = boost::geometry;

//! points
template <typename T>
using Point2d_t = bg::model::point<T, 2, bg::cs::cartesian>;
using Point2d = Point2d_t<float>;

 //! Point operators
inline bool operator==(const Point2d& lhs, const Point2d& rhs) { return(bg::get<0>(lhs) == bg::get<0>(rhs) && bg::get<1>(lhs) == bg::get<1>(rhs)); }
inline bool operator!=(const Point2d& lhs, const Point2d& rhs) { return !(lhs == rhs); }
inline Point2d operator+(const Point2d& lhs, const Point2d& rhs) { return Point2d(bg::get<0>(lhs)+ bg::get<0>(rhs), bg::get<1>(lhs) + bg::get<1>(rhs)); }
inline Point2d operator+(const Point2d& lhs, const float& rhs) { return Point2d(bg::get<0>(lhs)+ rhs , bg::get<1>(lhs) + rhs); }

inline Point2d operator-(const Point2d& lhs, const Point2d& rhs) { return Point2d(bg::get<0>(lhs) - bg::get<0>(rhs), bg::get<1>(lhs) - bg::get<1>(rhs)); }
inline Point2d operator-(const Point2d& lhs, const float& rhs) { return Point2d(bg::get<0>(lhs)- rhs, bg::get<1>(lhs) + rhs); }

inline Point2d operator*(const Point2d& point, const float& factor) { return Point2d(bg::get<0>(point) * factor , bg::get<1>(point) * factor); }
inline Point2d operator/(const Point2d& point, const float& divisor) { return Point2d(bg::get<0>(point) / divisor , bg::get<1>(point) / divisor); }

using Pose = Eigen::Vector3d;

inline std::string print(const Point2d &p) {
  std::stringstream ss;
  ss << "Point2d: x: " << bg::get<0>(p) << ", y: " << bg::get<1>(p) << std::endl;
  return ss.str();
}

inline float distance(const Point2d &p1, const Point2d &p2) {
  float dx = bg::get<0>(p1) - bg::get<0>(p2);
  float dy = bg::get<1>(p1) - bg::get<1>(p2);
  return sqrt(dx * dx + dy * dy);
}

template <typename G, typename T>
struct Shape {
  Shape(const Pose &center, std::vector<T> points, int32_t id) : obj_(), id_(id), center_(center) {
    for (auto it = points.begin(); it != points.end(); ++it)
      add_point(*it);
  }

  Shape(const Pose &center, const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> &points, int32_t id) : obj_(), id_(id), center_(center) {
    auto row_num = points.rows();
    for (auto rowIter = 0; rowIter < row_num; ++rowIter) {
      // std::vector<T> vec = points.rows(rowIter);
      // for(auto col = 0;col<=1;++col){
      // Point2d_t<T> p = (points(rowIter,0),points(rowIter,1))
      add_point(T(points.coeff(rowIter, 0), points.coeff(rowIter, 1)));
      //}
    }
  }

  virtual ~Shape() {}
  virtual Shape *Clone() const = 0;
  virtual std::string ShapeToString() const;

  // rotates object
  Shape<G, T> *rotate(const float &a) const;

  // translates object
  Shape<G, T> *translate(const Point2d &point) const;

  // return object transform
  Shape<G, T> *transform(const Pose &pose) const;

  bool Valid();

  virtual Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> toArray() const = 0;

  bool add_point(const T &p) {
    bg::append(obj_, p);
    return true;
  }

  G obj_;
  int32_t id_;
  Pose center_;  // fixed center pose of shape
};

template <typename G, typename T>
inline bool Shape<G, T>::Valid() {
  std::string message;
  bool valid = boost::geometry::is_valid(obj_, message);
  if (!valid) {
    std::cout << "why not valid? " << message << std::endl;
  }
  return valid;
}

template <typename G, typename T>
inline Shape<G, T> *Shape<G, T>::rotate(const float &a) const {
  namespace trans = boost::geometry::strategy::transform;
  // move shape relative to coordinate center
  trans::translate_transformer<double, 2, 2> translate_rel_to_center(-center_[0], -center_[1]);
  G obj_rel_translated;
  boost::geometry::transform(obj_, obj_rel_translated, translate_rel_to_center);

  // rotate (counterclockwise)
  trans::rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(-a);
  G obj_rotated;
  boost::geometry::transform(obj_rel_translated, obj_rotated, rotate);

  // move object backwards plus translation component
  trans::translate_transformer<double, 2, 2> translate_backwards(center_[0], center_[1]);
  G obj_transformed;
  boost::geometry::transform(obj_rotated, obj_transformed, translate_backwards);

  Shape<G, T> *shape_transformed = this->Clone();
  shape_transformed->obj_ = obj_transformed;
  shape_transformed->center_[2] += a;
  return shape_transformed;
}

template <typename G, typename T>
inline Shape<G, T> *Shape<G, T>::translate(const Point2d &point) const {
  namespace trans = boost::geometry::strategy::transform;
  trans::translate_transformer<double, 2, 2> translate_backwards(bg::get<0>(point), bg::get<1>(point));
  G obj_transformed;
  boost::geometry::transform(obj_, obj_transformed, translate_backwards);

  Shape<G, T> *shape_transformed = this->Clone();
  shape_transformed->obj_ = obj_transformed;
  shape_transformed->center_[0] += bg::get<0>(point);
  shape_transformed->center_[1] += bg::get<1>(point);
  return shape_transformed;
}

template <typename G, typename T>
inline Shape<G, T> *Shape<G, T>::transform(const Pose &pose) const {
  namespace trans = boost::geometry::strategy::transform;
  // move shape relative to coordinate center
  trans::translate_transformer<double, 2, 2> translate_rel_to_center(-center_[0], -center_[1]);
  G obj_rel_translated;
  boost::geometry::transform(obj_, obj_rel_translated, translate_rel_to_center);

  // rotate (counterclockwise)
  trans::rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(-pose[2]);
  G obj_rotated;
  boost::geometry::transform(obj_rel_translated, obj_rotated, rotate);

  // move object backwards plus translation component
  trans::translate_transformer<double, 2, 2> translate_backwards(center_[0] + pose[0], center_[1] + pose[1]);
  G obj_transformed;
  boost::geometry::transform(obj_rotated, obj_transformed, translate_backwards);

  Shape<G, T> *shape_transformed = this->Clone();
  shape_transformed->obj_ = obj_transformed;
  shape_transformed->center_[0] += pose[0];
  shape_transformed->center_[1] += pose[1];
  shape_transformed->center_[2] += pose[2];
  return shape_transformed;
}

template <typename G, typename T>
inline std::string Shape<G, T>::ShapeToString() const {
  std::stringstream ss;
  Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
  ss << toArray().format(OctaveFmt);
  return ss.str();
}

// template<typename G, typename T>
// inline bool Shape<G,T>::Collide(const G& shape1, const G& shape2)
//{
//  return Collide(shape1, shape2);
//}

}  // namespace geometry
}  // namespace modules

#endif  // MODULES_GEOMETRY_COMMONS_HPP_


// test suite for Quaternion class
#include <cmath>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <gtest/gtest.h>

#include <isce3/core/DenseMatrix.h>
#include <isce3/core/EulerAngles.h>
#include <isce3/core/Quaternion.h>
#include <isce3/core/Vector.h>

using namespace isce3::core;

struct QuatEulerTest : public ::testing::Test {

    void SetUp() override
    {

        // clang-format off
    sc_pos <<
      -2434573.80388191110,
      -4820642.06528653484,
      4646722.94036952127;

    sc_vel <<
      522.99592536068,
      5107.80853161647,
      5558.15620986960;

    rot_mat <<
      0.0        ,  0.99987663, -0.01570732,
      -0.79863551, -0.0094529 , -0.60174078,
      -0.60181502,  0.01254442,  0.79853698;

    quat_ant2ecf <<
      0.14889715185,
      0.02930644114,
      -0.90605724862,
      -0.39500763650;

    ypr_ant2tcn <<
      -90.06934003*d2r,
      0.78478177*d2r,
      36.99994432*d2r;

        // clang-format on
    }

    // common vars
    const double d2r {M_PI / 180.0};
    const double r2d {1.0 / d2r};
    const double abs_err {1e-10};
    const double yaw {-0.9 * d2r}, pitch {0.06 * d2r}, roll {0.15 * d2r};
    const double mb_ang_deg {37.0};
    const double squint_ang_deg {-0.9};
    Vec3 sc_pos, sc_vel;
    Mat3 rot_mat;
    Vec4 quat_ant2ecf;
    Vec3 ypr_ant2tcn;
};

TEST_F(QuatEulerTest, EulerBasicConstruct)
{
    auto elr = EulerAngles(yaw, pitch, roll);
    ASSERT_NEAR(elr.yaw(), yaw, abs_err) << "Wrong Yaw for Euler obj";
    ASSERT_NEAR(elr.pitch(), pitch, abs_err) << "Wrong Pitch for Euler obj";
    ASSERT_NEAR(elr.roll(), roll, abs_err) << "Wrong Roll for Euler obj";
}

TEST_F(QuatEulerTest, QuatConstructMethod)
{
    //// Constructors

    // from non unity quaternion vector  Vec4
    auto uq_v4 = Quaternion(Vec4(2.0 * quat_ant2ecf));
    EXPECT_NEAR(uq_v4.norm(), 1.0, abs_err)
            << "Quat from Vec4 is not normalied!";
    EXPECT_NEAR((quat_ant2ecf.tail(3) - uq_v4.vec()).norm(), 0.0, abs_err)
            << "Imag/vec part of Quat from Vec4 is not correct!";
    EXPECT_NEAR(std::abs(quat_ant2ecf(0) - uq_v4.w()), 0.0, abs_err)
            << "Real/scalar part of Quat from Vec4 is not correct!";

    // from non-unity 3-D vector Vec3
    auto uq_v3 = Quaternion(sc_pos);
    EXPECT_NEAR(std::abs(uq_v3.w()), 0.0, abs_err)
            << "Real/scalar part of Quat from Vec3 is wrong";
    EXPECT_NEAR((uq_v3.vec() - sc_pos.normalized()).norm(), 0.0, abs_err)
            << "Imag/Vec part of Quat from Vec3 is wrong";

    // from unitary rotmat , Mat3
    auto uq_mat3 = Quaternion(rot_mat);
    EXPECT_NEAR((uq_mat3.toRotationMatrix() - rot_mat).cwiseAbs().maxCoeff(),
            0.0, 1e-8)
            << "Quat from rotation matrix and back fails!";

    // from YPR
    auto uq_yaw = Quaternion(Eigen::AngleAxisd(yaw, Vec3::UnitZ()));
    auto uq_pitch = Quaternion(Eigen::AngleAxisd(pitch, Vec3::UnitY()));
    auto uq_roll = Quaternion(Eigen::AngleAxisd(roll, Vec3::UnitX()));
    auto uq_ypr = Quaternion(yaw, pitch, roll);
    ASSERT_TRUE(uq_ypr.isApprox(uq_yaw * uq_pitch * uq_roll))
            << "Quat from YPR must be the same as AngleAxis products "
               "Yaw*Pitch*Roll";

    // from Euler object
    auto uq_elr = Quaternion(EulerAngles(yaw, pitch, roll));
    EXPECT_TRUE(uq_ypr.isApprox(uq_elr))
            << "Quat from YPR must be equal to Quat from EulerAngles";

    // from angle and 3-D vector
    auto uq_angaxis = Quaternion(yaw, Vec3(2.0 * Vec3::UnitZ()));
    EXPECT_TRUE(uq_angaxis.isApprox(uq_yaw))
            << "Quat from angle Yaw and scaled Z axis shall be Quat from "
               "AngleAxis for yaw";

    //// methods

    // to YPR
    auto ypr_vec = uq_ypr.toYPR();
    EXPECT_NEAR(ypr_vec(0), yaw, abs_err) << "Wrong yaw angle!";
    EXPECT_NEAR(ypr_vec(1), pitch, abs_err) << "Wrong pitch angle!";
    EXPECT_NEAR(ypr_vec(2), roll, abs_err) << "Wrong roll angle!";

    // to isce3 EulerAngle object
    auto elr_obj = uq_ypr.toEulerAngles();
    EXPECT_NEAR(elr_obj.yaw(), yaw, abs_err)
            << "Wrong Yaw angle for EulerAngles Obj";
    EXPECT_NEAR(elr_obj.pitch(), pitch, abs_err)
            << "Wrong Pitch angle for EulerAngles Obj";
    EXPECT_NEAR(elr_obj.roll(), roll, abs_err)
            << "Wrong Roll angle for EulerAngles Obj";

    // to Eigen AngleAxis object
    auto aa_obj = uq_angaxis.toAngleAxis();
    EXPECT_NEAR(std::abs(aa_obj.angle()), std::abs(yaw), abs_err)
            << "Angle must be +/-Yaw";
    EXPECT_NEAR(
            (aa_obj.axis().cwiseAbs() - Vec3::UnitZ()).cwiseAbs().maxCoeff(),
            0.0, abs_err)
            << "Axis must be +/-Z axis!";

    // a practical SAR example via Rotatation of Vec3 in ECEF
    auto uq_ant2ecf = Quaternion(quat_ant2ecf);
    auto ant_ecf = uq_ant2ecf.rotate(Vec3::UnitZ());
    Vec3 center_ecf {-sc_pos.normalized()};
    double mb_ang {r2d * std::acos(center_ecf.dot(ant_ecf))};
    EXPECT_NEAR(mb_ang, mb_ang_deg, 1e-1) << "Wrong Geocentric MB angle!";
    double squint_ang {r2d * std::asin(ant_ecf.dot(sc_vel.normalized()))};
    EXPECT_NEAR(squint_ang, squint_ang_deg, 1e-2) << "Wrong Squint angle";
}

TEST_F(QuatEulerTest, EulerConstructMethod)
{
    //// Constructors
    const auto quat_ypr = Quaternion(yaw, pitch, roll);
    const Mat3 matrot {quat_ypr.toRotationMatrix()};

    // from rotation mat
    auto elr_rotmat = EulerAngles(matrot);
    EXPECT_NEAR(elr_rotmat.yaw(), yaw, abs_err)
            << "Wrong Euler yaw angle from rotmat";
    EXPECT_NEAR(elr_rotmat.pitch(), pitch, abs_err)
            << "Wrong Euler pitch angle from rotmat";
    EXPECT_NEAR(elr_rotmat.roll(), roll, abs_err)
            << "Wrong Euler roll angle from rotmat";

    // from quaternion
    auto elr_quat = EulerAngles(quat_ypr);
    EXPECT_NEAR(elr_quat.yaw(), yaw, abs_err)
            << "Wrong Euler yaw angle from quat";
    EXPECT_NEAR(elr_quat.pitch(), pitch, abs_err)
            << "Wrong Euler pitch angle from quat";
    EXPECT_NEAR(elr_quat.roll(), roll, abs_err)
            << "Wrong Euler roll angle from quat";

    //// Methods

    // toRotationMatrix
    auto rotm = elr_quat.toRotationMatrix();
    EXPECT_NEAR((rotm - matrot).cwiseAbs().maxCoeff(), 0.0, abs_err)
            << "Wrong rotmat from Euler object!";

    // isApprox
    auto elr_other = EulerAngles(
            elr_quat.yaw() + abs_err, elr_quat.pitch(), elr_quat.roll());
    EXPECT_FALSE(elr_other.isApprox(elr_quat, abs_err))
            << "Two Euler angles must not be equal!";
    EXPECT_TRUE(elr_other.isApprox(elr_quat))
            << "Two Euler angles must be equal!";

    // rotate
    auto elr_ant2tcn =
            EulerAngles(ypr_ant2tcn(0), ypr_ant2tcn(1), ypr_ant2tcn(2));
    auto ant_tcn = elr_ant2tcn.rotate(Vec3::UnitZ());
    EXPECT_NEAR(r2d * std::acos(ant_tcn(2)), mb_ang_deg, 1e-2)
            << "Wrong Geodetic MB angle for Euler rotate!";

    // toQuaternion
    auto quat_elr = elr_quat.toQuaternion();
    EXPECT_TRUE(quat_elr.isApprox(quat_ypr, abs_err))
            << "Back and forth conversion between Euler and Quat fails";

    // in-place addition/subtraction
    auto elr_copy = EulerAngles(0.0, 0.0, 0.0);
    elr_copy += elr_quat;
    EXPECT_NEAR(elr_copy.yaw(), yaw, abs_err)
            << "Wrong Euler yaw angle after in-place add";
    EXPECT_NEAR(elr_copy.pitch(), pitch, abs_err)
            << "Wrong Euler pitch angle after in-place add";
    EXPECT_NEAR(elr_copy.roll(), roll, abs_err)
            << "Wrong Euler roll angle after in-place add";

    elr_copy -= elr_quat;
    EXPECT_NEAR(elr_copy.yaw(), 0.0, abs_err)
            << "Wrong Euler yaw angle after in-place sub";
    EXPECT_NEAR(elr_copy.pitch(), 0.0, abs_err)
            << "Wrong Euler pitch angle after in-place sub";
    EXPECT_NEAR(elr_copy.roll(), 0.0, abs_err)
            << "Wrong Euler roll angle after in-place sub";

    // in-place multiplication/concatenation
    elr_copy *= elr_quat;
    EXPECT_NEAR(elr_copy.yaw(), yaw, abs_err)
            << "Wrong Euler yaw angle after in-place mul";
    EXPECT_NEAR(elr_copy.pitch(), pitch, abs_err)
            << "Wrong Euler pitch angle after in-place mul";
    EXPECT_NEAR(elr_copy.roll(), roll, abs_err)
            << "Wrong Euler roll angle after in-place mul";

    // binary add/subtract operators
    auto elr_add = elr_quat + elr_quat;
    EXPECT_NEAR(elr_add.yaw(), 2.0 * yaw, abs_err)
            << "Wrong Euler yaw angle after binary add";
    EXPECT_NEAR(elr_add.pitch(), 2.0 * pitch, abs_err)
            << "Wrong Euler pitch angle after binary add";
    EXPECT_NEAR(elr_add.roll(), 2.0 * roll, abs_err)
            << "Wrong Euler roll angle after binary add";

    elr_add = elr_quat - elr_quat;
    EXPECT_NEAR(elr_add.yaw(), 0.0, abs_err)
            << "Wrong Euler yaw angle after binary sub";
    EXPECT_NEAR(elr_add.pitch(), 0.0, abs_err)
            << "Wrong Euler pitch angle after binary sub";
    EXPECT_NEAR(elr_add.roll(), 0.0, abs_err)
            << "Wrong Euler roll angle after binary sub";

    // binary multiplication/concatenation
    // For simplicity, an approximation is used for validation based on small
    // Euler angles (< 4.0 deg)
    auto elr_mul = (elr_quat + elr_quat) * elr_quat;
    EXPECT_NEAR(elr_mul.yaw(), 3.0 * yaw, 1e-4)
            << "Wrong Euler yaw angle after binary mul";
    EXPECT_NEAR(elr_mul.pitch(), 3.0 * pitch, 1e-4)
            << "Wrong Euler pitch angle after binary mul";
    EXPECT_NEAR(elr_mul.roll(), 3.0 * roll, 1e-4)
            << "Wrong Euler roll angle after binary mul";
}

int main(int argc, char** argv)
{

    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


#include <NTL/ZZ_pX.h>
#include <NTL/lzz_pX.h>
#include <NTL/GF2X.h>

#include <NTL/version.h>

NTL_CLIENT

#define make_string_aux(x) #x
#define make_string(x) make_string_aux(x)

int SmallModulusTest(long p, long n)
{
   zz_pBak bak;

   bak.save();

   zz_p::init(p);

   zz_pX a, b, c, cc;

   random(a, n);
   random(b, n);
   PlainMul(c, a, b);
   FFTMul(cc, a, b);

   int res;
   res = (c != cc);

   bak.restore();

   return res;
}

int GF2X_test()
{
   GF2X a, b, c, c1;

   long n;

#ifdef NTL_GF2X_LIB
   for (n = 32; n <= (1L << 18); n = n << 1) {
      random(a, n);
      random(b, n);
      OldMul(c, a, b);
      mul(c1, a, b);
      if (c1 != c) return 1;
   }
#endif

   return 0;
}

void GF2X_time()
{
   long n = 1000000L;
   long iter;

   GF2X a, b, c;

   double t;
   long i;

   random(a, n);
   random(b, n);

   mul(c, a, b);

   iter = 0;
   do {
      iter = iter ? (2*iter) : 1;
      t = GetTime();
      for (i = 0; i < iter; i++)
         mul(c, a, b);
      t = GetTime() - t;
   } while (t < 0.5);

   cerr << "time to multiply polynomials over GF(2) \n   of degree < 1000000: "
        << (t/iter) << "s\n";

#ifdef NTL_GF2X_LIB
   OldMul(c, a, b);

   iter = 0;
   do {
      iter = iter ? (2*iter) : 1;
      t = GetTime();
      for (i = 0; i < iter; i++)
         OldMul(c, a, b);
      t = GetTime() - t;
   } while (t < 0.5);

   cerr << "   **** using old code: "  << (t/iter) << "s\n";
#endif

}

int main()
{

   cerr << "This is NTL version " << NTL_VERSION << "\n";

   cerr << "Basic Configuration Options:\n";

#ifdef NTL_STD_CXX
   cerr << "NTL_STD_CXX\n";
#endif

#ifdef NTL_PSTD_NNS
   cerr << "NTL_PSTD_NNS\n";
#endif

#ifdef NTL_PSTD_NHF
   cerr << "NTL_PSTD_NHF\n";
#endif

#ifdef NTL_PSTD_NTN
   cerr << "NTL_PSTD_NTN\n";
#endif

#ifdef NTL_GMP_LIP
   cerr << "NTL_GMP_LIP\n";
#endif

#ifdef NTL_GMP_HACK
   cerr << "NTL_GMP_HACK\n";
#endif

#ifdef NTL_GF2X_LIB
   cerr << "NTL_GF2X_LIB\n";
#endif

#ifdef NTL_LONG_LONG_TYPE
   cerr << "NTL_LONG_LONG_TYPE: ";
   cerr << make_string(NTL_LONG_LONG_TYPE) << "\n";
#endif

#ifdef NTL_UNSIGNED_LONG_LONG_TYPE
   cerr << "NTL_UNSIGNED_LONG_LONG_TYPE: ";
   cerr << make_string(NTL_UNSIGNED_LONG_LONG_TYPE) << "\n";
#endif

#ifdef NTL_CXX_ONLY
   cerr << "NTL_CXX_ONLY\n";
#endif

#ifdef NTL_X86_FIX
   cerr << "NTL_X86_FIX\n";
#endif

#ifdef NTL_NO_X86_FIX
   cerr << "NTL_NO_X86_FIX\n";
#endif

#ifdef NTL_NO_INIT_TRANS
   cerr << "NTL_NO_INIT_TRANS\n";
#endif

#ifdef NTL_CLEAN_INT
   cerr << "NTL_CLEAN_INT\n";
#endif

#ifdef NTL_CLEAN_PTR
   cerr << "NTL_CLEAN_PTR\n";
#endif

#ifdef NTL_RANGE_CHECK
   cerr << "NTL_RANGE_CHECK\n";
#endif

cerr << "\n";
cerr << "Resolution of double-word types:\n";
cerr << make_string(NTL_LL_TYPE) << "\n";
cerr << make_string(NTL_ULL_TYPE) << "\n";

cerr << "\n";
cerr << "Performance Options:\n";

#ifdef NTL_LONG_LONG
   cerr << "NTL_LONG_LONG\n";
#endif

#ifdef NTL_AVOID_FLOAT
   cerr << "NTL_AVOID_FLOAT\n";
#endif

#ifdef NTL_SPMM_UL
   cerr << "NTL_SPMM_UL\n";
#endif

#ifdef NTL_SPMM_ULL
   cerr << "NTL_SPMM_ULL\n";
#endif

#ifdef NTL_SPMM_ASM
   cerr << "NTL_SPMM_ASM\n";
#endif

#ifdef NTL_AVOID_BRANCHING
   cerr << "NTL_AVOID_BRANCHING\n";
#endif

#ifdef NTL_TBL_REM
   cerr << "NTL_TBL_REM\n";
#endif

#ifdef NTL_GF2X_ALTCODE
   cerr << "NTL_GF2X_ALTCODE\n";
#endif

#ifdef NTL_GF2X_ALTCODE1
   cerr << "NTL_GF2X_ALTCODE1\n";
#endif

#ifdef NTL_GF2X_NOINLINE
   cerr << "NTL_GF2X_NOINLINE\n";
#endif

   cerr << "\n\n";

   if (_ntl_gmp_hack)
      cerr << "using GMP hack\n\n";

   cerr << "running tests...";

   long n, k;

   n = 200;
   k = 10*NTL_ZZ_NBITS;

   ZZ p;

   GenPrime(p, k);

   ZZ_p::init(p);         // initialization

   ZZ_pX f, g, h, r1, r2, r3;

   random(g, n);    // g = random polynomial of degree < n
   random(h, n);    // h =             "   "
   random(f, n);    // f =             "   "

   // SetCoeff(f, n);  // Sets coefficient of X^n to 1

   ZZ_p lc;

   do {
      random(lc);
   } while (IsZero(lc));

   SetCoeff(f, n, lc);

   // For doing arithmetic mod f quickly, one must pre-compute
   // some information.

   ZZ_pXModulus F;
   build(F, f);

   PlainMul(r1, g, h);  // this uses classical arithmetic
   PlainRem(r1, r1, f);

   MulMod(r2, g, h, F);  // this uses the FFT

   MulMod(r3, g, h, f);  // uses FFT, but slower

   // compare the results...

   if (r1 != r2) {
      cerr << "r1 != r2!!\n";
      return 1;
   }
   else if (r1 != r3) {
      cerr << "r1 != r3!!\n";
      return 1;
   }

   // small prime tests...I've made some changes in v5.3
   // that should be checked on various platforms, so
   // we might as well check them here.

   if (SmallModulusTest(17, 1000)) {
      cerr << "first SmallModulusTest failed!!\n";
      return 1;
   }

   if (SmallModulusTest((1L << (NTL_SP_NBITS))-1, 1000)) {
      cerr << "second SmallModulusTest failed!!\n";
      return 1;
   }

   // Test gf2x code....

   if (GF2X_test()) {
      cerr << "GF2X test failed!\n";
      return 1;
   }

   cerr << "OK\n";

   ZZ x1, x2, x3, x4;
   double t;
   long i;

   RandomLen(x1, 1024);
   RandomBnd(x2, x1);
   RandomBnd(x3, x1);

   mul(x4, x2, x3);

   t = GetTime();
   for (i = 0; i < 100000; i++)
      mul(x4, x2, x3);
   t = GetTime()-t;

   cerr << "time for 1024-bit mul: " << t*10 << "us";

   if (_ntl_gmp_hack) {
      _ntl_gmp_hack = 0;
      mul(x4, x2, x3);

      t = GetTime();
      for (i = 0; i < 100000; i++)
         mul(x4, x2, x3);
      t = GetTime()-t;

      cerr << " (" << (t*10) << "us without GMP)";

      _ntl_gmp_hack = 1;
   }

   cerr << "\n";

   rem(x2, x4, x1);

   t = GetTime();
   for (i = 0; i < 100000; i++)
      rem(x2, x4, x1);
   t = GetTime()-t;

   cerr << "time for 2048/1024-bit rem: " << t*10 << "us";

   if (_ntl_gmp_hack) {
      _ntl_gmp_hack = 0;
      rem(x2, x4, x1);

      t = GetTime();
      for (i = 0; i < 100000; i++)
         rem(x2, x4, x1);
      t = GetTime()-t;
      cerr << " (" << (t*10) << "us without GMP)";

      _ntl_gmp_hack = 1;
   }

   cerr << "\n";

   GenPrime(p, 1024);
   RandomBnd(x1, p);
   if (IsZero(x1)) set(x1);

   InvMod(x2, x1, p);

   t = GetTime();
   for (i = 0; i < 1000; i++)
      InvMod(x2, x1, p);
   t = GetTime()-t;

   cerr << "time for 1024-bit modular inverse: " << t*1000 << "us";

   if (_ntl_gmp_hack) {
      _ntl_gmp_hack = 0;
      InvMod(x2, x1, p);

      t = GetTime();
      for (i = 0; i < 1000; i++)
         InvMod(x2, x1, p);
      t = GetTime()-t;
         cerr << " (" << (t*1000) << "us without GMP)";

      _ntl_gmp_hack = 1;
   }

   cerr << "\n";

   // test modulus switching

   n = 1024;
   k = 1024;
   RandomLen(p, k);

   ZZ_p::init(p);
   ZZ_pInfo->check();

   ZZ_pX j1, j2, j3;

   random(j1, n);
   random(j2, n);

   t = GetTime();
   for (i = 0; i < 20; i++) mul(j3, j1, j2);
   t = GetTime()-t;

   cerr << "time to multiply degree 1023 polynomials\n   modulo a 1024-bit number: ";
   cerr << (t/20) << "s";

   if (_ntl_gmp_hack) {
      _ntl_gmp_hack = 0;

      ZZ_p::init(p);
      ZZ_pInfo->check();

      t = GetTime();
      for (i = 0; i < 20; i++) mul(j3, j1, j2);
      t = GetTime()-t;

      cerr << " (" << (t/20) << "s without GMP)";
      _ntl_gmp_hack = 1;
   }

   cerr << "\n";

   GF2X_time();

   return 0;
}


/* mwm
 *
 */

#pragma once

#ifdef _MICROCONTROLLER
#include "../matrix/matrix/math.hpp"
#else
#include <Eigen/Dense>
#endif

#ifdef _MICROCONTROLLER
//we use the PX4 class
template<typename Real, int M, int N>
using Matrix = matrix::Matrix<Real, M, N>;

template<typename Real, int M>
using SquareMatrix = matrix::SquareMatrix<Real, M>;

template<typename Real, int M, int N>
Matrix<Real, M,N> ZeroMatrix() {
  Matrix<Real, M, N> m;
  m.zero();
  return m;
}

template<typename Real, int M>
SquareMatrix<Real, M> IdentityMatrix() {
  SquareMatrix<Real, M> m;
  m.identity();
  return m;
}

#else
template<typename Real, int M, int N>
using Matrix = Eigen::Matrix<Real, M, N>;
template<typename Real, int M>
using SquareMatrix = Eigen::Matrix<Real, M, M>;

template<typename Real, int M, int N>
Matrix<Real, M, N> ZeroMatrix() {
  return Matrix<Real, M, N>::Zero();
}

template<typename Real, int M>
SquareMatrix<Real, M> IdentityMatrix() {
  return SquareMatrix<Real, M>::Identity();

}
#endif


#include <boost/foreach.hpp>
#include <cfloat>
#include "ConvexHull.hpp"

using namespace std;

namespace
{
	Vector2D GetReflectedPoint(Edge const& edge,Vector2D const& point)
	{
	  const Vector2D par(normalize(Parallel(edge)));
	  const Vector2D edge0=edge.vertices.first;
	  const Vector2D temp=point-edge0;
	  return 2*par*ScalarProd(par,temp)-temp+edge0;
	}

  bool check_same_point(const vector<Vector2D>& vertices,
			const Vector2D& p,
			double tol)
  {
    BOOST_FOREACH(const Vector2D& v, vertices)
      {
	if(dist_sqr(v-p)<tol)
	  return true;
      }
    return false;
  }
}

void ConvexHull(vector<Vector2D> &result,Tessellation const& tess,int index)
{
	vector<int> edge_index=tess.GetCellEdges(index);
	const double eps=1e-14;
	vector<Vector2D> points;
	points.reserve(10);
	double R=tess.GetWidth(index);
	points.push_back(tess.GetEdge(edge_index[0]).vertices.first);
	points.push_back(tess.GetEdge(edge_index[0]).vertices.second);
	// Remove identical points
	for(size_t i=1;i<edge_index.size();++i)
	{
	  const Edge& edge = tess.GetEdge(edge_index[i]);
		if(!check_same_point(points,edge.vertices.first,eps*pow(R,2)))
		  points.push_back(edge.vertices.first);
		if(!check_same_point(points,edge.vertices.second,eps*pow(R,2)))
		  points.push_back(edge.vertices.second);
	}

	const Vector2D cm = tess.GetCellCM(index);

	// Start building the convexhull
	size_t n=points.size();
	vector<double> angles(n);
	for(size_t i=0;i<n;++i)
	  angles.at(i)=atan2(points.at(i).y-cm.y,points.at(i).x-cm.x);
	const vector<size_t> indeces = sort_index(angles);
	result = VectorValues(points,indeces);
}

void ConvexEdges(vector<int> &result,Tessellation const& tess,int index)
{
	vector<int> const& edges=tess.GetCellEdges(index);
	const Vector2D mypoint=tess.GetMeshPoint(index);
	int nedges=static_cast<int>(edges.size());
	result.resize(static_cast<size_t>(nedges));
	vector<double> angles(static_cast<size_t>(nedges));
	for(int i=0;i<nedges;++i)
	{
		Edge const& edge=tess.GetEdge(edges[static_cast<size_t>(i)]);
		const int other=(edge.neighbors.first==index)? edge.neighbors.second : edge.neighbors.first;
		Vector2D otherpoint=(other==-1) ? GetReflectedPoint(edge,mypoint) : tess.GetMeshPoint(other);
		angles[static_cast<size_t>(i)]=atan2(otherpoint.y-mypoint.y,otherpoint.x-mypoint.x);
	}
	vector<int> temp;
	sort_index(angles,temp);
	for(size_t i=0;i<static_cast<size_t>(nedges);++i)
	  result[i]=edges[static_cast<size_t>(temp[i])];
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm algebra elementary log10
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/algorithm/algebra/elementary/log10.h"

namespace fa = fern::algorithm;

template<
    class Value>
using OutOfDomainPolicy = fa::log10::OutOfDomainPolicy<Value>;

BOOST_AUTO_TEST_CASE(out_of_domain_policy)
{
    {
        OutOfDomainPolicy<double> policy;
        BOOST_CHECK( policy.within_domain( 5));
        BOOST_CHECK(!policy.within_domain(-5));
        BOOST_CHECK( policy.within_domain( 0));
        BOOST_CHECK( policy.within_domain(-0));
    }
}

template<
    class Value,
    class Result>
void verify_value(
    Value const& value,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::algebra::log10(sequential, value, result_we_get);
    BOOST_CHECK_EQUAL(result_we_get, result_we_want);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    verify_value<float, float>( 0.0f, -fern::infinity<float>());
    verify_value<float, float>( 1.0f, 0.0f);
    verify_value<float, float>( 9.0f, std::log10(9.0f));
}


// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

/// \file
/// \brief test core essential matrix class

#include <test_eigen.h>

#include <vital/types/essential_matrix.h>

#include <Eigen/SVD>

#include <iostream>
#include <vector>

static constexpr double pi = 3.14159265358979323846;

using namespace kwiver::vital;

// ----------------------------------------------------------------------------
int main(int argc, char** argv)
{
  ::testing::InitGoogleTest( &argc, argv );
  return RUN_ALL_TESTS();
}

// ----------------------------------------------------------------------------
static bool is_similar(
  matrix_3x3d const& m1, matrix_3x3d const& m2, double tol )
{
  return kwiver::testing::similar_matrix_comparator{}( m1, m2, tol );
}

// ----------------------------------------------------------------------------
TEST(essential_matrix, constructors)
{
  rotation_d rot_d(vector_3d(0.0, 0.0, 0.0));
  rotation_f rot_f(vector_3f(0.0, 0.0, 0.0));
  vector_3d t_d(0.0, 1.0, 0.0);
  vector_3f t_f(0.0, 1.0, 0.0);
  essential_matrix_d d1 = essential_matrix_d(rot_d, t_d);
  essential_matrix_f f1 = essential_matrix_f(rot_f, t_f);

  essential_matrix_d d2 = essential_matrix_d(d1);
  essential_matrix_f f2 = essential_matrix_f(f1);

  EXPECT_MATRIX_SIMILAR(d1.matrix(), d2.matrix(), 1e-5);
  EXPECT_MATRIX_SIMILAR(f1.matrix(), f2.matrix(), 1e-5);

  essential_matrix_f f_from_d(d1);
  EXPECT_MATRIX_SIMILAR(f_from_d.matrix(), f1.matrix(), 1e-5);

  essential_matrix_d d_from_f(f1);
  EXPECT_MATRIX_SIMILAR(d_from_f.matrix(), d1.matrix(), 1e-5);
}

// ----------------------------------------------------------------------------
TEST(essential_matrix, twisted_rotation)
{
  rotation_d rot(vector_3d(0.0, 0.0, 0.0));
  vector_3d t(0.48, 0.6, 0.64);
  essential_matrix_d m(rot, t);
  rotation_d twist = m.twisted_rotation();
  EXPECT_EQ(twist.quaternion().x(), t.x());
  EXPECT_EQ(twist.quaternion().y(), t.y());
  EXPECT_EQ(twist.quaternion().z(), t.z());
  EXPECT_EQ(twist.quaternion().w(), 0.0);
}

// ----------------------------------------------------------------------------
TEST(essential_matrix, clone)
{
  rotation_d rot(vector_3d(0.0, 0.0, 0.0));
  vector_3d t(0.48, 0.6, 0.64);
  essential_matrix_d m(rot, t);
  essential_matrix_sptr m_clone = m.clone();
  EXPECT_MATRIX_SIMILAR(m.matrix(), m_clone->matrix(), 1e-12);
}

// ----------------------------------------------------------------------------
TEST(essential_matrix, get)
{
  rotation_d rot(vector_3d(0.0, 0.0, 0.0));
  vector_3d t(0.48, 0.6, 0.64);
  essential_matrix_d m(rot, t);
  EXPECT_EQ(m.get_rotation(), rot);
  EXPECT_EQ(m.get_translation(), t);
}

// ----------------------------------------------------------------------------
TEST(essential_matrix, properties)
{
  rotation_d rot(vector_3d(1.0, 2.0, 3.0));
  vector_3d t(-1.0, 1.0, 4.0);

  essential_matrix_d em(rot, t);
  matrix_3x3d mat = em.matrix();

  Eigen::JacobiSVD<matrix_3x3d> svd(mat, Eigen::ComputeFullV |
                                         Eigen::ComputeFullU);
  EXPECT_MATRIX_NEAR( ( vector_3d{ 1, 1, 0 } ), svd.singularValues(), 1e-14 );
  EXPECT_NEAR( 1.0, em.translation().norm(), 1e-14 );

  const matrix_3x3d W = (matrix_3x3d() << 0.0, -1.0, 0.0,
                                          1.0,  0.0, 0.0,
                                          0.0,  0.0, 1.0).finished();
  const matrix_3x3d& U = svd.matrixU();
  const matrix_3x3d& V = svd.matrixV();
  vector_3d t_extracted = U.col(2);

  vector_3d t_norm = t.normalized();
  EXPECT_MATRIX_SIMILAR( t_extracted, t_norm, 1e-14 );

  matrix_3x3d R1_extracted = U*W*V.transpose();
  matrix_3x3d R2_extracted = U*W.transpose()*V.transpose();

  if ( !is_similar( rot.matrix(), R1_extracted, 1e-14 ) &&
       !is_similar( rot.matrix(), R2_extracted, 1e-14 ) )
  {
    ADD_FAILURE()
      << "Extracted rotation should match input or twisted pair\n"
      << "Input:\n" << rot.matrix() << "\n"
      << "Result (v1):\n" << R1_extracted << "\n"
      << "Result (v2):\n" << R2_extracted;
  }
}

// ----------------------------------------------------------------------------
TEST(essential_matrix, twisted_pair)
{
  rotation_d rot(vector_3d(1.0, 2.0, 3.0));
  vector_3d t(-1.0, 1.0, 4.0);

  essential_matrix_d em(rot, t);

  // any combination of these should be an equivalent essential matrix
  rotation_d R1 = em.rotation();
  rotation_d R2 = em.twisted_rotation();
  vector_3d t1 = em.translation();
  vector_3d t2 = -t1;

  rotation_d rot_t_180{ pi, t.normalized() };
  EXPECT_MATRIX_NEAR( ( rot_t_180 * R1 ).matrix(), R2.matrix(), 1e-14 )
    << "Twisted pair rotation should be 180 degree rotation around t";

  essential_matrix_d em1(R1, t1), em2(R1, t2), em3(R2, t1), em4(R2, t2);
  matrix_3x3d M1(em1.matrix()), M2(em2.matrix()),
              M3(em3.matrix()), M4(em4.matrix());
  matrix_3x3d M(em.matrix());

  EXPECT_MATRIX_SIMILAR( M, M1, 1e-14 )
    << "Possible factorization 1 should match source";
  EXPECT_MATRIX_SIMILAR( M, M2, 1e-14 )
    << "Possible factorization 2 should match source";
  EXPECT_MATRIX_SIMILAR( M, M3, 1e-14 )
    << "Possible factorization 3 should match source";
  EXPECT_MATRIX_SIMILAR( M, M4, 1e-14 )
    << "Possible factorization 4 should match source";
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_CORE_FUNCTIONS_GLOBALNORM_HPP_INCLUDED
#define NT2_CORE_FUNCTIONS_GLOBALNORM_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <boost/mpl/int.hpp>

namespace nt2 { namespace tag
  {
    /*!
      @brief globalnorm generic tag

      Represents the globalnorm function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct globalnorm_ : ext::abstract_<globalnorm_>
    {
      /// @brief Parent hierarchy
      typedef ext::abstract_<globalnorm_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_globalnorm_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::globalnorm_, Site> dispatching_globalnorm_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::globalnorm_, Site>();
    }
    template<class... Args>
    struct impl_globalnorm_;
  }

  /*!
    @brief Global norm

    Computes the norm of a whole table expression with static or dynamic choice
    of the norm computation formula.

    Call protocols to globalnorm are summarized in the following table. We advise to use static
    calls whenever possible as it prevents cascaded run-time if clauses and goes directly
    to the right call at execution.

    @code
    |--------------------|-------------------|------------------------------|-------------------|
    | mnorm(a0, p)                                                                              |
    |--------------------|-------------------|------------------------------|-------------------|
    |    static p        |  dynamic p        |     formula (pseudo-code)    |  equivalent to    |
    |--------------------|-------------------|------------------------------|-------------------|
    | nt2::one_          | 1                 |       sum(abs(x(_)))         | globalnorm1(x)    |
    | nt2::two_          | 2                 |   sqrt(sum(sqr(abs(x(_)))))  | globalnorm2(x)    |
    |    -               | p (positive)      |    sum(abs(x)^p)^(1/p)       | globalnormp(x,p)  |
    | nt2::inf_          | nt2::Inf<T>()     |       max(abs((x))           | globalnorminf(x)  |
    | nt2::fro_          | -1                |   sqrt(sum(sqr(abs(x(_)))))  | globalnormfro(x)  |
    |--------------------|-------------------|------------------------------|-------------------|
    | mnorm<p>(a0)                                                                              |
    |--------------------|-------------------|------------------------------|-------------------|
    |    static p        |                   |     matrix                   |                   |
    |--------------------|-------------------|------------------------------|-------------------|
    | nt2::tag::one_ or 1|        -          |       sum(abs(x(_)))         |  globalnorm1(x)   |
    | nt2::tag::two_ or 2|        -          |   sqrt(sum(sqr(abs(x(_)))))  |  globalnorm2(x)   |
    | p  (integer only)  |        -          |    sum(abs(x)^p)^(1/p)       |  globalnormp(x,p) |
    | nt2::tag::inf_     |        -          |       max(abs((x))           |  globalnorminf(x) |
    | nt2::tag::fro_     |        -          |   sqrt(sum(sqr(abs(x(_)))))  |  globalnormfro(x) |
    |--------------------|-------------------|------------------------------|-------------------|
    @endcode

    @par Semantic:

    For any expression @c a0 of type @c A0, the following call:

    @code
    as_real<A0::value_type>::type x = globalnorm(a0);
    @endcode

    is equivalent to:

    @code
    as_real<A0::value_type>::type x = globalnorm2(a0);
    @endcode

    For any expression @c a0 of type @c A0 and any floating point value @c p, the
    following call:

    @code
    as_real<A0::value_type>::type x = globalnorm(a0,p);
    @endcode

    is equivalent to:

    @code
    as_real<A0::value_type>::type x = globalnormp(a0,p);
    @endcode

    if @c p is finite and to :

    @code
    as_real<A0::value_type>::type x = globalmax(abs(a0));
    @endcode

    if @c is +Inf and to :

    @code
    as_real<A0::value_type>::type x = globalmin(abs(a0));
    @endcode

    if @c p is -Inf.

    @note If 0 < p < 1 or p = -inf, globalnorm does not share the properties that
    define a mathematical norm,  but only a quasi-norm if  0 < p < 1 and a notation
    facility for p = -inf.

    @par Static Interface

    globalnorm can also be invoked with a template parameter which is either a
    functor tag describing the constant value to use instead of @c p or an
    Integral Constant. For example,

    @code
    as_real<A0::value_type>::type x = globalnorm<tag::two_>(a0);
    @endcode

    is equivalent to:

    @code
    as_real<A0::value_type>::type x = globalnorm2(a0);
    @endcode

    Similarly,

    @code
    as_real<A0::value_type>::type x = globalnorm<5>(a0);
    @endcode

    is equivalent to:

    @code
    as_real<A0::value_type>::type x = globalnormp(a0,5);
    @endcode

    @note Whenever a constant functor tag or an Integral Constant is used, compile
    time optimization is performed (if available) so the correct variant of globalnorm is
    called. For example, calls similar to globalnorm<2>(a0) will invoke
    globalnorm2(a0) instead of globalnormp(a0, 2), and globalnorm<5>(a0) will simply
    invoke directly globalnormp(a0, 5) but without any runtime selection.

    @param a0 Expression to compute the norm of
    @param a1 Type of norm to compute
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(nt2::tag::globalnorm_, globalnorm, 2)

  /// @overload
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(nt2::tag::globalnorm_, globalnorm, 1)

  /// @overload
  template<typename Tag, typename A0>
  BOOST_FORCEINLINE typename meta::as_real<typename A0::value_type>::type
  globalnorm(const A0& a0)
  {
    return globalnorm(a0, nt2::meta::as_<Tag>());
  }

  /// @overload
  template<int Value, typename A0>
  BOOST_FORCEINLINE typename meta::as_real<typename A0::value_type>::type
  globalnorm(const A0& a0)
  {
    return globalnorm(a0, boost::mpl::int_<Value>() );
  }
}

#endif


#include "eigen_csv.hpp"

#include <Eigen/Dense>

#include <cmath>
#include <iostream>

// Generate filename for state estimates
std::string est_filename(const std::string& filter, int i, int j) {
    std::string f = "out/ct_est_";
    f += filter;
    f += "_";
    f += std::to_string(i+1);
    f += "_";
    f += std::to_string(j+1);
    f += ".csv";
    return f;
}

int main() {

    using namespace std;
    using namespace Eigen;

    string filename;

    // Number of tests
    int Ntest = 100;

    // Number of filters & filter names
    int Nfilters = 5;
    vector<string> filter_names(Nfilters);
    filter_names[0] = "ukf";
    filter_names[1] = "cut4";
    filter_names[2] = "cut6";
    filter_names[3] = "cut8";
    filter_names[4] = "house";

    // Read times
    VectorXd T;
    EigenCSV::read("out/times.csv", false, true, T);
    int Ntimes = T.size();

    // Results table
    MatrixXd table(Ntimes, 1+3*Nfilters);

    for (int l = 0; l < Nfilters; l++) {

        VectorXd
        pos_rmse(Ntimes), vel_rmse(Ntimes), ome_rmse(Ntimes),
        pos_avge(Ntimes), vel_avge(Ntimes), ome_avge(Ntimes),
        pos_stde(Ntimes), vel_stde(Ntimes), ome_stde(Ntimes);

        for (int i = 0; i < Ntimes; i++) {

            MatrixXd tabtru, pos_tru, vel_tru, ome_tru;

            filename = "out/ct_true_";
            filename += to_string(i+1);
            filename += ".csv";

            EigenCSV::read(filename, true, true, tabtru);

            int nt = tabtru.rows();

            pos_tru.resize(nt, 2);
            vel_tru.resize(nt, 2);
            ome_tru.resize(nt, 1);

            pos_tru << tabtru.col(1), tabtru.col(3);
            vel_tru << tabtru.col(2), tabtru.col(4);
            ome_tru = tabtru.col(5);

            MatrixXd pos_err(nt*Ntest, 2), vel_err(nt*Ntest, 2), ome_err(nt*Ntest, 1);

            for (int j = 0; j < Ntest; j++) {

                MatrixXd tabest, pos_est(nt,2), vel_est(nt,2), ome_est(nt,1);

                filename = est_filename(filter_names[l], i, j);

                EigenCSV::read(filename, true, true, tabest);

                pos_est << tabest.col(1), tabest.col(3);
                vel_est << tabest.col(2), tabest.col(4);
                ome_est = tabest.col(5);

                pos_err.block(j*nt, 0, nt, 2) = pos_est - pos_tru;
                vel_err.block(j*nt, 0, nt, 2) = vel_est - vel_tru;
                ome_err.block(j*nt, 0, nt, 1) = ome_est - ome_tru;

            }

            pos_rmse(i) = sqrt(pos_err.array().square().mean() * 2);
            vel_rmse(i) = sqrt(vel_err.array().square().mean() * 2);
            ome_rmse(i) = sqrt(ome_err.array().square().mean());

            pos_avge(i) = pos_err.mean();
            vel_avge(i) = vel_err.mean();
            ome_avge(i) = ome_err.mean();

            pos_stde(i) = sqrt((pos_err.array() - pos_avge(i)).square().mean());
            vel_stde(i) = sqrt((vel_err.array() - pos_avge(i)).square().mean());
            ome_stde(i) = sqrt((ome_err.array() - ome_avge(i)).square().mean());

        }

        MatrixXd tabfil(Ntimes, 10);
        tabfil << T, pos_rmse, pos_avge, pos_stde,
                     vel_rmse, vel_avge, vel_stde,
                     ome_rmse, ome_avge, ome_stde;

        vector<string> header(10);
        header[0] = "TIME";
        header[1] = "POS RMSE"; header[2] = "POS AVGE"; header[3] = "POS STDE";
        header[4] = "VEL RMSE"; header[5] = "VEL AVGE"; header[6] = "VEL STDE";
        header[7] = "OME RMSE"; header[8] = "OME AVGE"; header[9] = "OME STDE";

        filename = "out/ct_err_";
        filename += filter_names[l];
        filename += ".csv";

        EigenCSV::write(tabfil, header, filename);

    }

    return 0;

}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_triangulation.h"

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

#include "theia/sfm/camera/camera.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/sfm/types.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/ransac.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {

namespace {
// The pixel observation and projection matrix needed in order to triangulate a
// 3D point.
struct PointObservation {
  Matrix3x4d projection_matrix;
  Camera camera;
  Eigen::Vector2d normalized_feature;
  Eigen::Vector2d observed_pixel;
};

// Returns true if the point is in front of the camera and false if the point is
// behind the camera.
bool IsPointInFrontOfCamera(const Matrix3x4d& projection_matrix,
                            const Eigen::Vector4d& point) {
  return point.dot(projection_matrix.row(2)) > 0;
}

class TriangulationEstimator
    : public Estimator<PointObservation, Eigen::Vector4d> {
 public:
  TriangulationEstimator() {}

  double SampleSize() const { return 2; }

  // Triangulates the 3D point from 2 observations.
  bool EstimateModel(const std::vector<PointObservation>& observations,
                     std::vector<Eigen::Vector4d>* triangulated_points) const {
    triangulated_points->resize(1);
    if (!Triangulate(observations[0].projection_matrix,
                     observations[1].projection_matrix,
                     observations[0].normalized_feature,
                     observations[1].normalized_feature,
                     &triangulated_points->at(0))) {
      return false;
    }

    // Only return true if the point is in front of both cameras and the
    // triangulation was a success.
    return IsPointInFrontOfCamera(observations[0].projection_matrix,
                                  triangulated_points->at(0)) &&
           IsPointInFrontOfCamera(observations[1].projection_matrix,
                                  triangulated_points->at(0));
  }

  double Error(const PointObservation& observation,
               const Eigen::Vector4d& triangulated_point) const {
    Eigen::Vector2d reprojection;
    const double depth =
        observation.camera.ProjectPoint(triangulated_point, &reprojection);
    if (depth <= 0) {
      return std::numeric_limits<double>::max();
    }
    return (observation.observed_pixel - reprojection).squaredNorm();
  }
};

}  // namespace

bool EstimateTriangulation(const RansacParameters& ransac_params,
                           const std::vector<Camera>& cameras,
                           const std::vector<theia::Feature>& features,
                           Eigen::Vector4d* triangulated_point,
                           RansacSummary* summary) {
  CHECK_EQ(cameras.size(), features.size());
  CHECK_NOTNULL(triangulated_point);

  // If we only have a few data points, then we should exhaustively search the
  // solution space for the best combination.
  static const int kMaxNumDataPointsForExhaustiveSearch = 15;
  if (cameras.size() < 2) {
    return false;
  }

  // Create point correspondences.
  std::vector<PointObservation> point_observations(cameras.size());
  for (int i = 0; i < point_observations.size(); i++) {
    // Create the projection atirx.
    Matrix3x4d projection_matrix;
    projection_matrix.leftCols<3>() =
        cameras[i].GetOrientationAsRotationMatrix();
    projection_matrix.rightCols<1>() =
        -projection_matrix.leftCols<3>() * cameras[i].GetPosition();

    point_observations[i].projection_matrix = projection_matrix;
    point_observations[i].camera = cameras[i];
    point_observations[i].normalized_feature =
        cameras[i].PixelToNormalizedCoordinates(features[i].point_).hnormalized();
    point_observations[i].observed_pixel = features[i].point_;
  }

  // RANSAC triangulation.
  TriangulationEstimator triangulation_estimator;
  std::unique_ptr<SampleConsensusEstimator<TriangulationEstimator> > ransac;
  if (cameras.size() <= kMaxNumDataPointsForExhaustiveSearch) {
    // Set the minimum number of iterations to be the exact number of possible
    // combinations. This forces all combinations to be tested.
    const int num_combinations = cameras.size() * (cameras.size() - 1) / 2;
    RansacParameters exhaustive_params = ransac_params;
    exhaustive_params.min_iterations = num_combinations;
    exhaustive_params.max_iterations = num_combinations;
    ransac = CreateAndInitializeRansacVariant<TriangulationEstimator>(
        RansacType::EXHAUSTIVE, exhaustive_params, triangulation_estimator);
  } else {
    ransac = CreateAndInitializeRansacVariant<TriangulationEstimator>(
        RansacType::RANSAC, ransac_params, triangulation_estimator);
  }

  // Run the RANSAC scheme.
  CHECK(ransac->Initialize());
  return ransac->Estimate(point_observations, triangulated_point, summary);
}

}  // namespace theia


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "MLP.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidDataSet.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/FluidTensor.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <limits>
#include <random>

namespace fluid {
namespace algorithm {

class SGD
{
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXXd = Eigen::ArrayXXd;
  using Permutation = Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic>;

public:
  explicit SGD() = default;
  ~SGD() = default;

  double train(MLP& model, const RealMatrixView in, RealMatrixView out,
               index nIter, index batchSize, double learningRate,
               double momentum, double valFrac)
  {
    using namespace _impl;
    using namespace std;
    using namespace Eigen;
    index       nExamples = in.rows();
    index       inputSize = in.cols();
    index       outputSize = out.cols();
    ArrayXXd    input = asEigen<Eigen::Array>(in);
    ArrayXXd    output = asEigen<Eigen::Array>(out);
    Permutation valPerm(nExamples);
    valPerm.setIdentity();
    shuffle(valPerm.indices().data(),
            valPerm.indices().data() + valPerm.indices().size(),
            mt19937{random_device{}()});
    input = valPerm * input.matrix();
    output = valPerm * output.matrix();
    index nVal = std::lround(nExamples * valFrac);
    index nTrain = nExamples - nVal;

    ArrayXXd trainInput = input.block(0, 0, nTrain, inputSize);
    ArrayXXd trainOutput = output.block(0, 0, nTrain, outputSize);
    ArrayXXd valInput = input.block(nTrain, 0, nVal, inputSize);
    ArrayXXd valOutput = output.block(nTrain, 0, nVal, outputSize);

    Permutation iterPerm(nTrain);
    iterPerm.setIdentity();
    double error = 0;
    index  patience = mInitialPatience;
    double prevValLoss = std::numeric_limits<double>::max();
    while (nIter-- > 0)
    {
      shuffle(iterPerm.indices().data(),
              iterPerm.indices().data() + iterPerm.indices().size(),
              mt19937{random_device{}()});
      ArrayXXd inPerm = iterPerm * trainInput.matrix();
      ArrayXXd outPerm = iterPerm * trainOutput.matrix();
      for (index batchStart = 0; batchStart < inPerm.rows();
           batchStart += batchSize)
      {
        index thisBatchSize = (batchStart + batchSize) <= nTrain
                                  ? batchSize
                                  : nTrain - batchStart;
        ArrayXXd batchIn =
            inPerm.block(batchStart, 0, thisBatchSize, inPerm.cols());
        ArrayXXd batchOut =
            outPerm.block(batchStart, 0, thisBatchSize, outPerm.cols());
        ArrayXXd batchPred = ArrayXXd::Zero(thisBatchSize, outputSize);
        model.forward(batchIn, batchPred);
        ArrayXXd diff = batchPred - batchOut;
        model.backward(diff);
        model.update(learningRate, momentum);
      }
      if (nVal > 0)
      {
        ArrayXXd valPred = ArrayXXd::Zero(nVal, outputSize);
        model.forward(valInput, valPred);
        double valLoss = model.loss(valPred, valOutput);
        if (valLoss < prevValLoss)
          patience = mInitialPatience;
        else
          patience--;
        if (patience <= 0) break;
        prevValLoss = valLoss;
      }
    }
    ArrayXXd finalPred = ArrayXXd::Zero(nExamples, outputSize);
    model.forward(input, finalPred);
    bool isNan = !((finalPred == finalPred)).all();
    if (isNan)
    {
      model.clear();
      return -1;
    }
    error = model.loss(finalPred, output);
    model.setTrained(true);
    return error;
  }

private:
  index mInitialPatience{10};
};
} // namespace algorithm
} // namespace fluid