/************************************************************************************
 * videodev-simulation:
 * creates an black screen with an small white circle at a simulated movement
 * needed for testing the PID functionality
 *
 * This is needed only for debugging.
 ************************************************************************************/

#include <ctype.h>
#ifdef BUILD_WINDOWS
#include "windows.h"
#else
#include <arpa/inet.h>
#endif
#include <sys/stat.h>
#include <math.h>

#include "debug.h"
#include "convert.h"
#include "configuration.h"
#include "videodev-simulation.h"

Simulation simulation;
gpointer simulation_threadprocess_wrapper (gpointer data);

void cb_posctl_btnsimreset (GtkWidget *widget, gpointer data) {
	simulation.Reset();
}


VideoDev_Simulation::VideoDev_Simulation() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	conf_width = 1920;
	conf_height = 1080;
	inframe = NULL;
	simulation_thread = NULL;
};


VideoDev_Simulation::~VideoDev_Simulation() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	if (inframe != NULL) Close();
	inframe = NULL;
}



/*
 * searchs in the given path for any videodump files and present them as
 * video device.
 */
int VideoDev_Simulation::GetDeviceList(std::list<std::string> *list) {
	list->push_back("SIMULATION");
	return 1;
}



/*
 * Open Device
 * prepare the buffer, InitMMAP and read all controls
 */
int VideoDev_Simulation::Open() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	if (inframe != NULL) Close();

	conf_format = convert_from_pixelformat (V4L2_PIX_FMT_RGB24);
	simulation.SetResolution(conf_width, conf_height);

	simulation_thread = g_thread_new("Simulation", simulation_threadprocess_wrapper, NULL);

	inframe = (unsigned char *) malloc (conf_width * conf_height * 3);

	return VDEV_STATUS_OK;
};


/*
 * Close Device
 * Free videobuffer
 */
int VideoDev_Simulation::Close() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	if (inframe != NULL) {
		free (inframe);
		inframe = NULL;
	}
	simulation.Stop();

	return VDEV_STATUS_OK;
};



/*****************************************************************************************************
 * VideoGrabbing
 */


/*
 * send the start capture signal to the cam
 */
int VideoDev_Simulation::CaptureStart() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	if (inframe != NULL) Close();
	if (Open() != VDEV_STATUS_OK) return VDEV_STATUS_ERROR;
	pixelformat = V4L2_PIX_FMT_RGB24;
	get_cycletime(&lastframetv);

	return VDEV_STATUS_OK;
};


int VideoDev_Simulation::CaptureStop() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	if (inframe != NULL) Close();
	return VDEV_STATUS_OK;
};


/*
 * try to grab one frame and convert it into RGB32.
 * If something goes wrong return an error code.
 * Return code VDEV_STATUS_AGAIN is not an error. There was no video image ready to read.
 */
#define SIMULATION_SIZE 16
int VideoDev_Simulation::Grab(VideoFrameRaw *vf) {
	int posx, posy, x ,y, r, radius;
	double a, dsec;

	// try to match a speed of 20Hz
	dsec = get_cycletime(&lastframetv);
	if (dsec < 0.040) usleep (50000 - dsec*1000.0);

	memset (inframe, 0x0, conf_width*conf_height*3);
	simulation.GetPos(&posx, &posy);

	radius = conf_width * SIMULATION_SIZE / 1920;
	for (r = 1; r < radius; r++) for (a = 0; a < M_PI * 2.0; a += 0.1) {
		x = posx + (sin(a) * r);
		y = posy + (cos(a) * r);
		if (x >= 0 && x < conf_width && y >= 0 && y < conf_height) {
			inframe[3*(x+y*conf_width)+0] = 200;
			inframe[3*(x+y*conf_width)+1] = 200;
			inframe[3*(x+y*conf_width)+2] = 200;
		}
	}

	LockMutex();
	vf->CopyFrom(V4L2_PIX_FMT_RGB24, conf_width, conf_height, (conf_width*conf_height*3), inframe);
	UnLockMutex();

	return VDEV_STATUS_OK;
}


/*****************************************************************************************************
 * Controls
 */

/*
 * set video control identified by id
 */
int VideoDev_Simulation::SetDevCtrl(unsigned int id, int value) {
	return VDEV_STATUS_OK;
};


/*
 * get video control identified by id
 */
int VideoDev_Simulation::GetDevCtrl(unsigned int id, int *value) {
	return VDEV_STATUS_OK;
};


/*********************************************************************************************************
 * Simulation
 */

gpointer simulation_threadprocess_wrapper (gpointer data) {
	simulation.ThreadProcess();
	return NULL;
}

Simulation::Simulation() {
	w = 1920;
	h = 1080;
	posX = w/2;
	posY = h/2;
	running = 0;

#ifdef DEBUG_POSCTL
	debug_tofile((char*)"simulation.log", 1, (char*)"dAngle,dLen,x,y,dx,dy,timedelay,a1.defAngle,a1.defLen,a1.v,a1.dx,a1.dy,a2.defAngle,a2.defLen,a2.v,a2.dx,a2.dy,finalX,finalY\n");
#endif

	Reset();
};


