#include "Unravel.hpp"

Unravel::Unravel(){
    init();
}

void Unravel::init() {
    // allocation
    myParameters = new double[4];
    vectorNew = new double[3];
    vectorOld = new double[3];

    // initialisation
    assert(myParameters != NULL);
    assert(vectorNew != NULL);
    assert(vectorOld != NULL);
    for ( unsigned int i = 0; i < 7; i++ ) {
        myParameters[i] = 0.0;
    }
    for ( unsigned int i = 0; i < 3; i++ ) {
        vectorNew[i] = vectorOld[i] = 0.0;
    }
}

void Unravel::iterate() {
    swap(vectorNew, vectorOld);

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

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

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

// setters, getters, all i/o to other classes/objects
void * Unravel::getProperty(const string identifier) {
    if ( identifier == "dimension" ) {
        unsigned int * _return = new unsigned int;
        *_return = 3;
        return _return;
    }
    return NULL;
}

void Unravel::setProperty(const string identifier, const void * value) {
    return;
}

double * & Unravel::parameters() {
    return myParameters;
}

double & Unravel::parameter(const unsigned int index) {
    return myParameters[index];
}

double * & Unravel::vector() {
    return vectorNew;
}

double * & Unravel::previousVector() {
    return vectorOld;
}