#include "catmullromspline.h"

namespace segl {

CatmullRomSpline::CatmullRomSpline(float _s) {
	cr = new Matrix(4, 4);
	setS(_s);
}

void CatmullRomSpline::makeMatrix() {
	cr->set(-s	, 0, 0);
	cr->set( 2-s	, 0, 1);
	cr->set( s-2	, 0, 2);
	cr->set( s	, 0, 3);
	
	cr->set( 2*s	 , 1, 0);
	cr->set( s-3  , 1, 1);
	cr->set( 3-2*s, 1, 2);
	cr->set(-s    , 1, 3);

	cr->set(-s, 2, 0);
	cr->set( 0, 2, 1);
	cr->set( s, 2, 2);
	cr->set( 0, 2, 3);

	cr->set( 0, 3, 0);
	cr->set( 1, 3, 1);
	cr->set( 0, 3, 2);
	cr->set( 0, 3, 3);
}

Matrix CatmullRomSpline::getPosition(float u, Matrix &controlpoints) {
// 	Matrix controlpoints(4, a.getN());
// 	for(int i=0; i<a.getN(); i++) {
// 		controlpoints.set(a.get(0, i), 0, i);
// 		controlpoints.set(b.get(0, i), 1, i);
// 		controlpoints.set(c.get(0, i), 2, i);
// 		controlpoints.set(d.get(0, i), 3, i);
// 	}
	Matrix param(1, 4);
	param.set(u*u*u, 0, 0);
	param.set(u*u,   0, 1);
	param.set(u,     0, 2);
	param.set(1,     0, 3);
	
	return param * (*cr) * controlpoints;
}

Punkt3D CatmullRomSpline::getPosition(float u, Punkt3D a, Punkt3D b, Punkt3D c, Punkt3D d) {
	Matrix erg(1, 3);
	Matrix controlpoints(4, 3);
	
	controlpoints.set(a.x, 0, 0);
	controlpoints.set(a.y, 0, 1);
	controlpoints.set(a.z, 0, 2);
	
	controlpoints.set(b.x, 1, 0);
	controlpoints.set(b.y, 1, 1);
	controlpoints.set(b.z, 1, 2);
	
	controlpoints.set(c.x, 2, 0);
	controlpoints.set(c.y, 2, 1);
	controlpoints.set(c.z, 2, 2);
	
	controlpoints.set(d.x, 3, 0);
	controlpoints.set(d.y, 3, 1);
	controlpoints.set(d.z, 3, 2);
	
	erg = getPosition(u, controlpoints);
	
	return Punkt3D(erg.get(0,0), erg.get(0,1), erg.get(0,2));
}

Punkt2D CatmullRomSpline::getPosition(float u, Punkt2D a, Punkt2D b, Punkt2D c, Punkt2D d) {
	Matrix erg(1, 2);
	Matrix controlpoints(4, 2);
	
	controlpoints.set(a.x, 0, 0);
	controlpoints.set(a.y, 0, 1);
	
	controlpoints.set(b.x, 1, 0);
	controlpoints.set(b.y, 1, 1);
	
	controlpoints.set(c.x, 2, 0);
	controlpoints.set(c.y, 2, 1);
	
	controlpoints.set(d.x, 3, 0);
	controlpoints.set(d.y, 3, 1);
	
	erg = getPosition(u, controlpoints);
	
	return Punkt2D(erg.get(0,0), erg.get(0,1));
}


void CatmullRomSpline::setS(float _s) {
	s = _s;
	makeMatrix();
}

CatmullRomSpline::~CatmullRomSpline() {
	delete(cr);
}

} // namespace segl