#include <numeric>
#include <iostream>
#include <stdexcept>

#include <Eigen/Geometry>

#define FRC_NO_ICU 1
#include "FRC.h"

#include "path.h"
#include "rnn.h"

namespace path {

static Eigen::Vector2f cart2pol(Eigen::Vector2f c)
{
	return Eigen::Vector2f(
		std::sqrt(c.array().square().sum()),
		std::atan2(c[0], c[1])
	);
}

static Eigen::Vector2f pol2cart(Eigen::Vector2f p)
{
	return Eigen::Vector2f(
		p[0] * std::sin(p[1]),
		p[0] * std::cos(p[1])
	);
}

float atan2(Eigen::Vector2f c)
{
	return std::atan2(c[0],c[1]);
}
float rad(Eigen::Vector2f c)
{
	return std::sqrt(c.array().square().sum());
}

Eigen::Vector2f norm(Eigen::Vector2f v1, Eigen::Vector2f v2, float d)
{
	return Eigen::Vector2f((v1[0]+v2[0])/d, v1[1]);
}

constexpr static unsigned char code(char c)
{
	return static_cast<unsigned char>(c)-'a';
}

spacialRepresentation::spacialRepresentation(const sKeymap& k)
	: points(Eigen::Matrix<float, 2, 26, Eigen::ColMajor>::Zero())
{
	auto s = k.exportKeys();
	for (int i = 0; i != static_cast<signed>(s.size()); ++i) {
		if (s[i] != code(' ')) {
			short c = s[i];
			// std::cout<<"s: "<<keymap(s)<<std::endl<<"i: "<<i<<" c: "<<c<<std::endl;
			// std::cout<<points<<std::endl;
			points.col(c) = Eigen::Vector2f(i/10, i%10);
		}
	}
}

unsigned char spacialRepresentation::closestKeyTo(Eigen::Vector2f pos) const
{
	Eigen::VectorXf dists =
		(points.colwise() - pos)
		.array().square().colwise().sum().sqrt();
	return std::min_element(dists.data(), dists.data()+26) - dists.data();
}

std::unique_ptr<spacialRepresentation> keymapToSpace(const sKeymap& k)
{
	return std::make_unique<spacialRepresentation>(k);
}

std::vector<Eigen::VectorXf> wordToPath(spacialRepresentation * const k, const std::string& word)
{
	// std::clog<<word<<std::endl;
	std::vector<Eigen::VectorXf> out;
	size_t ci=1;
	Eigen::Vector2f cpos(k->points.col(word[0]-'a')), cvel(0,0),
		tpos(k->points.col(word[1]-'a'));
	const static float maxvel{.4}, accel{.18}, thresh{.25};
	// std::clog<<"index: "<<ci<<std::endl
		// <<"tv=p "<<cart2pol(tpos - cpos).transpose()<<" =c  "<<(tpos - cpos).transpose()<<std::endl;
	// std::clog<<"cpos: ("<<cpos.transpose()<<") tpos: ("<<tpos.transpose()<<")"<<std::endl
		// <<"cvel=p ("<<cvel.transpose()<<") =c ("<<pol2cart(cvel).transpose()<<")"<<std::endl;
	do {
		Eigen::Vector2f tdelta(cart2pol(tpos - cpos));
	
		Eigen::VectorXf curr = Eigen::VectorXf::Zero(isize);
		auto closestKey = k->closestKeyTo(cpos);
		curr[closestKey] = 1;
		curr[26] = rad(cpos - k->points.col(closestKey));
		auto caccel = Eigen::Vector2f(
			std::min(accel, tdelta[0]),
			tdelta[1]
		);
		curr.array().segment<2>(28) = caccel.array();
		cvel = norm(caccel, cvel, (maxvel+accel)/maxvel);
		curr[27] = cvel[0];
		out.push_back(curr);
		cpos += pol2cart(cvel);
		if (cpos[0] != cpos[0] || cpos[1] != cpos[1]) {
			std::cerr<<cvel.transpose()<<std::endl;
			throw std::runtime_error("current position is nan");
		}
		if (out.size() > 60 * ci) {
			// std::clog<<"index: "<<std::endl
				// <<"tv=p "<<cart2pol(tpos - cpos).transpose()<<" =c "<<(tpos - cpos).transpose()<<std::endl;
			// std::cerr<<"cpos: ("<<cpos.transpose()<<") tpos: ("<<tpos.transpose()<<")"<<std::endl
				// <<"cvel=p ("<<cvel.transpose()<<") =c ("<<pol2cart(cvel).transpose()<<")"<<std::endl;
			throw std::runtime_error("probably in infinite loop");
		}
		// printf("\r\033[0K");
		// std::clog<<cpos.transpose()<<"; ";
		if (tdelta[0] <= thresh) {
			++ci;
			if (ci >= word.size())
				break;
			tpos = k->points.col(word[ci]-'a');
			// std::clog<<"index: "<<ci<<std::endl
				// <<"tv=p "<<cart2pol(tpos - cpos).transpose()<<" =c "<<(tpos - cpos).transpose()<<std::endl;
			// std::clog<<"cpos: ("<<cpos.transpose()<<") tpos: ("<<tpos.transpose()<<")"<<std::endl
				// <<"cvel=p ("<<cvel.transpose()<<") =c ("<<pol2cart(cvel).transpose()<<")"<<std::endl;
		}
	} while (ci < word.size());
	return out;
}

inline int chooseCategorical(Eigen::ArrayXf cats, pcg32_k64& r)
{
	std::uniform_real_distribution<float> uniform(0.0, cats.sum());
	float choose = uniform(r);
	for (int stop = 0; stop != cats.size(); ++stop) {
		choose -= cats[stop];
		if (choose <= 0) {
			return stop;
		}
	}
	//Silence a warning
	return cats.size()-1;
}

std::string wordFromPath(const std::vector<Eigen::VectorXf>& path, const std::vector<Eigen::VectorXf>& nodes, pcg32_k64& r)
{
	std::string out;
	auto alphabet = "abcdefghijklmnopqrstuvwxyz";
	for (size_t i = 0; i != path.size(); ++i) {
		if (chooseCategorical(nodes[i], r) == 0) {
			out.append(1, alphabet[chooseCategorical(path[i].head<26>(), r)]);
		}
	}
	return out;
}

}