#include <ctime>
#include <cmath>

#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>

#include <boost/unordered_map.hpp>
#include <boost/functional.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/algorithm/string/join.hpp>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "maybe_omp.h"
#include <tclap/CmdLine.h>

#include "model.h"
#include "propagator.h"
#include "param.h"
#include "neuralClasses.h"
#include "graphClasses.h"
#include "util.h"
#include "multinomial.h"
//#include "gradientCheck.h"

//#define EIGEN_DONT_PARALLELIZE

using namespace std;
using namespace TCLAP;
using namespace Eigen;
using namespace boost;
using namespace boost::random;

using namespace nplm;

typedef unordered_map<Matrix<int,Dynamic,1>, double> vector_map;

typedef long long int data_size_t; // training data can easily exceed 2G instances

int main(int argc, char** argv)
{
    param myParam;
    try {
      // program options //
      CmdLine cmd("Trains a two-layer neural probabilistic language model.", ' ' , "0.1");

      // The options are printed in reverse order

      ValueArg<string> unigram_probs_file("", "unigram_probs_file", "Unigram model (deprecated and ignored)." , false, "", "string", cmd);

      ValueArg<int> num_threads("", "num_threads", "Number of threads. Default: maximum.", false, 0, "int", cmd);

      ValueArg<double> final_momentum("", "final_momentum", "Final value of momentum. Default: 0.9.", false, 0.9, "double", cmd);
      ValueArg<double> initial_momentum("", "initial_momentum", "Initial value of momentum. Default: 0.9.", false, 0.9, "double", cmd);
      ValueArg<bool> use_momentum("", "use_momentum", "Use momentum (hidden layer weights only). 1 = yes, 0 = no. Default: 0.", false, 0, "bool", cmd);

      ValueArg<double> normalization_init("", "normalization_init", "Initial normalization parameter. Default: 0.", false, 0.0, "double", cmd);
      ValueArg<bool> normalization("", "normalization", "Learn individual normalization factors during training. 1 = yes, 0 = no. Default: 0.", false, 0, "bool", cmd);

      ValueArg<int> num_noise_samples("", "num_noise_samples", "Number of noise samples for noise-contrastive estimation. Default: 25.", false, 25, "int", cmd);

      ValueArg<double> L2_reg("", "L2_reg", "L2 regularization strength (hidden layer weights only). Default: 0.", false, 0.0, "double", cmd);

      ValueArg<double> learning_rate("", "learning_rate", "Learning rate for stochastic gradient ascent. Default: 0.01.", false, 0.01, "double", cmd);

      ValueArg<int> validation_minibatch_size("", "validation_minibatch_size", "Minibatch size for validation. Default: 64.", false, 64, "int", cmd);
      ValueArg<int> minibatch_size("", "minibatch_size", "Minibatch size (for training). Default: 64.", false, 64, "int", cmd);

      ValueArg<int> num_epochs("", "num_epochs", "Number of epochs. Default: 10.", false, 10, "int", cmd);

      ValueArg<double> init_range("", "init_range", "Maximum (of uniform) or standard deviation (of normal) for initialization. Default: 0.01", false, 0.01, "double", cmd);
      ValueArg<bool> init_normal("", "init_normal", "Initialize parameters from a normal distribution. 1 = normal, 0 = uniform. Default: 0.", false, 0, "bool", cmd);

      ValueArg<string> loss_function("", "loss_function", "Loss function (log, nce). Default: nce.", false, "nce", "string", cmd);
      ValueArg<string> activation_function("", "activation_function", "Activation function (identity, rectifier, tanh, hardtanh). Default: rectifier.", false, "rectifier", "string", cmd);
      ValueArg<int> num_hidden("", "num_hidden", "Number of hidden nodes. Default: 100.", false, 100, "int", cmd);

      ValueArg<bool> share_embeddings("", "share_embeddings", "Share input and output embeddings. 1 = yes, 0 = no. Default: 0.", false, 0, "bool", cmd);
      ValueArg<int> output_embedding_dimension("", "output_embedding_dimension", "Number of output embedding dimensions. Default: 50.", false, 50, "int", cmd);
      ValueArg<int> input_embedding_dimension("", "input_embedding_dimension", "Number of input embedding dimensions. Default: 50.", false, 50, "int", cmd);
      ValueArg<int> embedding_dimension("", "embedding_dimension", "Number of input and output embedding dimensions. Default: none.", false, -1, "int", cmd);

      ValueArg<int> vocab_size("", "vocab_size", "Vocabulary size. Default: auto.", false, 0, "int", cmd);
      ValueArg<int> input_vocab_size("", "input_vocab_size", "Vocabulary size. Default: auto.", false, 0, "int", cmd);
      ValueArg<int> output_vocab_size("", "output_vocab_size", "Vocabulary size. Default: auto.", false, 0, "int", cmd);
      ValueArg<int> ngram_size("", "ngram_size", "Size of n-grams. Default: auto.", false, 0, "int", cmd);

      ValueArg<string> model_prefix("", "model_prefix", "Prefix for output model files." , false, "", "string", cmd);
      ValueArg<string> words_file("", "words_file", "Vocabulary." , false, "", "string", cmd);
      ValueArg<string> input_words_file("", "input_words_file", "Vocabulary." , false, "", "string", cmd);
      ValueArg<string> output_words_file("", "output_words_file", "Vocabulary." , false, "", "string", cmd);
      ValueArg<string> validation_file("", "validation_file", "Validation data (one numberized example per line)." , false, "", "string", cmd);
      ValueArg<string> train_file("", "train_file", "Training data (one numberized example per line)." , true, "", "string", cmd);

      cmd.parse(argc, argv);

      // define program parameters //
      myParam.train_file = train_file.getValue();
      myParam.validation_file = validation_file.getValue();
      myParam.input_words_file = input_words_file.getValue();
      myParam.output_words_file = output_words_file.getValue();
      if (words_file.getValue() != "")
	  myParam.input_words_file = myParam.output_words_file = words_file.getValue();

      myParam.model_prefix = model_prefix.getValue();

      myParam.ngram_size = ngram_size.getValue();
      myParam.vocab_size = vocab_size.getValue();
      myParam.input_vocab_size = input_vocab_size.getValue();
      myParam.output_vocab_size = output_vocab_size.getValue();
      if (vocab_size.getValue() >= 0)
	  myParam.input_vocab_size = myParam.output_vocab_size = vocab_size.getValue();

      myParam.num_hidden = num_hidden.getValue();
      myParam.activation_function = activation_function.getValue();
      myParam.loss_function = loss_function.getValue();

      myParam.num_threads = num_threads.getValue();

      myParam.num_noise_samples = num_noise_samples.getValue();

      myParam.input_embedding_dimension = input_embedding_dimension.getValue();
      myParam.output_embedding_dimension = output_embedding_dimension.getValue();
      if (embedding_dimension.getValue() >= 0)
	      myParam.input_embedding_dimension = myParam.output_embedding_dimension = embedding_dimension.getValue();

      myParam.minibatch_size = minibatch_size.getValue();
      myParam.validation_minibatch_size = validation_minibatch_size.getValue();
      myParam.num_epochs= num_epochs.getValue();
      myParam.learning_rate = learning_rate.getValue();
      myParam.use_momentum = use_momentum.getValue();
      myParam.share_embeddings = share_embeddings.getValue();
      myParam.normalization = normalization.getValue();
      myParam.initial_momentum = initial_momentum.getValue();
      myParam.final_momentum = final_momentum.getValue();
      myParam.L2_reg = L2_reg.getValue();
      myParam.init_normal= init_normal.getValue();
      myParam.init_range = init_range.getValue();
      myParam.normalization_init = normalization_init.getValue();

      cerr << "Command line: " << endl;
      cerr << boost::algorithm::join(vector<string>(argv, argv+argc), " ") << endl;

      const string sep(" Value: ");
      cerr << train_file.getDescription() << sep << train_file.getValue() << endl;
      cerr << validation_file.getDescription() << sep << validation_file.getValue() << endl;
      cerr << input_words_file.getDescription() << sep << input_words_file.getValue() << endl;
      cerr << output_words_file.getDescription() << sep << output_words_file.getValue() << endl;
      cerr << model_prefix.getDescription() << sep << model_prefix.getValue() << endl;

      cerr << ngram_size.getDescription() << sep << ngram_size.getValue() << endl;
      cerr << input_vocab_size.getDescription() << sep << input_vocab_size.getValue() << endl;
      cerr << output_vocab_size.getDescription() << sep << output_vocab_size.getValue() << endl;

      if (embedding_dimension.getValue() >= 0)
      {
	  cerr << embedding_dimension.getDescription() << sep << embedding_dimension.getValue() << endl;
      }
      else
      {
	  cerr << input_embedding_dimension.getDescription() << sep << input_embedding_dimension.getValue() << endl;
	  cerr << output_embedding_dimension.getDescription() << sep << output_embedding_dimension.getValue() << endl;
      }
      cerr << share_embeddings.getDescription() << sep << share_embeddings.getValue() << endl;
      if (share_embeddings.getValue() && input_embedding_dimension.getValue() != output_embedding_dimension.getValue())
      {
	  cerr << "error: sharing input and output embeddings requires that input and output embeddings have same dimension" << endl;
	  exit(1);
      }

      cerr << num_hidden.getDescription() << sep << num_hidden.getValue() << endl;

      if (string_to_activation_function(activation_function.getValue()) == InvalidFunction)
      {
	 cerr << "error: invalid activation function: " << activation_function.getValue() << endl;
	  exit(1);
      }
      cerr << activation_function.getDescription() << sep << activation_function.getValue() << endl;

      if (string_to_loss_function(loss_function.getValue()) == InvalidLoss)
      {
	 cerr << "error: invalid loss function: " << loss_function.getValue() << endl;
	  exit(1);
      }
      cerr << loss_function.getDescription() << sep << loss_function.getValue() << endl;

      cerr << init_normal.getDescription() << sep << init_normal.getValue() << endl;
      cerr << init_range.getDescription() << sep << init_range.getValue() << endl;

      cerr << num_epochs.getDescription() << sep << num_epochs.getValue() << endl;
      cerr << minibatch_size.getDescription() << sep << minibatch_size.getValue() << endl;
      if (myParam.validation_file != "")
	  cerr << validation_minibatch_size.getDescription() << sep << validation_minibatch_size.getValue() << endl;
      cerr << learning_rate.getDescription() << sep << learning_rate.getValue() << endl;
      cerr << L2_reg.getDescription() << sep << L2_reg.getValue() << endl;

      cerr << num_noise_samples.getDescription() << sep << num_noise_samples.getValue() << endl;

      cerr << normalization.getDescription() << sep << normalization.getValue() << endl;
      if (myParam.normalization)
	  cerr << normalization_init.getDescription() << sep << normalization_init.getValue() << endl;

      cerr << use_momentum.getDescription() << sep << use_momentum.getValue() << endl;
      if (myParam.use_momentum)
      {
	  cerr << initial_momentum.getDescription() << sep << initial_momentum.getValue() << endl;
	  cerr << final_momentum.getDescription() << sep << final_momentum.getValue() << endl;
      }

      cerr << num_threads.getDescription() << sep << num_threads.getValue() << endl;

      if (unigram_probs_file.getValue() != "")
      {
	  cerr << "Note: --unigram_probs_file is deprecated and ignored." << endl;
      }
    }
    catch (TCLAP::ArgException &e)
    {
      cerr << "error: " << e.error() <<  " for arg " << e.argId() << endl;
      exit(1);
    }

    myParam.num_threads = setup_threads(myParam.num_threads);
    int save_threads;

    //unsigned seed = std::time(0);
    unsigned seed = 1234; //for testing only
    mt19937 rng(seed);

    /////////////////////////READING IN THE TRAINING AND VALIDATION DATA///////////////////
    /////////////////////////////////////////////////////////////////////////////////////

    // Read training data
    vector<int> training_data_flat;
    readDataFile(myParam.train_file, myParam.ngram_size, training_data_flat, myParam.minibatch_size);
    data_size_t training_data_size = training_data_flat.size() / myParam.ngram_size;
    cerr << "Number of training instances: "<< training_data_size << endl;

    Map< Matrix<int,Dynamic,Dynamic> > training_data(training_data_flat.data(), myParam.ngram_size, training_data_size);

    // If neither --input_vocab_size nor --input_words_file is given, set input_vocab_size to the maximum word index
    if (myParam.input_vocab_size == 0 and myParam.input_words_file == "")
    {
        myParam.input_vocab_size = training_data.topRows(myParam.ngram_size-1).maxCoeff()+1;
    }

    // If neither --output_vocab_size nor --output_words_file is given, set output_vocab_size to the maximum word index
    if (myParam.output_vocab_size == 0 and myParam.words_file == "")
    {
        myParam.output_vocab_size = training_data.row(myParam.ngram_size-1).maxCoeff()+1;
    }

    // Randomly shuffle training data to improve learning
    for (data_size_t i=training_data_size-1; i>0; i--)
    {
        data_size_t j = uniform_int_distribution<data_size_t>(0, i-1)(rng);
	training_data.col(i).swap(training_data.col(j));
    }

    // Read validation data
    vector<int> validation_data_flat;
    int validation_data_size = 0;

    if (myParam.validation_file != "")
    {
	readDataFile(myParam.validation_file, myParam.ngram_size, validation_data_flat);
	validation_data_size = validation_data_flat.size() / myParam.ngram_size;
	cerr << "Number of validation instances: " << validation_data_size << endl;
    }

    Map< Matrix<int,Dynamic,Dynamic> > validation_data(validation_data_flat.data(), myParam.ngram_size, validation_data_size);

    ///// Read in vocabulary file. We don't actually use it; it just gets reproduced in the output file

    vector<string> input_words;
    if (myParam.input_words_file != "")
    {
        readWordsFile(myParam.input_words_file, input_words);
	if (myParam.input_vocab_size == 0)
	    myParam.input_vocab_size = input_words.size();
    }

    vector<string> output_words;
    if (myParam.output_words_file != "")
    {
        readWordsFile(myParam.output_words_file, output_words);
	if (myParam.output_vocab_size == 0)
	    myParam.output_vocab_size = output_words.size();
    }

    ///// Construct unigram model and sampler that will be used for NCE

    vector<data_size_t> unigram_counts(myParam.output_vocab_size);
    for (data_size_t train_id=0; train_id < training_data_size; train_id++)
    {
        int output_word = training_data(myParam.ngram_size-1, train_id);
	unigram_counts[output_word] += 1;
    }
    multinomial<data_size_t> unigram (unigram_counts);

    ///// Create and initialize the neural network and associated propagators.

    model nn(myParam.ngram_size,
        myParam.input_vocab_size,
        myParam.output_vocab_size,
        myParam.input_embedding_dimension,
	      myParam.num_hidden,
        myParam.output_embedding_dimension,
        myParam.share_embeddings);

    nn.initialize(rng, myParam.init_normal, myParam.init_range, -log(myParam.output_vocab_size));
    nn.set_activation_function(string_to_activation_function(myParam.activation_function));
    loss_function_type loss_function = string_to_loss_function(myParam.loss_function);

    propagator prop(nn, myParam.minibatch_size);
    propagator prop_validation(nn, myParam.validation_minibatch_size);
    SoftmaxNCELoss<multinomial<data_size_t> > softmax_loss(unigram);
    // normalization parameters
    vector_map c_h, c_h_running_gradient;

    ///////////////////////TRAINING THE NEURAL NETWORK////////////////////////////////////
    /////////////////////////////////////////////////////////////////////////////////////

    data_size_t num_batches = (training_data_size-1)/myParam.minibatch_size + 1;
    cerr<<"Number of training minibatches: "<<num_batches<<endl;

    int num_validation_batches = 0;
    if (validation_data_size > 0)
    {
        num_validation_batches = (validation_data_size-1)/myParam.validation_minibatch_size+1;
	cerr<<"Number of validation minibatches: "<<num_validation_batches<<endl;
    }

    double current_momentum = myParam.initial_momentum;
    double momentum_delta = (myParam.final_momentum - myParam.initial_momentum)/(myParam.num_epochs-1);
    double current_learning_rate = myParam.learning_rate;
    double current_validation_ll = 0.0;

    int ngram_size = myParam.ngram_size;
    int input_vocab_size = myParam.input_vocab_size;
    int output_vocab_size = myParam.output_vocab_size;
    int minibatch_size = myParam.minibatch_size;
    int validation_minibatch_size = myParam.validation_minibatch_size;
    int num_noise_samples = myParam.num_noise_samples;

    if (myParam.normalization)
    {
	for (data_size_t i=0;i<training_data_size;i++)
	{
	    Matrix<int,Dynamic,1> context = training_data.block(0,i,ngram_size-1,1);
	    if (c_h.find(context) == c_h.end())
	    {
	        c_h[context] = -myParam.normalization_init;
	    }
	}
    }

    for (int epoch=0; epoch<myParam.num_epochs; epoch++)
    {
        cerr << "Epoch " << epoch+1 << endl;
        cerr << "Current learning rate: " << current_learning_rate << endl;

        if (myParam.use_momentum)
	    cerr << "Current momentum: " << current_momentum << endl;
	else
            current_momentum = -1;

	cerr << "Training minibatches: ";

	double log_likelihood = 0.0;

	int num_samples = 0;
	if (loss_function == LogLoss)
	    num_samples = output_vocab_size;
	else if (loss_function == NCELoss)
	    num_samples = 1+num_noise_samples;

	Matrix<double,Dynamic,Dynamic> minibatch_weights(num_samples, minibatch_size);
	Matrix<int,Dynamic,Dynamic> minibatch_samples(num_samples, minibatch_size);
	Matrix<double,Dynamic,Dynamic> scores(num_samples, minibatch_size);
	Matrix<double,Dynamic,Dynamic> probs(num_samples, minibatch_size);

        for(data_size_t batch=0;batch<num_batches;batch++)
        {
            if (batch > 0 && batch % 10000 == 0)
            {
	        cerr << batch <<"...";
            }

            data_size_t minibatch_start_index = minibatch_size * batch;
            int current_minibatch_size = min(static_cast<data_size_t>(minibatch_size), training_data_size - minibatch_start_index);
	    Matrix<int,Dynamic,Dynamic> minibatch = training_data.middleCols(minibatch_start_index, current_minibatch_size);

            double adjusted_learning_rate = current_learning_rate/current_minibatch_size;
            //cerr<<"Adjusted learning rate: "<<adjusted_learning_rate<<endl;

            /*
            if (batch == rand() % num_batches)
            {
                cerr<<"we are checking the gradient in batch "<<batch<<endl;
                /////////////////////////CHECKING GRADIENTS////////////////////////////////////////
                gradientChecking(myParam,minibatch_start_index,current_minibatch_size,word_nodes,context_nodes,hidden_layer_node,hidden_layer_to_output_node,
                              shuffled_training_data,c_h,unif_real_vector,eng_real_vector,unif_int_vector,eng_int_vector,unigram_probs_vector,
                              q_vector,J_vector,D_prime);
            }
            */

            ///// Forward propagation

            prop.fProp(minibatch.topRows(ngram_size-1));

	    if (loss_function == NCELoss)
	    {
	        ///// Noise-contrastive estimation

	        // Generate noise samples. Gather positive and negative samples into matrix.

	        start_timer(3);

		minibatch_samples.block(0, 0, 1, current_minibatch_size) = minibatch.bottomRows(1);

		for (int sample_id = 1; sample_id < num_noise_samples+1; sample_id++)
		    for (int train_id = 0; train_id < current_minibatch_size; train_id++)
		        minibatch_samples(sample_id, train_id) = unigram.sample(rng);

		stop_timer(3);

		// Final forward propagation step (sparse)
		start_timer(4);
		prop.output_layer_node.param->fProp(prop.second_hidden_activation_node.fProp_matrix,
						    minibatch_samples, scores);
		stop_timer(4);

		// Apply normalization parameters
		if (myParam.normalization)
		{
		    for (int train_id = 0;train_id < current_minibatch_size;train_id++)
		    {
			Matrix<int,Dynamic,1> context = minibatch.block(0, train_id, ngram_size-1, 1);
			scores.col(train_id).array() += c_h[context];
		    }
		}

		double minibatch_log_likelihood;
		start_timer(5);
		softmax_loss.fProp(scores.leftCols(current_minibatch_size),
				   minibatch_samples,
				   probs, minibatch_log_likelihood);
		stop_timer(5);
		log_likelihood += minibatch_log_likelihood;

		///// Backward propagation

		start_timer(6);
		softmax_loss.bProp(probs, minibatch_weights);
		stop_timer(6);

		// Update the normalization parameters

		if (myParam.normalization)
		{
		    for (int train_id = 0;train_id < current_minibatch_size;train_id++)
		    {
			Matrix<int,Dynamic,1> context = minibatch.block(0, train_id, ngram_size-1, 1);
			c_h[context] += adjusted_learning_rate * minibatch_weights.col(train_id).sum();
		    }
		}

		// Be careful of short minibatch
		prop.bProp(minibatch.topRows(ngram_size-1),
			   minibatch_samples.leftCols(current_minibatch_size),
			   minibatch_weights.leftCols(current_minibatch_size),
			   adjusted_learning_rate, current_momentum, myParam.L2_reg);
	    }
	    else if (loss_function == LogLoss)
	    {
	        ///// Standard log-likelihood
	        start_timer(4);
		prop.output_layer_node.param->fProp(prop.second_hidden_activation_node.fProp_matrix, scores);
		stop_timer(4);

		double minibatch_log_likelihood;
		start_timer(5);
		SoftmaxLogLoss().fProp(scores.leftCols(current_minibatch_size),
				       minibatch.row(ngram_size-1),
				       probs,
				       minibatch_log_likelihood);
		stop_timer(5);
		log_likelihood += minibatch_log_likelihood;

		///// Backward propagation

		start_timer(6);
		SoftmaxLogLoss().bProp(minibatch.row(ngram_size-1).leftCols(current_minibatch_size),
				       probs.leftCols(current_minibatch_size),
				       minibatch_weights);
		stop_timer(6);

		prop.bProp(minibatch.topRows(ngram_size-1).leftCols(current_minibatch_size),
			   minibatch_weights,
			   adjusted_learning_rate, current_momentum, myParam.L2_reg);
	    }
        }
	cerr << "done." << endl;

	if (loss_function == LogLoss)
	{
	    cerr << "Training log-likelihood: " << log_likelihood << endl;
            cerr << "         perplexity:     "<< exp(-log_likelihood/training_data_size) << endl;
	}
	else if (loss_function == NCELoss)
	    cerr << "Training NCE log-likelihood: " << log_likelihood << endl;

        current_momentum += momentum_delta;

	#ifdef USE_CHRONO
	cerr << "Propagation times:";
	for (int i=0; i<timer.size(); i++)
	  cerr << " " << timer.get(i);
	cerr << endl;
	#endif

	if (myParam.model_prefix != "")
	{
	    cerr << "Writing model" << endl;
	    if (myParam.input_words_file != "")
	        nn.write(myParam.model_prefix + "." + lexical_cast<string>(epoch+1), input_words, output_words);
	    else
	        nn.write(myParam.model_prefix + "." + lexical_cast<string>(epoch+1));
	}

        if (epoch % 1 == 0 && validation_data_size > 0)
        {
            //////COMPUTING VALIDATION SET PERPLEXITY///////////////////////
            ////////////////////////////////////////////////////////////////

            double log_likelihood = 0.0;

	    Matrix<double,Dynamic,Dynamic> scores(output_vocab_size, validation_minibatch_size);
	    Matrix<double,Dynamic,Dynamic> output_probs(output_vocab_size, validation_minibatch_size);
	    Matrix<int,Dynamic,Dynamic> minibatch(ngram_size, validation_minibatch_size);

            for (int validation_batch =0;validation_batch < num_validation_batches;validation_batch++)
            {
                int validation_minibatch_start_index = validation_minibatch_size * validation_batch;
		int current_minibatch_size = min(validation_minibatch_size,
						 validation_data_size - validation_minibatch_start_index);
		minibatch.leftCols(current_minibatch_size) = validation_data.middleCols(validation_minibatch_start_index,
											current_minibatch_size);
		prop_validation.fProp(minibatch.topRows(ngram_size-1));

		// Do full forward prop through output word embedding layer
		start_timer(4);
		prop_validation.output_layer_node.param->fProp(prop_validation.second_hidden_activation_node.fProp_matrix, scores);
		stop_timer(4);

		// And softmax and loss. Be careful of short minibatch
		double minibatch_log_likelihood;
		start_timer(5);
		SoftmaxLogLoss().fProp(scores.leftCols(current_minibatch_size),
				       minibatch.row(ngram_size-1),
				       output_probs,
				       minibatch_log_likelihood);
		stop_timer(5);
		log_likelihood += minibatch_log_likelihood;
	    }

            cerr << "Validation log-likelihood: "<< log_likelihood << endl;
            cerr << "           perplexity:     "<< exp(-log_likelihood/validation_data_size) << endl;

	    // If the validation perplexity decreases, halve the learning rate.
            if (epoch > 0 && log_likelihood < current_validation_ll)
            {
                current_learning_rate /= 2;
            }
            current_validation_ll = log_likelihood;
	}

    }
    return 0;
}


