#ifndef USINGPCA_H
#define USINGPCA_H

#include <vector>

#include "sensors/SensorReader.h"
#include "Interpolator.h"

#include <eigen3/Eigen/Dense>

enum class PracticeType {
	//REST,
	JUMPING_JACK,
	SITUPS,
	PUSHUPS,
	KNEEBEND,
	FORWARDBEND,
};

struct Practice {

	PracticeType type;
	Recording rec;
	std::vector<uint64_t> keyGyro;

	//Practice(const PracticeType p, const Recording& rec, const std::vector<uint64_t>& keyGyro) : p(p), rec(rec), keyGyro(keyGyro) {;}

	K::Interpolator<uint64_t, SensorGyro> getInterpol() const {
		K::Interpolator<uint64_t, SensorGyro> interpol;
		for (auto it : rec.gyro.values) {interpol.add(it.ts, it.val);}
		interpol.makeRelative();
		return interpol;
	}

};

class UsingPCA {


public:

	static Eigen::VectorXf getWindow(Practice& p, uint64_t pos) {
		K::Interpolator<uint64_t, SensorGyro> interpol = p.getInterpol();
		Eigen::VectorXf vec(600/50*3, 1);
		int idx = 0;
		for (int offset = -300; offset < 300; offset += 50) {
			SensorGyro gyro = interpol.get(pos + offset);
			vec(idx++,0) = (gyro.x);
			vec(idx++,0) = (gyro.y);
			vec(idx++,0) = (gyro.z);
		}
		std::cout << vec << std::endl;
		return vec;
	}

	static std::vector<Eigen::VectorXf> getClassWindows(Practice& p) {

		std::vector<Eigen::VectorXf> windows;
		for (uint64_t pos = 1000; pos < 5000; pos += 500) {
			Eigen::VectorXf window = getWindow(p, pos);
			windows.push_back(window);
		}
		return windows;

	}

	static Eigen::MatrixXf getR(std::vector<Eigen::VectorXf>& vecs) {
		Eigen::MatrixXf mat = Eigen::MatrixXf::Zero(vecs[0].rows(), vecs[0].rows());
		for (const Eigen::VectorXf& vec : vecs) {
			mat += vec * vec.transpose();
		}
		mat /= vecs.size();
		return mat;
	}

	static Eigen::VectorXf getM(std::vector<Eigen::VectorXf>& vecs) {
		Eigen::MatrixXf mat = Eigen::MatrixXf::Zero(vecs[0].rows(), vecs[0].cols());
		for (const Eigen::VectorXf& vec : vecs) {
			mat += vec;
		}
		mat /= vecs.size();
		return mat;
	}

	static void run() {

		std::vector<Practice> practices;

		practices.push_back(
					Practice {
						PracticeType::JUMPING_JACK,
						SensorReader::read("/mnt/firma/kunden/HandyGames/daten/jumpingjack/jumpingjack_gl_5_subject_3_left.txt"),
						{1950, 2900, 3850, 4850, 5850, 6850, 7850, 8850, 9800, 10800, 11850}
					}
					);


		std::vector<Eigen::VectorXf> windows = getClassWindows(practices.back());
		Eigen::MatrixXf R = getR(windows);
		Eigen::MatrixXf m = getM(windows);
		Eigen::MatrixXf Q = R - (m * m.transpose());

		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
		es.compute(Q);

		int i = 0;

	}

};

#endif // USINGPCA_H