/*
 * main.cpp
 *
 *  Created on: 30.11.2010
 *      Author: istsveise
 */
#include "Task.h"
#include "Display.h"
#include "Fahren.h"
#ifndef POS_H_
	#include "POS.h"
#endif
#ifndef NOTBREMSE_H_
	#include "Notbremse.h"
#endif

#include <iostream>
#include <stdlib.h>
#include <pthread.h>
#include <semaphore.h>
#include <cmath>

using namespace std;

// global mutex to protect vehicle
pthread_mutex_t vehicleMutex;

POS vehiclePosition;
double lineDir;
const double PI = 3.14159265359;

double Abstand(POS p,double Ldir)
{
	vector3d origin;
	vector3d point(p.x,p.y);
	double polx = 1.0;
	double ldirRad = Ldir*PI/180;
	double m = tan(ldirRad);
	double poly = polx*m;
	vector3d pointOnLine(polx,poly);
	vector3d normVec;
	origin.crossProduct(pointOnLine,normVec);
	normVec.normalize();
	double d = normVec.scalarProduct(point);
	return d;
}

void InitAll(int x)
{
	pthread_mutex_init(&vehicleMutex,NULL);
	pthread_mutex_lock(&vehicleMutex);
	lineDir = x;
	vehiclePosition.x = 0;
	vehiclePosition.y = 0;
	pthread_mutex_unlock(&vehicleMutex);
}

int main(int argc,char* argv[])
{
	InitAll(30); // direction of line [0, 90]

	sleep(1);

	Display D(100);
	Notbremse N(100);

	cout << "Los geht's...!" << endl;
	Fahren F(/* period in ms */ 100,
			/* speed in pixels per second */ 20,
			/* start direction [0, 90] */ 0);

	//sleep for ever
	sem_t block;
	sem_init(&block,0,0);
	sem_wait(&block);

	return EXIT_SUCCESS;
}