// Camera.cpp: Implementierung der Klasse CCamera.
//
//////////////////////////////////////////////////////////////////////

#include "Camera.h"
#include <math.h>

//////////////////////////////////////////////////////////////////////
// Konstruktion/Destruktion
//////////////////////////////////////////////////////////////////////

CCamera::CCamera()
{
	CameraEvent[0] = new CCameraEvent(0.0, 0.0);
	m_iEvents = 1;
	CameraEvent[m_iEvents-1]->endvalue[CAMVALUE_POS_Z][0] = CameraEvent[m_iEvents-1]->pos_z[0];
	m_iActEvent = 0;
	m_dActTime = 0.0;
	m_iRound = 0;
	m_dRoundDuration = 0.0;
}

CCamera::~CCamera()
{

}

void CCamera::AddEvent(double duration) {
	CameraEvent[m_iEvents++] = new CCameraEvent(m_dRoundDuration, duration);
	CameraEvent[m_iEvents-1]->pos_z[0] = CameraEvent[m_iEvents-1]->endvalue[CAMVALUE_POS_Z][0] = CameraEvent[m_iEvents-2]->GetEndValue(CAMVALUE_POS_Z, 0);
	CameraEvent[m_iEvents-1]->rot_x[0] = CameraEvent[m_iEvents-1]->endvalue[CAMVALUE_ROT_X][0] = CameraEvent[m_iEvents-2]->GetEndValue(CAMVALUE_ROT_X, 0);
	CameraEvent[m_iEvents-1]->rot_y[0] = CameraEvent[m_iEvents-1]->endvalue[CAMVALUE_ROT_Y][0] = CameraEvent[m_iEvents-2]->GetEndValue(CAMVALUE_ROT_Y, 0);
	CameraEvent[m_iEvents-1]->rot_z[0] = CameraEvent[m_iEvents-1]->endvalue[CAMVALUE_ROT_Z][0] = CameraEvent[m_iEvents-2]->GetEndValue(CAMVALUE_ROT_Z, 0);
	m_dRoundDuration += duration;
}

void CCamera::UseConst(double y, double* erg) {
	erg[0] = y;
	erg[1] = 0.0;
	erg[2] = 0.0;
	erg[3] = 0.0;
	erg[4] = 0.0;
	erg[5] = 0.0;
}

void CCamera::UseLinear(double x0, double y0, double x1, double y1, double* erg) {
	erg[0] = y0;
	erg[1] = (y0-y1) / (x0-x1);
	erg[2] = 0.0;
	erg[3] = 0.0;
	erg[4] = 0.0;
	erg[5] = 0.0;
}

void CCamera::SetValue(int camvalue, int camtype, double y, double dy, double ddy) {
	double* erg = CameraEvent[m_iEvents-1]->values[camvalue];
	switch (camtype) {
	case CAM_CONSTANT: { 
		CameraEvent[m_iEvents-1]->endvalue[camvalue][0] = CameraEvent[m_iEvents-2]->GetEndValue(camvalue, 0);
		CameraEvent[m_iEvents-1]->endvalue[camvalue][1] = 0.0;
		CameraEvent[m_iEvents-1]->endvalue[camvalue][2] = 0.0;
		break; }
	case CAM_LINEAR: { 
		CameraEvent[m_iEvents-1]->endvalue[camvalue][0] = y;
		CameraEvent[m_iEvents-1]->endvalue[camvalue][2] = 0.0;
		break; }
	case CAM_SPEED: { 
		CameraEvent[m_iEvents-1]->endvalue[camvalue][0] = y;
		CameraEvent[m_iEvents-1]->endvalue[camvalue][1] = dy;
		break; }
	case CAM_ACCELERATION: { 
		CameraEvent[m_iEvents-1]->endvalue[camvalue][0] = y;
		CameraEvent[m_iEvents-1]->endvalue[camvalue][1] = dy;
		CameraEvent[m_iEvents-1]->endvalue[camvalue][2] = ddy;
		break; }
	}
	
//	double EventStart = CameraEvent[m_iEvents-1]->t_start;
	double EventStart = 0.0;
//	double EventEnd = EventStart + CameraEvent[m_iEvents-1]->t_duration;
	double EventEnd = CameraEvent[m_iEvents-1]->t_duration;

	switch (camtype) {
		case CAM_CONSTANT: { UseConst(CameraEvent[m_iEvents-2]->GetEndValue(camvalue, 0), erg); 
							 break; }
		case CAM_LINEAR:   { UseLinear(	EventStart,
										CameraEvent[m_iEvents-2]->GetEndValue(camvalue, 0),
										EventEnd,
										CameraEvent[m_iEvents-1]->GetEndValue(camvalue, 0),
										erg);
										CameraEvent[m_iEvents-1]->endvalue[camvalue][1] = erg[1];
							 break; }
		case CAM_SPEED:    { UseSpeed(	EventStart,
										CameraEvent[m_iEvents-2]->GetEndValue(camvalue, 0),
										EventEnd,
										CameraEvent[m_iEvents-1]->GetEndValue(camvalue, 0),
										EventStart,
										CameraEvent[m_iEvents-2]->GetEndValue(camvalue, 1),
										EventEnd,
										CameraEvent[m_iEvents-1]->GetEndValue(camvalue, 1),
										erg); 
										CameraEvent[m_iEvents-1]->endvalue[camvalue][2] = erg[1] + 2 * erg[2] * EventEnd + 3 * erg[3] * EventEnd * EventEnd ;
							 break; }
		case CAM_ACCELERATION:{ UseAccel(EventStart,
										CameraEvent[m_iEvents-2]->GetEndValue(camvalue, 0),
										EventEnd,
										CameraEvent[m_iEvents-1]->GetEndValue(camvalue, 0),
										EventStart,
										CameraEvent[m_iEvents-2]->GetEndValue(camvalue, 1),
										EventEnd,
										CameraEvent[m_iEvents-1]->GetEndValue(camvalue, 1),
										EventStart,
										CameraEvent[m_iEvents-2]->GetEndValue(camvalue, 2),
										EventEnd,
										CameraEvent[m_iEvents-1]->GetEndValue(camvalue, 2),
										erg); 
							 break; }
	}
}