#include "medialib/synchronize.h"

#include "medialib/audioConversion.h"

#include <fftw3.h>
#include <unordered_map>
#include <cassert>

#include "cvlib/imageDLib.h"

#include "cvlib/slidingWindow.h"
#include "cvlib/match.h"
#include "cvlib/ppm.h"
#include "medialib/spectrogram.h"

#include "dlib/image_transforms/spatial_filtering_abstract.h"
#include <dlib/image_processing.h>

namespace medialib
{

std::vector<float> synchronizeAudio(AudioBuffers &audioBuffers, float searchRange)
{
//	AudioBuffers audioBuffers;

//	for(size_t i=0; i<buffers.size(); ++i)
//		audioBuffers.push_back(AudioBuffer(buffers[i]));

	std::vector<float> timeDiff;

	if(audioBuffers.size()<2)
		return timeDiff;

	int maxSamples=std::ceil((float)audioBuffers[0].getSampleRate()*searchRange);
	//   size_t maxChannels=audioBuffers[0].getChannels();

	//get maximum segment size and maxium channels of all the audio buffers
	for(size_t i=1; i<audioBuffers.size(); ++i)
	{
		const AudioBuffer &audioBuffer=audioBuffers[i];

		//       int localSegementSize=std::ceil((float)audioBuffer.getSampleRate()*searchRange);

		maxSamples=std::max(maxSamples, (int)std::ceil((float)audioBuffer.getSampleRate()*searchRange));
		//       if(localSegementSize!=maxSegmentSize)
		//       {
		//           if(localSegementSize>maxSamples)
		//               maxSamples=localSegementSize;
		//       }

		//        if(audioBuffer.getChannels() > maxChannels)
		//            maxChannels=audioBuffer.getChannels();
	}

	size_t windowSize=1024;
	SpectrogramGenerator<float> spectrogramGenerator(windowSize, WindowFunctionType::Hamming, 0, true);
	size_t outputSize=spectrogramGenerator.calculateBufferSize(maxSamples);
	std::vector<float> spectrogram(outputSize);
	std::vector<float> maximum(outputSize);
	std::vector<uint8_t> binaryImage(outputSize);
	size_t width, height;

	for(size_t i=0; i<audioBuffers.size(); ++i)
	{
		const AudioBuffer &audioBuffer=audioBuffers[i];

		if(spectrogramGenerator.process(audioBuffer, 0, spectrogram.data(), outputSize, width, height))
//		if(spectrogramGenerator.process(audioBuffer.getBuffer(), audioBuffer.getSamples(), audioBuffer.getFormat(), spectrogram.data(), outputSize, width, height))
		{
			cvlib::SimpleImage imageSpectrogram(cvlib::ImageFormat::GreyScale32f, width, height, (uint8_t *)spectrogram.data(), outputSize*sizeof(float));

			std::string fileName="spectrogram"+std::to_string(i)+".ppm";
			cvlib::savePpm(imageSpectrogram, fileName);

//			cvlib::maximumSlidingWindow(spectrogram.data(), maximum.data(), width, height, 20);
//
//			cvlib::ImageStruct imageMaximum(width, height, cvlib::ImageFormat::GreyScale32f, (uint8_t *)maximum.data(), outputSize*sizeof(float));
//			cvlib::ImageStruct imageBinary(width, height, cvlib::ImageFormat::Binary, binaryImage.data(), outputSize*sizeof(uint8_t));
//
//			cvlib::match(imageSpectrogram, imageMaximum, imageBinary);
//
//			std::vector<std::pair<double, dlib::point>> maxPoints;
//			dlib::find_points_above_thresh(maxPoints, imageBinary, 0, std::numeric_limits<unsigned long>::max());
		}
	}

	return timeDiff;
}

//struct SampleMagnitude
//{
//	float magnitude;
//	size_t sampleIndex;
//};
//
//typedef std::unordered_map<size_t, SampleMagnitude> FrequencyBinMap;
//
//std::vector<float> synchronizeAudio(AudioBuffers audioBuffers, float searchRange, float segmentSeconds)
//{
//    std::vector<float> timeDiff;
//
//    if(audioBuffers.size()<2)
//        return timeDiff;
//
//    std::vector<int> segmentSizes;
//    typedef std::unordered_map<int, fftwf_plan> PlanMap;
//    PlanMap segmentPlans;
//    int maxSegmentSize=std::ceil((float)audioBuffers[0].sampleRate*segmentSeconds);
//	size_t maxChannels=audioBuffers[0].channels;
//
//	typedef std::unordered_map<int, > PlanMap;
//
//    segmentSizes.push_back(maxSegmentSize);
//
//    //get maximum segment size and maxium channels of all the audio buffers
//    for(size_t i=1; i<audioBuffers.size(); ++i)
//    {
//        AudioBuffer &audioBuffer=audioBuffers[i];
//
//        int localSegementSize=std::ceil((float)audioBuffer.sampleRate*segmentSeconds);
//
//        if(localSegementSize!=maxSegmentSize)
//        {
//            if(localSegementSize>maxSegmentSize)
//                maxSegmentSize=localSegementSize;
//
//            if(std::find(segmentSizes.begin(), segmentSizes.end(), localSegementSize) == segmentSizes.end())
//                segmentSizes.push_back(localSegementSize);
//        }
//
//		if(audioBuffer.channels > maxChannels)
//			maxChannels=audioBuffer.channels;
//    }
//
//	if(maxChannels <= 0)
//		return timeDiff;
//
//    int outSamples=(maxSegmentSize/2)+1;
//    size_t inBufferSize=sizeof(float) * maxSegmentSize;
//	std::vector<float *> inBuffers(maxChannels);
//
//	for(size_t i=0; i<maxChannels; ++i)
//		inBuffers[i]=(float *)fftwf_malloc(inBufferSize);
//	fftwf_complex *outBuffer=(fftwf_complex *)fftwf_malloc(sizeof(fftwf_complex) * outSamples);
//
//    for(int &segmentSize:segmentSizes)
//    {
//        segmentPlans.insert(PlanMap::value_type(segmentSize, fftwf_plan_dft_r2c_1d(segmentSize, inBuffers[0], outBuffer, FFTW_ESTIMATE)));
//    }
//
//    for(size_t i=0; i<audioBuffers.size(); ++i)
//    {
//        AudioBuffer &audioBuffer=audioBuffers[i];
//
//        int localSegementSize=std::ceil((float)audioBuffer.sampleRate*segmentSeconds);
//        fftwf_plan &plan=segmentPlans[localSegementSize];
//        int segments=((float)audioBuffer.sampleRate*searchRange)/localSegementSize;
//
//		size_t offset=0;
//
//		for(size_t bufferIndex=0; bufferIndex<segments; ++bufferIndex)
//		{
//			if(audioBuffer.format == AudioFormat::Float) //in native float format no need to copy
//			{
//				assert(false); //need to implement this
//
////				fftwf_execute_dft_r2c(plan, in, out);
//			}
//			else
//			{
//				//move to in buffer for fft
//				convertToFloat(audioBuffer, offset, localSegementSize, inBuffers, inBufferSize);
//
//				for(size_t channel=0; channel<audioBuffer.channels; ++channel)
//				{
//					fftwf_execute_dft_r2c(plan, inBuffers[channel], outBuffer, localSegementSize);
//
//					pushFrequencies(frequencyMap, outBuffer);
//				}
//			}
//			offset+=localSegementSize;
//		}
//    }
//}
//
//void pushFrequencies(FrequencyBinMap &frequencyBinMap, fftwf_complex *buffer, size_t size)
//{
//	for(size_t i=0; i<size; ++i)
//	{
//		fftwf_complex &complex=buffer[i];
//		float sqrMagnitude=complex[0]*complex[0]+complex[1]*complex[1]; //using relative difference so sqrt not needed for actual magnitude
//
//
//		frequencyBinMap.insert(FrequencyBinMap)
//
//	}
//}

}//namespace medialib


