/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Rice University
 *  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 Rice University 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 OWNER 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.
 *********************************************************************/

/* Author: Ryan Luna */

#include <iostream>
#include "PlanarManipulator.h"
#include <boost/math/constants/constants.hpp>
#include <stdexcept>

#define PI boost::math::constants::pi<double>()
#define TWOPI boost::math::constants::two_pi<double>()

PlanarManipulator::PlanarManipulator(unsigned int numLinks, double linkLength, const std::pair<double, double> &origin)
  : numLinks_(numLinks), linkLengths_(numLinks_, linkLength)
{
    baseFrame_ = Eigen::Affine2d::Identity();
    baseFrame_.translation()[0] = origin.first;
    baseFrame_.translation()[1] = origin.second;

    hasBounds_.assign(numLinks_, false);
    lowerBounds_.assign(numLinks_, std::numeric_limits<double>::min());
    upperBounds_.assign(numLinks_, std::numeric_limits<double>::max());
}

PlanarManipulator::PlanarManipulator(unsigned int numLinks, const std::vector<double> &linkLengths,
                                     const std::pair<double, double> &origin)
  : numLinks_(numLinks), linkLengths_(linkLengths)
{
    if (linkLengths_.size() != numLinks)
    {
        std::cerr << "Length of linkLengths (" << linkLengths.size() << ") is not equal to the number of links ("
                  << numLinks << ")" << std::endl;
        throw;
    }

    baseFrame_ = Eigen::Affine2d::Identity();
    baseFrame_.translation()[0] = origin.first;
    baseFrame_.translation()[1] = origin.second;

    hasBounds_.assign(numLinks_, false);
    lowerBounds_.assign(numLinks_, std::numeric_limits<double>::min());
    upperBounds_.assign(numLinks_, std::numeric_limits<double>::max());
}

PlanarManipulator::~PlanarManipulator()
{
}

// Return the number of links
unsigned int PlanarManipulator::getNumLinks() const
{
    return numLinks_;
}

const Eigen::Affine2d &PlanarManipulator::getBaseFrame() const
{
    return baseFrame_;
}

void PlanarManipulator::setBaseFrame(const Eigen::Affine2d &frame)
{
    baseFrame_ = frame;
}

// Return the length of each link
const std::vector<double> &PlanarManipulator::getLinkLengths() const
{
    return linkLengths_;
}

// Set the bounds for link # link
void PlanarManipulator::setBounds(unsigned int link, double low, double high)
{
    assert(link < hasBounds_.size());

    hasBounds_[link] = true;
    lowerBounds_[link] = low;
    upperBounds_[link] = high;
}

// Set the bounds for ALL links
void PlanarManipulator::setBounds(const std::vector<double> &low, const std::vector<double> &high)
{
    assert(low.size() == high.size());
    assert(low.size() == numLinks_);

    hasBounds_.assign(numLinks_, true);
    lowerBounds_ = low;
    upperBounds_ = high;
}

bool PlanarManipulator::hasBounds(unsigned int link) const
{
    return hasBounds_[link];
}

void PlanarManipulator::getBounds(unsigned int link, double &low, double &high) const
{
    low = lowerBounds_[link];
    high = upperBounds_[link];
}
const std::vector<double> &PlanarManipulator::lowerBounds() const
{
    return lowerBounds_;
}

const std::vector<double> &PlanarManipulator::upperBounds() const
{
    return upperBounds_;
}

// Forward kinematics for the given joint configuration.  The frames for links
// 1 through the end-effector are returned.
void PlanarManipulator::FK(const std::vector<double> &joints, std::vector<Eigen::Affine2d> &frames) const
{
    FK(&joints[0], frames);
}

void PlanarManipulator::FK(const Eigen::VectorXd &joints, std::vector<Eigen::Affine2d> &frames) const
{
    FK(&joints(0), frames);
}

void PlanarManipulator::FK(const double *joints, std::vector<Eigen::Affine2d> &frames) const
{
    frames.clear();
    Eigen::Affine2d frame(baseFrame_);

    for (unsigned int i = 0; i < numLinks_; ++i)
    {
        // Rotate, then translate.  Just like the old gypsy woman said.
        Eigen::Affine2d offset(Eigen::Rotation2Dd(joints[i]) * Eigen::Translation2d(linkLengths_[i], 0));
        frame = frame * offset;
        frames.push_back(frame);
    }
}

