#include "Unravel3D.hpp"
#include <algorithm>
#include <cmath>

Unravel3D::Unravel3D():
		KernelBasicParameter(7),
		KernelBasicVector(3){

}

void Unravel3D::operator()() {
	std::swap(vectorNew, vectorOld);

    vectorNew[0] = parameters[0]*(vectorOld[2] + parameters[1]);
    vectorNew[1] = parameters[2]*(vectorOld[0] + parameters[3]);
    vectorNew[2] = parameters[4]*(vectorOld[1] + parameters[5]);

    const double dist = vectorNew[0]*vectorNew[0] + vectorNew[1]*vectorNew[1] + vectorNew[2]*vectorNew[2];

    if ( dist > parameters[6]*parameters[6] ) {
        const double sqrtDist = sqrt(dist);
        const double p = 1.0 - parameters[6] * ( static_cast<int> ( sqrtDist / parameters[6] ) + 1.0 ) / sqrtDist;
        vectorNew[0] *= p;
        vectorNew[1] *= p;
        vectorNew[2] *= p;
    }
}