/*
 *  gravity_compensation_node.cpp
 *
 *  Created on: Nov 12, 2013
 *  Authors:   Francisco Viña
 *            fevb <at> kth.se
 */

/* Copyright (c) 2013, Francisco Viña, CVAP, KTH
   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 KTH 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 KTH BE LIABLE FOR ANY
   DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
   (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
   ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>
#include <gravity_compensation/gravity_compensation.h>
#include <gravity_compensation/gravity_compensation_params.h>
#include <sensor_msgs/Imu.h>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>
#include <boost/thread.hpp>

class GravityCompensationNode
{
public:
	ros::NodeHandle n_;

	// subscribe to accelerometer (imu) readings
	ros::Subscriber topicSub_imu_;
	ros::Subscriber topicSub_ft_raw_;

	ros::Publisher topicPub_ft_zeroed_;
	ros::Publisher topicPub_ft_compensated_;

	tf::TransformBroadcaster tf_br_;

	GravityCompensationNode()
	{
		n_ = ros::NodeHandle("~");
		m_g_comp_params  = new GravityCompensationParams();
		m_g_comp = NULL;
		m_received_imu = false;

		// subscribe to accelerometer topic and raw F/T sensor topic
		topicSub_imu_ = n_.subscribe("imu", 1, &GravityCompensationNode::topicCallback_imu, this);
		topicSub_ft_raw_ = n_.subscribe("ft_raw", 1, &GravityCompensationNode::topicCallback_ft_raw, this);

		/// implementation of topics to publish
		std::string ns;
		if(n_.hasParam("ns"))
		{
			n_.getParam("ns", ns);
			topicPub_ft_zeroed_ = n_.advertise<geometry_msgs::WrenchStamped> (ns+std::string("/ft_zeroed"), 1);
			topicPub_ft_compensated_ = n_.advertise<geometry_msgs::WrenchStamped> (ns+std::string("/ft_compensated"), 1);
		}

		else
		{
			topicPub_ft_zeroed_ = n_.advertise<geometry_msgs::WrenchStamped> ("ft_zeroed", 1);
			topicPub_ft_compensated_ = n_.advertise<geometry_msgs::WrenchStamped> ("ft_compensated", 1);
		}
	}

	~GravityCompensationNode()
	{
		delete m_g_comp;
		delete m_g_comp_params;
	}

	bool getROSParameters()
	{
		/// Get F/T sensor bias
		XmlRpc::XmlRpcValue biasXmlRpc;
		Eigen::Matrix<double, 6, 1> bias;
		if (n_.hasParam("bias"))
		{
			n_.getParam("bias", biasXmlRpc);
		}

		else
		{
			ROS_ERROR("Parameter 'bias' not set, shutting down node...");
			n_.shutdown();
			return false;
		}

		if(biasXmlRpc.size()!=6)
		{
			ROS_ERROR("Invalid F/T bias parameter size (should be size 6), shutting down node");
			n_.shutdown();
			return false;
		}

		for (int i = 0; i < biasXmlRpc.size(); i++)
		{
			bias(i) = (double)biasXmlRpc[i];
		}

		// get the mass of the gripper
		double gripper_mass;
		if (n_.hasParam("gripper_mass"))
		{
			n_.getParam("gripper_mass", gripper_mass);
		}

		else
		{
			ROS_ERROR("Parameter 'gripper_mass' not available");
			n_.shutdown();
			return false;
		}

		if(gripper_mass<0.0)
		{
			ROS_ERROR("Parameter 'gripper_mass' < 0");
			n_.shutdown();
			return false;
		}

		// get the pose of the COM of the gripper
		// we assume that it is fixed with respect to FT sensor frame
		// first get the frame ID
		tf::StampedTransform gripper_com;
		std::string gripper_com_frame_id;
		if (n_.hasParam("gripper_com_frame_id"))
		{
			n_.getParam("gripper_com_frame_id", gripper_com_frame_id);
		}

		else
		{
			ROS_ERROR("Parameter 'gripper_com_frame_id' not available");
			n_.shutdown();
			return false;
		}

		gripper_com.frame_id_ = gripper_com_frame_id;

		// now get the CHILD frame ID
		std::string gripper_com_child_frame_id;
		if (n_.hasParam("gripper_com_child_frame_id"))
		{
			n_.getParam("gripper_com_child_frame_id", gripper_com_child_frame_id);
		}

		else
		{
			ROS_ERROR("Parameter 'gripper_com_child_frame_id' not available");
			n_.shutdown();
			return false;
		}

		gripper_com.child_frame_id_ = gripper_com_child_frame_id;

		// now get the actual gripper COM pose
		Eigen::Matrix<double, 6, 1> gripper_com_pose;
		XmlRpc::XmlRpcValue gripper_com_pose_XmlRpc;
		if (n_.hasParam("gripper_com_pose"))
		{
			n_.getParam("gripper_com_pose", gripper_com_pose_XmlRpc);
		}

		else
		{
			ROS_ERROR("Parameter 'gripper_com_pose' not set, shutting down node...");
			n_.shutdown();
			return false;
		}

		if(gripper_com_pose_XmlRpc.size()!=6)
		{
			ROS_ERROR("Invalid 'gripper_com_pose' parameter size (should be size 6), shutting down node");
			n_.shutdown();
			return false;
		}

		for(unsigned int i=0; i<gripper_com_pose_XmlRpc.size(); i++)
		{
			gripper_com_pose(i) = gripper_com_pose_XmlRpc[i];
		}

		tf::Vector3 p;
		tf::vectorEigenToTF(gripper_com_pose.topRows(3), p);
		tf::Quaternion q;
		q.setRPY((double)gripper_com_pose(3),
				(double)gripper_com_pose(4),
				(double)gripper_com_pose(5));

		gripper_com = tf::StampedTransform(tf::Transform(q, p),
				ros::Time::now(),
				gripper_com_frame_id,
				gripper_com_child_frame_id);

		// get the publish frequency for the gripper
		// center of mass tf
		n_.param("gripper_com_broadcast_frequency",
				m_gripper_com_broadcast_frequency, 100.0);

		m_g_comp_params->setBias(bias);
		m_g_comp_params->setGripperMass(gripper_mass);
		m_g_comp_params->setGripperCOM(gripper_com);

		m_g_comp = new GravityCompensation(m_g_comp_params);
		return true;
	}

	void topicCallback_imu(const sensor_msgs::Imu::ConstPtr &msg)
	{
		m_imu = *msg;
		m_received_imu = true;
	}

	void topicCallback_ft_raw(const geometry_msgs::WrenchStamped::ConstPtr &msg)
	{
		static int error_msg_count=0;

		if(!m_received_imu)
		{
			return;
		}

		if((ros::Time::now()-m_imu.header.stamp).toSec() > 0.1)
		{
			error_msg_count++;
			if(error_msg_count % 10==0)
				ROS_ERROR("Imu reading too old, not able to g-compensate ft measurement");
			return;
		}

		geometry_msgs::WrenchStamped ft_zeroed;
		m_g_comp->Zero(*msg, ft_zeroed);
		topicPub_ft_zeroed_.publish(ft_zeroed);

		geometry_msgs::WrenchStamped ft_compensated;
		m_g_comp->Compensate(ft_zeroed, m_imu, ft_compensated);
		topicPub_ft_compensated_.publish(ft_compensated);
	}

	// thread function for publishing the gripper center of mass transform
	void publish_gripper_com_tf()
	{
		static ros::Rate gripper_com_broadcast_rate(m_gripper_com_broadcast_frequency);
		try
		{
			while(ros::ok())
			{
				tf::StampedTransform gripper_com = m_g_comp_params->getGripperCOM();
				gripper_com.stamp_ = ros::Time::now();
				tf_br_.sendTransform(gripper_com);

				gripper_com_broadcast_rate.sleep();
			}
		}

		catch(boost::thread_interrupted&)
		{
			return;
		}
	}

private:

	GravityCompensationParams *m_g_comp_params;
	GravityCompensation *m_g_comp;
	sensor_msgs::Imu m_imu;
	bool m_received_imu;
	double m_gripper_com_broadcast_frequency;

};

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

	ros::init(argc, argv, "gravity_compensation");
	GravityCompensationNode g_comp_node;

	if(!g_comp_node.getROSParameters())
	{
		ROS_ERROR("Error getting ROS parameters");
		return 0;
	}

	// loop frequency
	double loop_rate_;
	g_comp_node.n_.param("loop_rate", loop_rate_, 1000.0);
	ros::Rate loop_rate(loop_rate_);

	// add a thread for publishing the gripper COM transform frame
	boost::thread t_tf(boost::bind(&GravityCompensationNode::publish_gripper_com_tf, &g_comp_node));

//	ros::AsyncSpinner s(2);
//	s.start();

	while(ros::ok())
	{
		ros::spinOnce();
		loop_rate.sleep();
	}

	t_tf.join();

	return 0;
}


/*
 * setConfig.cpp
 *
 *  Created on: 09-09-2011
 *      Author: konradb3
 */