void PlanarManipulator::FK(const std::vector<double> &joints, Eigen::Affine2d &eeFrame) const
{
    FK(&joints[0], eeFrame);
}

void PlanarManipulator::FK(const Eigen::VectorXd &joints, Eigen::Affine2d &eeFrame) const
{
    FK(&joints(0), eeFrame);
}

void PlanarManipulator::FK(const double *joints, Eigen::Affine2d &eeFrame) const
{
    eeFrame = baseFrame_;

    for (unsigned int i = 0; i < numLinks_; ++i)
    {
        // Rotate, then translate.  Just like the old gypsy woman said.
        Eigen::Affine2d offset(Eigen::Rotation2Dd(joints[i]) * Eigen::Translation2d(linkLengths_[i], 0));
        eeFrame = eeFrame * offset;
    }
}

// Inverse kinematics for the given end effector frame.  Only one solution is returned.
// Returns false if no solution exists to the given pose.
bool PlanarManipulator::IK(std::vector<double> &solution, const Eigen::Affine2d &eeFrame) const
{
    std::vector<double> seed(numLinks_, M_PI / 2.0);
    return IK(solution, seed, eeFrame);
}

bool PlanarManipulator::IK(std::vector<double> &solution, const std::vector<double> &seed,
                           const Eigen::Affine2d &desiredFrame) const
{
    // desired frame is clearly impossible to achieve
    if (infeasible(desiredFrame))
        return false;

    // This is the orientation for the end effector
    double angle = acos(desiredFrame.rotation()(0, 0));
    // Due to numerical instability, sometimes acos will return nan if
    // the value is slightly larger than one.  Check for this and try the
    // asin of the next value
    if (angle != angle)  // a nan is never equal to itself
        angle = asin(desiredFrame.rotation()(0, 1));

    // If still nan, return false
    if (angle != angle)
        return false;

    // Get the current pose
    Eigen::Affine2d frame;
    FK(seed, frame);
    Eigen::VectorXd current;
    frameToPose(frame, current);

    // Get the desired pose
    Eigen::VectorXd desired;
    frameToPose(desiredFrame, desired);

    // Compute the error
    Eigen::VectorXd e(desired - current);

    Eigen::VectorXd joints(seed.size());
    for (size_t i = 0; i < seed.size(); ++i)
        joints(i) = seed[i];

    double alpha = 0.1;  // step size

    unsigned int iter = 1;
    double eps = 1e-6;

    Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(3, numLinks_);
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(3, numLinks_, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::MatrixXd D = Eigen::MatrixXd::Zero(numLinks_, 3);

    // Iterate the jacobian
    while (e.norm() > eps)
    {
        // Get jacobian for this joint configuration
        Jacobian(joints, jac);

        // Compute inverse of the jacobian
        // Really unstable
        // Eigen::MatrixXd jac_inv = ((jac.transpose() * jac).inverse()) * jac.transpose();

        // Moore-Penrose Pseudoinverse
        svd.compute(jac);
        const Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType &d = svd.singularValues();

        // "Invert" the singular value matrix
        for (int i = 0; i < d.size(); ++i)
            if (d(i) > eps)
                D(i, i) = 1.0 / d(i);
            else
                D(i, i) = 0.0;

        // Inverse is V*D^-1*U^t
        Eigen::MatrixXd jac_inv = svd.matrixV() * D * svd.matrixU().transpose();

        // Get the joint difference
        Eigen::VectorXd delta_theta = jac_inv * e;

        // Check for failure
        if (delta_theta(0) != delta_theta(0))  // nan
        {
            // std::cout << jac_inv.matrix() << std::endl;
            return false;
        }

        // Increment the current joints by a (small) multiple of the difference
        joints = joints + (alpha * delta_theta);

        // Figure out the current EE pose and update e.
        FK(joints, frame);
        frameToPose(frame, current);
        // double n = e.norm();
        e = desired - current;
        iter++;

        if (iter > 5000)
            return false;  // diverge
    }

    // Store the solution.  Make sure angles are in range [-pi, pi]
    solution.resize(numLinks_);
    for (size_t i = 0; i < solution.size(); ++i)
    {
        double angle = joints(i);
        while (angle > M_PI)
            angle -= 2.0 * M_PI;
        while (angle < -M_PI)
            angle += 2.0 * M_PI;
        solution[i] = angle;
    }

    return true;
}

bool PlanarManipulator::FABRIK(std::vector<double> &solution, const Eigen::Affine2d &eeFrame, double xyTol,
                               double thetaTol) const
{
    std::vector<double> seed(numLinks_, M_PI / 2.0);
    return FABRIK(solution, seed, eeFrame, xyTol, thetaTol);
}

bool PlanarManipulator::FABRIK(std::vector<double> &solution, const std::vector<double> &seed,
                               const Eigen::Affine2d &desiredFrame, double xyTol, double thetaTol) const
{
    unsigned int numLinks = getNumLinks();  // have to use this local variable.  Compiler error (bug?) in
                                            // Eigen::Translation2d when I use numLinks_

    if (seed.size() != numLinks)
    {
        std::cerr << "Seed has length " << seed.size() << " but there are " << numLinks << " links" << std::endl;
        return false;
    }

    // desired frame is clearly impossible to achieve
    if (infeasible(desiredFrame))
        return false;

    // copy the seed into the solution
    solution.assign(seed.begin(), seed.end());

    // Compute the location of the origin of the last link with the correct rotation
    // This location will not move
    Eigen::Vector2d loc(-linkLengths_.back(), 0);
    loc = desiredFrame * loc;

    // The desired orientation of the end effector
    double v = desiredFrame.matrix()(0, 0);
    // Go Go Gadget Numerical Stabilizer!
    if (v < -1.0)
        v = -1.0;
    if (v > 1.0)
        v = 1.0;
    double eeTheta = acos(v);
    if (desiredFrame.matrix()(1, 0) < 0)  // sin is negative, so the angle is negative
        eeTheta = -eeTheta;

    // The location of each joint, starting with the origin and ending with the end effector
    // For an n-joint chain, jointPositions with have n+1 entries.
    std::vector<Eigen::Vector2d> jointPositions;
    jointPositions.push_back(baseFrame_.translation());  // root position

    // Compute the initial locations of each joint
    Eigen::Affine2d frame(baseFrame_);
    for (unsigned int i = 0; i < seed.size(); ++i)
    {
        Eigen::Affine2d offset(Eigen::Rotation2Dd(seed[i]) * Eigen::Translation2d(linkLengths_[i], 0));
        frame = frame * offset;
        jointPositions.push_back(frame.translation());
    }

    // Store the errors here
    double xyError = std::numeric_limits<double>::max();
    double thetaError = std::numeric_limits<double>::max();

    int numIterations = 0;
    int maxIter = 250;  // after this many iterations, declare failure.  This is probably WAY too many - tends to
                        // converge very fast, after 1-3 iterations
    while ((xyError > xyTol || thetaError > thetaTol) && numIterations++ < maxIter)
    {
        xyError = thetaError = 0.0;

        // Placing last link where it must go
        jointPositions[numLinks] = desiredFrame.translation();  // goal position, nailed it.
        jointPositions[numLinks - 1] = loc;                     // goal orientation, nailed it.

        // A reverse coordinate frame, working backward along the chain.  Used to resolve joint limits in backward phase
        // VERY IMPORTANT to translate first, then rotate.  This NOT like the old gypsy woman said.  I'm getting my
        // money back.
        Eigen::Affine2d backwardFrame(Eigen::Translation2d(jointPositions[numLinks - 1]) *
                                      Eigen::Rotation2Dd(PI + eeTheta));  // end of chain, looking down the chain

        // Move backward from the end effector toward the base
        for (int i = jointPositions.size() - 3; i >= 0;
             --i)  // -1 is end effector position.  Skip -2 to preserve the end effector orientation.
        {
            // draw a line between position i+1 and i, and place pt i at the kinematically feasible place along the line
            double t = linkLengths_[i] / (jointPositions[i + 1] - jointPositions[i]).norm();
            jointPositions[i] = (1 - t) * jointPositions[i + 1] + t * jointPositions[i];

            // Ensure the angle for joint i is within bounds.  If not, move the joint position
            Eigen::Vector2d vec = backwardFrame.inverse() * jointPositions[i];
            double angle = -atan2(vec(1), vec(0));  // The angle computed is the negative of the real angle (because we
                                                    // worked backward).

            // If not in bounds, the angle will be one of the joint limits
            if (hasBounds_[i] && (angle < lowerBounds_[i] || angle > upperBounds_[i]))
            {
                double dlow = fabs(angle - lowerBounds_[i]);
                double dhigh = fabs(angle - upperBounds_[i]);
                angle = (dlow < dhigh ? lowerBounds_[i] : upperBounds_[i]);
            }

            Eigen::Affine2d offset(Eigen::Rotation2Dd(-angle) * Eigen::Translation2d(linkLengths_[i], 0));
            backwardFrame = backwardFrame * offset;
            jointPositions[i] = backwardFrame.translation();
        }

        jointPositions[0] = baseFrame_.translation();  // move base back to where it is supposed to be
        Eigen::Affine2d forwardFrame(baseFrame_);

        // Move forward toward the end effector
        for (size_t i = 0; i < jointPositions.size() - 1;
             ++i)  // This pass moves everything except end effector.  Orientation at the end may be violated, but we
                   // check for this
        {
            // draw a line between position i+1 and i and place i+1 at the kinematically feasible place along the line
            double t = linkLengths_[i] / (jointPositions[i + 1] - jointPositions[i]).norm();
            jointPositions[i + 1] = (1 - t) * jointPositions[i] + t * jointPositions[i + 1];

            // Figure out the joint angle required for this
            Eigen::Vector2d vec = forwardFrame.inverse() * jointPositions[i + 1];
            double angle = atan2(vec(1), vec(0));

            // If not in bounds, the angle will be one of the joint limits
            if (hasBounds_[i] && (angle < lowerBounds_[i] || angle > upperBounds_[i]))
            {
                double dlow = fabs(angle - lowerBounds_[i]);
                double dhigh = fabs(angle - upperBounds_[i]);
                angle = (dlow < dhigh ? lowerBounds_[i] : upperBounds_[i]);
            }
            solution[i] = angle;

            // Update frame
            Eigen::Affine2d offset(Eigen::Rotation2Dd(angle) * Eigen::Translation2d(linkLengths_[i], 0));
            forwardFrame = forwardFrame * offset;
            jointPositions[i + 1] = forwardFrame.translation();  // joint location is the translation of the frame.  Do
                                                                 // this here in case joint limits changed the angle
        }

        // Compute translation and orientation error
        xyError = (jointPositions.back() - desiredFrame.translation()).norm();

        v = forwardFrame.matrix()(0, 0);
        // Go Go Gadget Numerical Stabilizer!
        if (v > 1.0)
            v = 1.0;
        if (v < -1.0)
            v = -1.0;

        // The real angle
        double thetaActual = acos(v);
        if (forwardFrame.matrix()(1, 0) < 0)  // sin is negative, so the angle is negative
            thetaActual = -thetaActual;
        thetaError = fabs(eeTheta - thetaActual);
    }

    // Winning.  Extract joint angles.  Oh wait.  Already done BOOM.
    if (xyError < xyTol && thetaError < thetaTol)
        return true;

    return false;
}

void PlanarManipulator::Jacobian(const double *joints, Eigen::MatrixXd &jac) const
{
    if (jac.rows() != 3 || jac.cols() != numLinks_)
        jac = Eigen::MatrixXd::Zero(3, numLinks_);

    std::vector<double> sins(numLinks_);
    std::vector<double> coss(numLinks_);
    double theta = 0.0;
    for (size_t i = 0; i < numLinks_; ++i)
    {
        theta += joints[i];
        sins[i] = sin(theta);
        coss[i] = cos(theta);
    }

    for (size_t i = 0; i < numLinks_; ++i)
    {
        double entry1 = 0.0;
        double entry2 = 0.0;
        for (size_t j = numLinks_; j > i; --j)
        {
            entry1 += -(linkLengths_[j - 1] * sins[j - 1]);
            entry2 += linkLengths_[j - 1] * coss[j - 1];
        }
        jac(0, i) = entry1;
        jac(1, i) = entry2;
        jac(2, i) = 1.0;
    }
}

// Return the Jacobian for the manipulator at the given joint state.
void PlanarManipulator::Jacobian(const std::vector<double> &joints, Eigen::MatrixXd &jac) const
{
    Jacobian(&joints[0], jac);
}

void PlanarManipulator::Jacobian(const Eigen::VectorXd &joints, Eigen::MatrixXd &jac) const
{
    Jacobian(&joints(0), jac);  // TODO: This is untested.  I dunno if an eigen::vector is contiguous.  It probably is.
}

void PlanarManipulator::frameToPose(const Eigen::Affine2d &frame, Eigen::VectorXd &pose)
{
    pose = Eigen::VectorXd(3);
    pose(0) = frame.translation()(0);
    pose(1) = frame.translation()(1);
    pose(2) = acos(frame.matrix()(0, 0));
}

bool PlanarManipulator::infeasible(const Eigen::Affine2d &frame) const
{
    // Check for impossible query
    // Compute the location of the origin of the last link with the correct rotation
    Eigen::Vector2d loc(-linkLengths_[numLinks_ - 1], 0);
    loc = frame * loc;

    // Length of the chain, except for the last link
    double len = 0.0;
    for (size_t i = 0; i < numLinks_ - 1; ++i)
        len += linkLengths_[i];

    // Make sure the chain (without the last link) is long enough to reach the
    // origin of the last link
    Eigen::Vector2d org(baseFrame_.translation());
    if ((org - loc).norm() > len)  // infeasible IK request
        return true;

    return false;
}


//transforms_ex.cpp
/*
* the transforms from object to shadow and vice versa
* .implementation.
* (C) 2005 olegabr. All rights reserved.
*/
#include "transforms_ex.h"
#include "object3d.h"
#include "shadow2d.h"
#include <vector>
#include <algorithm>
#include <numeric>
#include <set>
#include <list>
#include <cmath>

#include <boost/lambda/lambda.hpp>

#include <iostream>
using std::cout; using std::endl;

shadow2d& create_mask(const shadow2d& sh, shadow2d& mask)
{
	shadow2d::const_iterator s = sh.begin(), send = sh.end();
	shadow2d::iterator m = mask.begin();
	for (; s != send; ++s, ++m)
	{
		size2d_t sz = m->get_size();
		projection2d::const_iterator ps = s->begin(), psend = s->end();
		int i1(0), i2(0), j1(0), j2(0);
		{
			projection1d::const_iterator pps = ps->begin(), ppsend = ps->end();
			for (; pps != ppsend; ++pps)
				if (*pps > 0.) break; else ++i1;
		}

		for (; ps != psend; ++ps)
			if (*(ps->end()-1) > 0.) break; else ++i2;

		for (ps = s->begin(); ps != psend; ++ps, ++j1)
			if (*(ps->begin()) > 0.) break;
		for (; ps != psend; ++ps, ++j1)
			if (*(ps->begin()) == 0.) break;

		{
			ps = psend-1;
			projection1d::const_iterator pps = ps->begin(), ppsend = ps->end();
			for (; pps != ppsend; ++pps, ++j2)
				if (*pps > 0.) break;
			for (; pps != ppsend; ++pps, ++j2)
				if (*pps == 0.) break;
		}

		{
			int offset = 2;
			projection2d::iterator pm = m->begin(), pmend = m->end();
			pixel2d px(0,0); //i,j
			for (; pm != pmend; ++pm, ++px.j)
			{
				projection1d::iterator ppm = pm->begin(), ppmend = pm->end();
				for (px.i = 0; ppm != ppmend; ++ppm, ++px.i)
				{
					if
					(
						(px.i == 0)||
						(px.i == (sz.height - 1))||
						((px.i+px.j) <= i1+offset)||
						((int(sz.height)+px.j-px.i-1) <= i2+offset)||
						((sz.width - 1 + px.i-px.j) <= sz.width - 1 - (j1-offset))||
						((sz.width - 1 + sz.height - 1 - px.i-px.j) <= sz.height - 1 - (j2-offset))
					) *ppm = 0.;
					else *ppm = 1.;
				}
			}
		}
	}

	return mask;
}

shadow2d& apply_mask(const shadow2d& mask, shadow2d& sh)
{
	//shadow2d::iterator s = sh.begin(), send = sh.end();
	//shadow2d::const_iterator m = mask.begin();
	//for (; s != send; ++s, ++m)
	//{
	//	projection2d::iterator ps = s->begin(), psend = s->end();
	//	projection2d::const_iterator pm = m->begin();
	//	for (; ps != psend; ++ps, ++pm)
	//	{
	//		projection1d::iterator pps = ps->begin(), ppsend = ps->end();
	//		projection1d::const_iterator ppm = pm->begin();
	//		for (; pps != ppsend; ++pps, ++ppm)
	//			//if (*ppm < 0.5) *pps = 0.;
	//			*pps *= *ppm;
	//	}
	//}
	return sh;
}

namespace {
	object3d::value_type calc_mean(const object3d& o, const pixel3d& px)
	{
		double k(1.1);
		object3d::value_type value(0);
		for (pixel3d d(-1,-1,-1); d.i <= 1; ++d.i)
			for (d.j = -1; d.j <= 1; ++d.j)
				for (d.k = -1; d.k <= 1; ++d.k)
					value += o(px+d);

		return k*( value )/27.;
	}

	bool is_hole(const object3d& o, const pixel3d& px)
	{
		const object3d::value_type z = 0.001;
		int n(0);
		for (pixel3d d(-1,-1,-1); d.i <= 1; ++d.i)
			for (d.j = -1; d.j <= 1; ++d.j)
				for (d.k = -1; d.k <= 1; ++d.k)
					n += ((o(px+d) > z) ? 1 : 0);
		return
		(
			o(px) < z && /* == 0.*/
			n >= (26 - 23)
		);
	}

	struct filled_area
	{
		typedef std::size_t hash_t;
		typedef std::set<hash_t> data_type;

		filled_area(const size3d_t& sz) : size_(sz)
		{
			//use size_.depth to store the multiplication for optimization purpose
			size_.depth = size_.width*size_.height;
		}
		filled_area& add_pixel(const pixel3d& px)
		{
			data_.insert(pixel2hash_(px));
			return *this;
		}
		filled_area& add_area(filled_area& ar)
		{
			assert(ar.size_ == size_);
			data_.insert(ar.data_.begin(), ar.data_.end());
			ar.data_.clear();
			return *this;
		}
		bool is_adjacent(const pixel3d& px)
		{
			for (pixel3d d(-1,-1,-1); d.i <= 1; ++d.i)
				for (d.j = -1; d.j <= 1; ++d.j)
					for (d.k = -1; d.k <= 1; ++d.k)
					{
						if (d == pixel3d()) continue;
						if ( find_pixel_(px+d) ) return true;
					}
			return false;
		}
		std::size_t size() const { return data_.size(); }
		void clear(object3d& o, object3d::value_type replace_val)
		{
			for (data_type::iterator i(data_.begin()); i != data_.end(); ++i)
				o(hash2pixel_(*i)) = replace_val;
			//data_.clear();
		}
	private :
		hash_t pixel2hash_(const pixel3d& px)
		{
			return (px.k*size_.height + px.i)*size_.width + px.j;//ind;
		}
		pixel3d hash2pixel_(hash_t ind)
		{
			pixel3d px;
			px.k = ind/size_.depth;
			px.i = (ind - px.k*size_.depth)/size_.width;
			px.j = ind - px.k*size_.depth - px.i*size_.width;
			return px;
		}
		bool find_pixel_(const pixel3d& px)
		{
			return data_.find(pixel2hash_(px)) != data_.end();
		}
		data_type data_;
		size3d_t size_;
	};

	bool operator < (const filled_area& lh, const filled_area& rh)
	{
		return lh.size() < rh.size();
	}

	void remove_small_areas(object3d& o, object3d::value_type search_val, object3d::value_type replace_val)
	{
		typedef std::list<filled_area> areas_type;
		typedef std::vector<areas_type::iterator> areas_ptrs_type;
		areas_type areas;
		size3d_t sz(o.get_size());
		long count(0), COUNT(norm(sz));
		for (pixel3d p(0, 0, 0); p.i < sz.height; ++p.i)
		{
			for (p.j = 0; p.j < sz.width; ++p.j)
			{
				for (p.k = 0; p.k < sz.depth; ++p.k)
				{ ++count;
					object3d::value_type v(o(p) - search_val);
					object3d::value_type absv(v>0 ? v : -v);
					if (absv < 0.0001)
					{
						areas_ptrs_type arp;
						for (areas_type::iterator a(areas.begin()); a != areas.end(); ++a)
							if ( a->is_adjacent(p) ) arp.push_back(a);

						if (arp.size() > 1)
						{
							//cout << "arp.size = " << arp.size() << "\n";
							//cout << count << '/' << COUNT << "\tareas.size = " << areas.size() << '\n';
							std::sort(arp.begin(), arp.end(), *boost::lambda::_1 < *boost::lambda::_2);
							areas_ptrs_type::iterator a(arp.begin()), big(arp.end()-1), aend(arp.end()-1);
							for (; a != aend; ++a)
							{
								(*big)->add_area(**a);
								areas.erase(*a);
							}

							(*big)->add_pixel(p);
						}
						else if (arp.empty())
						{
							//cout << "arp.empty" << "\n\n";
							filled_area a(sz);
							a.add_pixel(p);
							areas.push_back(a);
							cout << count << '/' << COUNT << "\tareas.size = " << areas.size() << '\n';
						}
						else /*arp.size() == 1*/
						{
							//cout << "add_pixel" << "\n\n";
							arp.front()->add_pixel(p);
						}
					}
				}
			}
		}
		cout << "areas.size() = " << areas.size() << endl;
		areas.sort();
		areas.reverse();
		areas.erase(areas.begin());
		for (areas_type::iterator a(areas.begin()); a != areas.end(); ++a)
			a->clear(o, replace_val);

	}
	void add_mass(object3d& o, object3d::value_type search_val, long V)
	{
		//filled_area area;
		//size3d_t sz(o.get_size());
		//long rV(0);
		//for (pixel3d p(0, 0, 0); p.i < sz.height; ++p.i)
		//	for (p.j = 0; p.j < sz.width; ++p.j)
		//		for (p.k = 0; p.k < sz.depth; ++p.k)
		//		{
		//			++rV;
		//			object3d::value_type v(o(p) - search_val);
		//			object3d::value_type absv(v>0 ? v : -v);
		//			if (absv < 0.0001) area.add_pixel(p);
		//		}
		//for(; rV < V; ++rV)
		//{
		//	area.add_pixel_to_border();
		//}
	}
} //unnamed namespace

/*
 * V = 427437 pixels
 * density = 1.85
 */
object3d& apply_filter(const object3d& obj, const point3d& center, object3d& result)
{
	assert(obj.get_size() == result.get_size());
	const pixel3d pc = obj.point2pixel(center);
	const long V = 216600;
	const double d = 1.85;
	double rd(d); // real density
	long rN(1); // number of pixels in count
	result(pc) = d;
	long rV(1); // number of pixels in new object
	const int rmax(38);
	for (int r(1); r <= rmax; ++r)
	{
		/*
		 * calc main volume
		 */
		{pixel3d dp(-r, -r, -r);
			for (; dp.j <= r; ++dp.j)
			{
				for (dp.k = -r; dp.k <= r; ++dp.k)
				{
					pixel3d px(pc);
					px += dp;
					rd += obj(px);
					++rN;
					if (calc_mean(obj, px) >= (rd/rN))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		{pixel3d dp(r, -r, -r);
			for (; dp.j <= r; ++dp.j)
			{
				for (dp.k = -r; dp.k <= r; ++dp.k)
				{
					pixel3d px(pc);
					px += dp;
					rd += obj(px);
					++rN;
					if (calc_mean(obj, px) >= (rd/rN))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		{pixel3d dp(1-r, -r, -r);
			for (; dp.i < r; ++dp.i)
			{
				for (dp.k = -r; dp.k <= r; ++dp.k)
				{
					pixel3d px(pc);
					px += dp;
					rd += obj(px);
					++rN;
					if (calc_mean(obj, px) >= (rd/rN))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		{pixel3d dp(1-r, r, -r);
			for (; dp.i < r; ++dp.i)
			{
				for (dp.k = -r; dp.k <= r; ++dp.k)
				{
					pixel3d px(pc);
					px += dp;
					rd += obj(px);
					++rN;
					if (calc_mean(obj, px) >= (rd/rN))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		{pixel3d dp(1-r, 1-r, -r);
			for (; dp.j < r; ++dp.j)
			{
				for (dp.i = 1-r; dp.i < r; ++dp.i)
				{
					pixel3d px(pc);
					px += dp;
					rd += obj(px);
					++rN;
					if (calc_mean(obj, px) >= (rd/rN))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		{pixel3d dp(1-r, 1-r, r);
			for (; dp.j < r; ++dp.j)
			{
				for (dp.i = 1-r; dp.i < r; ++dp.i)
				{
					pixel3d px(pc);
					px += dp;
					rd += obj(px);
					++rN;
					if (calc_mean(obj, px) >= (rd/rN))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}

		/*
		 * fill holes
		 */
		--r;
		{pixel3d dp(-r, -r, -r);
			for (; dp.j <= r; ++dp.j)
			{
				for (dp.k = 0; dp.k <= r; ++dp.k)
				{
					pixel3d px(pc);
					px += dp;
					if (is_hole(obj, px))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		{pixel3d dp(r, -r, -r);
			for (; dp.j <= r; ++dp.j)
			{
				for (dp.k = 0; dp.k <= r; ++dp.k)
				{
					pixel3d px(pc);
					px += dp;
					if (is_hole(obj, px))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		{pixel3d dp(1-r, -r, -r);
			for (; dp.i < r; ++dp.i)
			{
				for (dp.k = 0; dp.k <= r; ++dp.k)
				{
					pixel3d px(pc);
					px += dp;
					if (is_hole(obj, px))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		{pixel3d dp(1-r, r, -r);
			for (; dp.i < r; ++dp.i)
			{
				for (dp.k = 0; dp.k <= r; ++dp.k)
				{
					pixel3d px(pc);
					px += dp;
					if (is_hole(obj, px))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		{pixel3d dp(1-r, 1-r, -r);
			for (; dp.j < r; ++dp.j)
			{
				for (dp.i = 0; dp.i < r; ++dp.i)
				{
					pixel3d px(pc);
					px += dp;
					if (is_hole(obj, px))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		{pixel3d dp(1-r, 1-r, r);
			for (; dp.j < r; ++dp.j)
			{
				for (dp.i = 0; dp.i < r; ++dp.i)
				{
					pixel3d px(pc);
					px += dp;
					if (is_hole(obj, px))
					{
						++rV;
						result(px) = d;
					}
				}
			}
		}
		++r;
		/*
		 * end of fill holes
		 */
		if (rV >= V) break;
	}

	remove_small_areas(result, d, 0.);
	remove_small_areas(result, 0., d);
	//add_mass(result, d, V);
	//cout << "new Volume = " << rV << endl;

	return result;
}

point3d calc_center(const shadow2d& sh)
{
	point3d p;
	shadow2d::const_iterator pr = sh.begin(), epr = sh.end();
	for (; pr != epr; ++pr)
		if (abs(pr->get_angle()) <= angle_t(43)) break;
	point2d p1 = pr->get_mass_center();
	angle_t a1 = pr->get_angle();
	for (; pr != epr; ++pr)
		if (pr->get_angle() == angle_t(0)) break;
	point2d p2 = pr->get_mass_center();
	angle_t a2 = pr->get_angle();

	p.z = .5*(p1.x + p2.x);
	p.y = (p1.y*cos(a2) - p2.y*cos(a1))/(sin(a1)*cos(a2) - sin(a2)*cos(a1));
	p.x = (p1.y - p.y*sin(a1))/cos(a1);

	return p;
}

object3d& filter_object(object3d& object)
{
	const int mean = 167;
	object3d::iterator o2d(object.begin()), end1(object.end());
	for (; o2d != end1; ++o2d)
	{
		object2d::iterator o(o2d->begin()), end2(o2d->end());
		for (; o != end2; ++o)
		{
			double d(*o-mean);
			*o = std::pow((d>0?d:0.), 10.);
		}
	}
	return object;
}

void black_threshold(shadow2d& sh, double threshold)
{
	shadow2d::iterator s = sh.begin(), send = sh.end();
	for (; s != send; ++s)
	{
		projection2d::iterator ps = s->begin(), psend = s->end();
		for (; ps != psend; ++ps)
		{
			projection1d::iterator pps = ps->begin(), ppsend = ps->end();
			for (; pps != ppsend; ++pps)
			{
				projection1d::value_type val = *pps;
				*pps = (val >= threshold) ? val : 0.;
			}
		}
	}
}


#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;
}