#include <stdio.h>
#include <stdlib.h>
#include "cs_holder.h"

const int maxRobotu = 10;
float const pomerVzdalenosti = 10;

void writeLineToLog(FILE *log)
{
	EnterCriticalSection(&GlobalCriticalSection);
	for (std::size_t robot=0; robot<robots.size(); robot++)
	{
		fprintf(log,"%i.\t", robot);
		fprintf(log,"%i\t", robots[robot].positionX);
		fprintf(log,"%i\t", robots[robot].positionY);
		fprintf(log,"%i\t", robots[robot].rotation);
		fprintf(log,"%i\t", robots[robot].leftMotorPower);
		fprintf(log,"%i\t", robots[robot].rightMotorPower);
		fprintf(log,"%i\t", robots[robot].leftMotorSpeed);
		fprintf(log,"%i\t", robots[robot].rightMotorSpeed);
		fprintf(log,"%i\t", robots[robot].led);
		fprintf(log,"%i\t", robots[robot].startButton);
		fprintf(log,"%i\t", robots[robot].leftEncoderValue);
		fprintf(log,"%i\t", robots[robot].rightEncoderValue);
		fprintf(log,"%i\t", robots[robot].leftServoValue);
		fprintf(log,"%i\t", robots[robot].rightServoValue);
		fprintf(log,"%i\t", robots[robot].lockLeftServo);
		fprintf(log,"%i\t", robots[robot].lockRightServo);
		for(int i=0;i<8;i++)
		{
			fprintf(log,"%i\t", robots[robot].sensorValues[i]);
		}
		fprintf(log,"\n");
	}
	fprintf(log,"\n");
	LeaveCriticalSection(&GlobalCriticalSection);
}

FILE * prepareDataToScript()
{
	FILE *log;
	log = fopen("outputSimulation.ms","w");

	//writing headers
	fprintf(log, "resetMaxFile #noprompt	--resetuje scénu\n\n");
	fprintf(log, "if (queryBox\"Chcete importovat vlastní model?\" beep:true)\n");
	fprintf(log, "then\n");
	fprintf(log, "(\n");
	fprintf(log, "\tmax file import\t\t-- otevre dialog importu(\n");
	fprintf(log, ")\n");
	fprintf(log, "else\n");
	fprintf(log, "(\n");
	fprintf(log, "\tbox isSelected:on\t\t-- bude pouzita krychle\n");
	fprintf(log, "\t$.name = \"robot000\"\n");
	fprintf(log, ")\n");
	fprintf(log, "for i=1 to %i do ( maxOps.cloneNodes $ cloneType:#copy )\n", maxRobotu);
	fprintf(log, "set animate on\n");
	fprintf(log, "\n");

	fprintf(log, "sliderTime = 0f\n");
	for(int i=0;i<maxRobotu;i++)
	{
		fprintf(log, "select #($robot%.3i); $.pos = [0,0,0]; rotate $ (eulerangles 0 0 0)\n", i);
	}
	fprintf(log, "\n");
	return log;
}

void writeLineToMaxScript(FILE * log, int & actualTime, std::map<int, robotProperties> & lastRobots)
{	
	EnterCriticalSection(&GlobalCriticalSection);
	int navic = 0;
	if(robots.size() > lastRobots.size())
		navic++;
	else
		navic=0;
	fprintf(log, "sliderTime = %if\n", actualTime);
	for(int i=0;i<robots.size()-navic;i++)
	{
		if(robots[i].active)
		{
			float moveX = lastRobots[i].positionX - robots[i].positionX;
			float moveY = lastRobots[i].positionY - robots[i].positionY;
			float rot = (lastRobots[i].rotation - robots[i].rotation)*180/M_PI;
			fprintf(log, "\tselect #($robot%.3i); move $ [%f,%f,0]; rotate $ (eulerangles 0 0 %f)\n", i, moveX*pomerVzdalenosti, moveY*pomerVzdalenosti, -rot);
		}
	}
	lastRobots = robots;
	LeaveCriticalSection(&GlobalCriticalSection);
	actualTime++;
}

DWORD WINAPI writeLog(LPVOID lpParam)
{
	FILE * log;
	log = prepareDataToScript();

	//writing data
	waitForConnectRobot();
	int actualTime = 1;
	EnterCriticalSection(&GlobalCriticalSection);
	std::map<int, robotProperties> lastRobots = robots;
	LeaveCriticalSection(&GlobalCriticalSection);
	while(!konec && robots.size()!=0)
	{
		writeLineToMaxScript(log, actualTime, lastRobots);
		Sleep(40);
	}
	
	//writing end data
	fprintf(log, ")\n");
	fprintf(log, "--konec skriptu");

	fclose(log);
	return 0;
}