#include <iostream>

#include <boost/lexical_cast.hpp>

#include <lms1xx.h>

void print_usage() {

	std::cout << " Usage : " << std::endl;
	std::cout << " set_config ip resolution rate " << std::endl;
	std::cout << " Exqample : " << std::endl;
	std::cout << " set_config 192.168.1.2 0.25 50 " << std::endl;
}

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

	LMS1xx laser;
	scanCfg sCfg;

	if(argc < 4) {
		print_usage();
		return 0;
	}

	laser.connect(argv[1]);

	if(!laser.isConnected()) {
		std::cout << "Unable to connect to device at address : " << argv[1] << std::endl;
		return 0;
	}

	sCfg.angleResolution = (int)(boost::lexical_cast<double>(std::string(argv[2])) * 10000);
	sCfg.scaningFrequency = boost::lexical_cast<int>(std::string(argv[3])) * 100;

	laser.login();
	laser.setScanCfg(sCfg);
	laser.saveConfig();

	sCfg = laser.getScanCfg();

	std::cout << "Configuration set to : " << std::endl;
	std::cout << "resolution : " << (double)sCfg.angleResolution/10000.0 << std::endl;
	std::cout << "frequency : " << (double)sCfg.scaningFrequency/100.0 << std::endl;

	laser.disconnect();

	return 0;
}


#include <boost/geometry/algorithms/remove_spikes.hpp>