void CCamera::IncActTime(double time) {
	SetActTime(time + m_dActTime);
}

void CCamera::SetActTime(double time) {
	if (m_iEvents <= 1) {
	  m_iActEvent = 0;
	  m_dActTime = 0.0;
	  return;
	}
	
	while ((time - m_iRound * m_dRoundDuration) >= m_dRoundDuration) {m_iRound++; m_iActEvent = 0;}
	m_dActTime = time - m_iRound * m_dRoundDuration;
	
	while ((time - m_iRound * m_dRoundDuration) >= CameraEvent[m_iActEvent]->t_start + CameraEvent[m_iActEvent]->t_duration) m_iActEvent++;
}

double CCamera::GetActTime() {
	return m_dActTime;
}
int CCamera::GetActEvent() {
	return m_iActEvent;
}

double CCamera::GetActValue(int camvalue) {
	return CameraEvent[m_iActEvent]->GetValue(camvalue, m_dActTime - CameraEvent[m_iActEvent]->t_start);
}

/* GAUSSIAN ELIMINATION ROUTINE, 9/8/95, JOHN STEARNS */
/* C LANGUAGE VERSION.  SOLVES SYSTEM OF SIMULATNOUS EQUATIONS */
/* A x = b, WHERE A IS ANY GENERAL MATRIX (NEED NOT BE SYMMETRIC) */
/* PROGRAM VARIABLES: */
/* a[i][j]= A MATRIX */
/* c[i]= RHS OF EQAUTION, b   */
/* x[i]= SOLUTION DESIRED */
/* Modified by Snubnose 12.03.2003 */
void CCamera::UseSpeed(double x0, double y0, double x1, double y1, double dx2, double dy2, double dx3, double dy3, double* erg) {
double pitemp,pidum,pibig,am,susum;
double ergebnorm[2], norm[2];
double ergebableit[2], ableit[2];
double x[5],a[5][5],c[5];
int i,n,j,ell,i1,elx,pij,i2,sul,ix,sui,suj,jx;

n = 4;

norm[0] = x0;
norm[1] = x1;
ergebnorm[0] = y0;
ergebnorm[1] = y1;
ableit[0] = dx2;
ableit[1] = dx3;
ergebableit[0] = dy2;
ergebableit[1] = dy3;

/*
for (j=1;j<=2;j++)
	{
	cout <<"Normal  "<<j<<"\n";
	cin >> norm[j-1];
	cout <<"Ergeb  "<<j<<"\n";
	cin >> ergebnorm[j-1];
	}
for (j=1;j<=2;j++)
	{
	cout <<"Ableit  "<<j<<"\n";
	cin >> ableit[j-1];
	cout <<"Ergeb  "<<j<<"\n";
	cin >> ergebableit[j-1];
	}
*/
	a[1][1] = norm[0] * norm[0] * norm[0];
	a[1][2] = norm[0] * norm[0];
	a[1][3] = norm[0];
	a[1][4] = 1.0;
	c[1]    = ergebnorm[0];

	a[2][1] = norm[1] * norm[1] * norm[1];
	a[2][2] = norm[1] * norm[1];
	a[2][3] = norm[1];
	a[2][4] = 1.0;
	c[2]    = ergebnorm[1];

	a[3][1] = 3 * ableit[0] * ableit[0];
	a[3][2] = 2 * ableit[0];
	a[3][3] = 1.0;
	a[3][4] = 0.0;
	c[3]    = ergebableit[0];

	a[4][1] = 3 * ableit[1] * ableit[1];
	a[4][2] = 2 * ableit[1];
	a[4][3] = 1.0;
	a[4][4] = 0.0;
	c[4]    = ergebableit[1];
#ifdef ShowGauss
	cout << "P1("<<norm[0]<<"|"<<ergebnorm[0]<<")\n";
	cout << "P2("<<norm[1]<<"|"<<ergebnorm[1]<<")\n";
	cout << "A1("<<ableit[0]<<"|"<<ergebableit[0]<<")\n";
	cout << "A2("<<ableit[1]<<"|"<<ergebableit[1]<<")\n";

	for (i=1;i<=n;i++)
	{
        cout<<"\n";
		for (j=1;j<=n;j++)
		{
		cout <<a[i][j]<<"\t";
		}
	cout<<" ="<<c[i]<<"\n";
	}
#endif
/* FORWARD ELIMINATION */
ell=n-1;
for (i1=1;i1<=ell;i1++)
{
	elx=i1+1;
	pij=i1;
	pibig=fabs(a[i1][i1]);
	for (i=elx;i<=n;i++)
	{
		am=fabs(a[i][i1]);
		if (am>pibig)
		{
			pibig=am;
			pij=i;
		}
	}
	if (pij != i1)
	{
		for (j=i1;j<=n;j++)
		{
		pidum=a[pij][j];
		a[pij][j]=a[i1][j];
		a[i1][j]=pidum;
		}
	pitemp=c[pij];
	c[pij]=c[i1];
	c[i1]=pitemp;

	}
	for (i2=elx;i2<=n;i2++)
	{
		for (j=elx;j<=n;j++)
		{
		a[i2][j]=a[i2][j]-a[i2][i1]/a[i1][i1]*a[i1][j];
		}
	c[i2]=c[i2]-a[i2][i1]/a[i1][i1]*c[i1];
	}
} /* NEXT i1 */
/* BACK SUBSTITUTE */
sul=n-1;
x[n]=c[n]/a[n][n];
for (ix=1;ix<=sul;ix++)
{
	susum=0;
	sui=n-ix;
	suj=sui+1;
	for (jx=suj;jx<=n;jx++)
		{
		susum=susum+(a[sui][jx]*x[jx]);
		}
	x[sui]=(c[sui]-susum)/a[sui][sui];
}
erg[5] = 0.0;
erg[4] = 0.0;
erg[3] = x[1];
erg[2] = x[2];
erg[1] = x[3];
erg[0] = x[4];
#ifdef ShowGauss	
cout<<"\n\nSolutions to the equations\n";
for (i=1;i<=n;i++)
{
cout <<"x("<<i<<") ="<<x[i]<<"\n";
}
cout << "\n";
#endif
}