void Simulation::Reset() {
	int i;
	static int first = 1;

	LockMutex();

	if(first) {
	  time_t t = time(NULL);
	  srand (t);
	  first = 0;
	}

	//
	// object movement
	dAngle = 360.0 * (double)rand() / (double) RAND_MAX;
	dLen = 1.0 + 5.0 * (double)rand() / (double) RAND_MAX;
	printf ("%s:%d %s dAngle:%f    dLen:%f\n", __FILE__, __LINE__, __FUNCTION__, dAngle, dLen);

	//
	// axis movement
	i = 0;

	axis[i % 2].defAngle = 180.0 + dAngle + (10.0 * (double)rand() / ((double) RAND_MAX)) - 5.0;
	axis[i % 2].defLen = dLen + ((dLen/2.0) * (double)rand() / ((double) RAND_MAX) - (dLen/4.0));
	axis[i % 2].v = 1.0;
	axis[i % 2].active = 1;

	i++;
	axis[i % 2].defAngle = 180.0 + dAngle + (10.0 * (double)rand() / ((double) RAND_MAX)) + 85.0;
	axis[i % 2].defLen = dLen + ((dLen/2.0) * (double)rand() / ((double) RAND_MAX) - (dLen/4.0));
	axis[i % 2].v = 1.0;
	axis[i % 2].active = 1;

	// normalize angle between 0..360°
	for (i = 0; i < 2; i++) {
		while (axis[i].defAngle < 0.0) 		axis[i].defAngle += 360.0;
		while (axis[i].defAngle > 360.0) 	axis[i].defAngle -= 360.0;
	}

	printf ("%s:%d %s Axis1: %f° Len:%f   Axis2: %f° Len:%f\n", __FILE__, __LINE__, __FUNCTION__,
			axis[0].defAngle,axis[0].defLen, axis[1].defAngle,axis[1].defLen);
	UnLockMutex();
}

Simulation::~Simulation() {
	LockMutex ();
	running = 0;
	UnLockMutex ();
};


void Simulation::GetPos (int *nx, int *ny) {
	LockMutex();
	if (nx != NULL) *nx = (int)posX;
	if (ny != NULL) *ny = (int)posY;
	UnLockMutex();
}


void Simulation::ThreadProcess() {
	int r = 1;
	struct timeval tv;
	double ms;
	double dx[3], dy[3];
#ifdef DEBUG_POSCTL
	double x, y;
#endif

	get_cycletime (&tv);
	if (running) return;
	running = 1;

	do {
		usleep (20000);
		ms = get_cycletime(&tv);
		LockMutex();

		//
		// simulate rotation movement

		// calculate movement
		dx[0] = sindeg(dAngle) * dLen * ms;
		dy[0] = cosdeg(dAngle) * dLen * ms;

		//
		// simulate motor axis movement
		for (int i = 0; i < 2; i++)
			if (axis[i].active) {
				dx[i+1] = sindeg(axis[i].defAngle) * axis[i].defLen * (1.0 + axis[i].v) * ms;
				dy[i+1] = cosdeg(axis[i].defAngle) * axis[i].defLen * (1.0 + axis[i].v) * ms;
			}
			else {
				dx[i+1] = 0.0;
				dy[i+1] = 0.0;
			}

		// this is needed for debugging
#ifdef DEBUG_POSCTL
		x = posX; y = posY;
#endif
		posX = posX + dx[0] + dx[1] + dx[2];
		posY = posY + dy[0] + dy[1] + dy[2];
		if (posX < 0) posX = w - 1.0;
		if (posY < 0) posY = h - 1.0;
		if (posX > w) posX = 0.0;
		if (posY > h) posY = 0.0;

#ifdef DEBUG_POSCTL
		debug_tofile((char*)"simulation.log", 0, (char*)"%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n",
				dAngle, dLen, x, y, dx[0], dy[0], ms,
				axis[0].defAngle,axis[0].defLen,axis[0].v,dx[1],dy[1],
				axis[1].defAngle,axis[1].defLen,axis[1].v,dx[2],dy[2],
				posX, posY
				);
#endif
		r = running;
		UnLockMutex();
	} while (r);

	running = 0;
}

void Simulation::Start() {

}


void Simulation::Stop() {
	running = 0;
}


void Simulation::SetResolution (int w, int h) {
	LockMutex();
	this->w = w;
	this->h = h;
	posX = w / 2;
	posY = h / 2;
	UnLockMutex();
}


void Simulation::AxisSetValue (int a, double v) {
	// printf ("%s:%d %s Axis:%d   Value:%f\n", __FILE__, __LINE__, __FUNCTION__, a, v);
	if (a < 0 || a > 1) return;
	axis[a].v = v;
	axis[a].active = 1;
}


void Simulation::AxisStop() {
	// printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	axis[0].active = 0;
	axis[1].active = 0;
}