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

#include <opencv2/opencv.hpp>
#include "opencv_lib.hpp"

#include <cstdint>
#include <cstdbool>
#include "Eigen/Dense"

typedef double scalar_t;
typedef Eigen::VectorXd vector_t;
typedef const Eigen::Ref<const vector_t> vector_in_t;
typedef Eigen::Ref<vector_t> vector_io_t;

class RungeKuttaGill
{
public:
	explicit RungeKuttaGill() : m_init(false) {}
	virtual ~RungeKuttaGill() {}
	void init(void);
	void step(scalar_t dt);
	virtual void print(void);

protected:
	virtual vector_t init_y(void) = 0;
	virtual void calc_f(vector_io_t f, vector_in_t y) = 0;

protected:
	vector_t m_y;

private:
	scalar_t m_coef[7];

	bool m_init;

	vector_t m_f;
	vector_t m_k[4];
};

void RungeKuttaGill::init(void)
{
	scalar_t sqrt2 = sqrt(2.0);
	m_coef[0] = 0.5;
	m_coef[1] = 0.5 * (-1.0 + sqrt2);
	m_coef[2] = 1.0 - 0.5 * sqrt2;
	m_coef[3] = 0.5 * sqrt2;
	m_coef[4] = 1.0 + 0.5 * sqrt2;
	m_coef[5] = 2.0 - sqrt2;
	m_coef[6] = 2.0 + sqrt2;

	m_y = init_y();

	uint32_t dim = m_y.size();
	m_f = vector_t(dim);
	for (uint32_t i = 0; i < 4; i++) {
		m_k[i] = vector_t(dim);
	}
}

void RungeKuttaGill::step(scalar_t dt)
{
	calc_f(m_k[0], m_y);
	m_k[0] *= dt;
	calc_f(m_k[1], m_y + m_coef[0] * m_k[0]);
	m_k[1] *= dt;
	calc_f(m_k[2], m_y + m_coef[1] * m_k[0] + m_coef[2] * m_k[1]);
	m_k[2] *= dt;
	calc_f(m_k[3], m_y - m_coef[3] * m_k[1] + m_coef[4] * m_k[2]);
	m_k[3] *= dt;

	m_y = m_y + (1.0 / 6.0) * (m_k[0] + m_coef[5] * m_k[1] + m_coef[6] * m_k[2] + m_k[3]);
}

void RungeKuttaGill::print(void)
{
	cout << " " << m_y.transpose() << endl;
}

//

class MassPoint : public RungeKuttaGill
{
public:
	explicit MassPoint() {}
	virtual ~MassPoint() {}
protected:
	virtual vector_t init_y(void);
	virtual void calc_f(vector_io_t f, vector_in_t y);
};

vector_t MassPoint::init_y(void)
{
	vector_t y = vector_t::Zero(3);
	y(1) = 1.0;
	return y;
}

void MassPoint::calc_f(vector_io_t f, vector_in_t y)
{
	const scalar_t k = 1;
	const scalar_t m = 1;
	// t, x, dx/dt
	f(0) = 1;	// d/dt(t)
	f(1) = y(2);	// d/dt(x)
	f(2) = -(k / m) * y(1);	// d/dt(dx/dt)
}

//

class Lorenz : public RungeKuttaGill
{
public:
	explicit Lorenz() {}
	virtual ~Lorenz() {}
	virtual void Lorenz::print(void);
protected:
	virtual vector_t init_y(void);
	virtual void calc_f(vector_io_t f, vector_in_t y);
};

vector_t Lorenz::init_y(void)
{
	vector_t y = vector_t::Zero(4);
	y(1) = 1.0;
	return y;
}

void Lorenz::calc_f(vector_io_t f, vector_in_t y)
{
	const scalar_t p = 10;
	const scalar_t r = 28;
	const scalar_t b = 8.0/3.0;
	// t, x, y, z
	f(0) = 1;
	f(1) = -p * y(1) + p * y(2);
	f(2) = -y(1) * y(3) + r * y(1) - y(2);
	f(3) = y(1) * y(2) - b * y(3);
}

//

#define SIZE (320)
const char winName[] = "window";
cv::Mat img;
bool p = false;
int px, py;

void Lorenz::print(void)
{
	scalar_t sc = 6.0;
	int x = SIZE/2 + m_y(1) * sc;
	int y = SIZE - m_y(3) * sc;
	int c = 128 - m_y(2) * 10;
	if (p) {
		cv::line(img, cv::Point(px, py), cv::Point(x, y), cv::Scalar(255, 255 - c, c));
		cv::imshow(winName, img);
		//cv::waitKey(1);
	}
	p = true;
	px = x;
	py = y;
}


int main(int argc, char **argv)
{
	//MassPoint sim;
	Lorenz sim;
	img = cv::Mat::zeros(SIZE, SIZE, CV_8UC3);

	sim.init();
	sim.print();
	cv::namedWindow(winName);
	cv::VideoWriter vOut;
	vOut.open("o.avi", CV_FOURCC_DEFAULT, 29.97, img.size());

	for (int k = 0; k < 128; k++) {
		for (int j = 0; j < 64; j++) {
			for (int i = 0; i < 16; i++) {
				sim.step(1.0 / 1024.0);
				//sim.print();
			}
			sim.print();
		}
		cv::waitKey(1);
		vOut << img;
		//cv::imwrite("png/" + std::to_string(k + 100) + ".png", img);
	}
	cv::waitKey(-1);

	return 0;
}