#include "sensors/SensorReader.h"
#include "Interpolator.h"
#include <sstream>

/** the step size to use for interpolating the output (in ms) */
static constexpr int stepSizeMS = 10;

/** interpolate and convert the readings for one sensor to a matLab matrix */
template <typename T> std::string toMatLab(const SensorReadings<T>& values) {

	// create and feed the interpolator with the timed sensor readings
	K::Interpolator<uint64_t, T> interpol;
	for(const auto& reading : values.values) {interpol.add(reading.ts, reading.val);}
	interpol.makeRelative();

	// create interpolated output
	const int lengthMS = interpol.values.back().key;
	std::stringstream ss;
	ss << "[" << std::endl;
	for (int ms = stepSizeMS; ms < lengthMS; ms += stepSizeMS) {
		const T cur = interpol.get(ms);
		ss << cur.x << " " << cur.y << " " << cur.z << std::endl;
	}
	ss << "];" << std::endl;

	return ss.str();

}


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

	std::cout << "converting " << (argc-1) << " files" << std::endl;

	for (int i = 1; i < argc; ++i) {

		std::string fileIn = argv[i];
		std::string fileOut = fileIn + ".m";

		// read all sensor values within the input file
		Recording rec = SensorReader::read(fileIn);

		// convert them to MatLab matrices
		std::ofstream out(fileOut);
		out << "Accel = " << toMatLab(rec.accel);
		out << "Gyro = " << toMatLab(rec.gyro);
		out << "Magnet = " << toMatLab(rec.magField);
		out.close();

	}

	return 0;

}