void CCamera::UseAccel(double x0, double y0, double x1, double y1, double dx2, double dy2, double dx3, double dy3, double ddx4, double ddy4, double ddx5, double ddy5, double* erg) {
double pitemp,pidum,pibig,am,susum;
double ergebnorm[2], norm[2];
double ergebableit[2], ableit[2];
double ergebableit2[2], ableit2[2];
double x[7],a[7][7],c[7];
int i,n,j,ell,i1,elx,pij,i2,sul,ix,sui,suj,jx;

n = 6;

norm[0] = x0;
norm[1] = x1;
ergebnorm[0] = y0;
ergebnorm[1] = y1;
ableit[0] = dx2;
ableit[1] = dx3;
ergebableit[0] = dy2;
ergebableit[1] = dy3;
ableit2[0] = ddx4;
ableit2[1] = ddx5;
ergebableit2[0] = ddy4;
ergebableit2[1] = ddy5;

/*
for (j=1;j<=2;j++)
	{
	cout <<"Normal  "<<j<<"\n";
	cin >> norm[j-1];
	cout <<"Ergeb  "<<j<<"\n";
	cin >> ergebnorm[j-1];
	}
for (j=1;j<=2;j++)
	{
	cout <<"Ableit  "<<j<<"\n";
	cin >> ableit[j-1];
	cout <<"Ergeb  "<<j<<"\n";
	cin >> ergebableit[j-1];
	}
*/
	a[1][1] = norm[0] * norm[0] * norm[0] * norm[0] * norm[0];
	a[1][2] = norm[0] * norm[0] * norm[0] * norm[0] ;
	a[1][3] = norm[0] * norm[0] * norm[0];
	a[1][4] = norm[0] * norm[0];
	a[1][5] = norm[0];
	a[1][6] = 1.0;
	c[1]    = ergebnorm[0];

	a[2][1] = norm[1] * norm[1] * norm[1] * norm[1] * norm[1];
	a[2][2] = norm[1] * norm[1] * norm[1] * norm[1];
	a[2][3] = norm[1] * norm[1] * norm[1];
	a[2][4] = norm[1] * norm[1];
	a[2][5] = norm[1];
	a[2][6] = 1.0;
	c[2]    = ergebnorm[1];

	a[3][1] = 5 * ableit[0] * ableit[0] * ableit[0] * ableit[0];
	a[3][2] = 4 * ableit[0] * ableit[0] * ableit[0];
	a[3][3] = 3 * ableit[0] * ableit[0];
	a[3][4] = 2 * ableit[0];
	a[3][5] = 1.0;
	a[3][6] = 0.0;
	c[3]    = ergebableit[0];

	a[4][1] = 5 * ableit[1] * ableit[1] * ableit[1] * ableit[1];
	a[4][2] = 4 * ableit[1] * ableit[1] * ableit[1];
	a[4][3] = 3 * ableit[1] * ableit[1];
	a[4][4] = 2 * ableit[1];
	a[4][5] = 1.0;
	a[4][6] = 0.0;
	c[4]    = ergebableit[1];

	a[5][1] = 20 * ableit2[0] * ableit2[0] * ableit2[0];
	a[5][2] = 12 * ableit2[0] * ableit2[0];
	a[5][3] = 6  * ableit2[0];
	a[5][4] = 2;
	a[5][5] = 0.0;
	a[5][6] = 0.0;
	c[5]    = ergebableit2[0];

	a[6][1] = 20 * ableit2[1] * ableit2[1] * ableit2[1];
	a[6][2] = 12 * ableit2[1] * ableit2[1];
	a[6][3] = 6  * ableit2[1];
	a[6][4] = 2;
	a[6][5] = 0.0;
	a[6][6] = 0.0;
	c[6]    = ergebableit2[1];

#ifdef ShowGauss
	cout << "P1("<<norm[0]<<"|"<<ergebnorm[0]<<")\n";
	cout << "P2("<<norm[1]<<"|"<<ergebnorm[1]<<")\n";
	cout << "A1("<<ableit[0]<<"|"<<ergebableit[0]<<")\n";
	cout << "A2("<<ableit[1]<<"|"<<ergebableit[1]<<")\n";
	cout << "B1("<<ableit2[0]<<"|"<<ergebableit2[0]<<")\n";
	cout << "B2("<<ableit2[1]<<"|"<<ergebableit2[1]<<")\n";

	for (i=1;i<=n;i++)
	{
        cout<<"\n";
		for (j=1;j<=n;j++)
		{
		cout <<a[i][j]<<"\t";
		}
	cout<<" ="<<c[i]<<"\n";
	}
#endif
/* FORWARD ELIMINATION */
ell=n-1;
for (i1=1;i1<=ell;i1++)
{
	elx=i1+1;
	pij=i1;
	pibig=fabs(a[i1][i1]);
	for (i=elx;i<=n;i++)
	{
		am=fabs(a[i][i1]);
		if (am>pibig)
		{
			pibig=am;
			pij=i;
		}
	}
	if (pij != i1)
	{
		for (j=i1;j<=n;j++)
		{
		pidum=a[pij][j];
		a[pij][j]=a[i1][j];
		a[i1][j]=pidum;
		}
	pitemp=c[pij];
	c[pij]=c[i1];
	c[i1]=pitemp;

	}
	for (i2=elx;i2<=n;i2++)
	{
		for (j=elx;j<=n;j++)
		{
		a[i2][j]=a[i2][j]-a[i2][i1]/a[i1][i1]*a[i1][j];
		}
	c[i2]=c[i2]-a[i2][i1]/a[i1][i1]*c[i1];
	}
} /* NEXT i1 */
/* BACK SUBSTITUTE */
sul=n-1;
x[n]=c[n]/a[n][n];
for (ix=1;ix<=sul;ix++)
{
	susum=0;
	sui=n-ix;
	suj=sui+1;
	for (jx=suj;jx<=n;jx++)
		{
		susum=susum+(a[sui][jx]*x[jx]);
		}
	x[sui]=(c[sui]-susum)/a[sui][sui];
}
erg[5] = x[1];
erg[4] = x[2];
erg[3] = x[3];
erg[2] = x[4];
erg[1] = x[5];
erg[0] = x[6];
#ifdef ShowGauss	
cout<<"\n\nSolutions to the equations\n";
for (i=1;i<=n;i++)
{
cout <<"x("<<i<<") ="<<x[i]<<"\n";
}
cout << "\n";
#endif
}
