#include "AdvDampedHarmonicOscillator.hh"
#include <cassert>
#include <cmath>

const Double AdvDampedHarmonicOscillator::N=4.0/abs(atan2(0.0, -1.0));  // 4.0/PI=4.0/abs(atan2(0.0, -1.0)

AdvDampedHarmonicOscillator::AdvDampedHarmonicOscillator() : AdvFuncBase(std::string("Damped Harmonic Oscillator"), std::string("dho"), NumberOfParamsForDampedHarmonicOscillator) {
}

AdvDampedHarmonicOscillator::~AdvDampedHarmonicOscillator() {
}

/**
 *  evaluate the value of the function
 *
 *  \param[in] x
 *  \param[in] h
 *  \param[in] w0
 *  \param[in] g
 */
Double AdvDampedHarmonicOscillator::eval(const Double x, const Double h, const Double w0, const Double g) {

    assert(abs(w0) > abs(g));

    Double xSq=x*x;
    Double w0Sq=w0*w0;
    Double gSq=g*g;
    Double t=w0Sq-xSq;
    Double u=w0Sq-gSq;
    Double numerator  = h*sqrt(u)*g*x;
    Double denominator = t*t + 4.0*gSq*xSq;
    return N*numerator/denominator;
}

#ifdef HAVE_DIFFERENTIAL_MEMBER

Double AdvDampedHarmonicOscillator::der1st(const Double x, const Double h, const Double w0, const Double g) {
    assert(abs(w0) > abs(g));

    Double xSq=x*x;
    Double w0Sq=w0*w0;
    Double gSq=g*g;
    Double t=w0Sq-xSq;
    Double numerator  = h*g*sqrt(w0Sq-gSq)*(t*(w0Sq+3.0*xSq)-4.0*gSq*xSq);
    Double denominator = pow(t*t + 4.0*gSq*xSq, 2.0);
    return N*numerator/denominator;
}

Double AdvDampedHarmonicOscillator::der2nd(const Double x, const Double h, const Double w0, const Double g) {
    assert(abs(w0) > abs(g));

    Double xSq=x*x;
    Double w0Sq=w0*w0;
    Double gSq=g*g;
    Double t=w0Sq-xSq;
    Double u=w0Sq-gSq;
    Double numerator   =h*g*x*sqrt(u)*(3.0*t*(w0Sq+xSq)*(t-2.0*gSq)-8.0*gSq*xSq*(u));
    Double denominator = pow(t*t + 4.0*gSq*xSq, 3.0);
    return N*numerator/denominator;
}

Double AdvDampedHarmonicOscillator::derW0(const Double x, const Double h, const Double w0, const Double g) {
    assert(abs(w0) > abs(g));

    Double xSq=x*x;
    Double w0Sq=w0*w0;
    Double gSq=g*g;
    Double t=w0Sq-xSq;
    Double u=w0Sq-gSq;
    Double numerator   = h*g*w0*x*(t*(3.0*w0Sq+xSq)-4.0*gSq*xSq);
    Double denominator = sqrt(u)*pow(t*t+4.0*gSq*xSq, 2.0);
    return N*numerator/denominator;
}

Double AdvDampedHarmonicOscillator::derG(const Double x, const Double h, const Double w0, const Double g) {
    assert(abs(w0) > abs(g));

    Double xSq=x*x;
    Double w0Sq=w0*w0;
    Double gSq=g*g;
    Double t=w0Sq-xSq;
    Double u=w0Sq-gSq;
    Double numerator   = h*x*t*(w0Sq*t-2.0*gSq*(w0Sq+xSq));
    Double denominator = sqrt(u)*pow(t*t+4.0*gSq*xSq, 2.0);
    return N*numerator/denominator;
}
#endif

#ifdef USE_POINTER
Double AdvDampedHarmonicOscillator::eval(const Double x, const Double *p) {
    return this->eval(x, p[0], p[1], p[2]);
}

#ifdef HAVE_DIFFERENTIAL_MEMBER
Double AdvDampedHarmonicOscillator::der1st(const Double x, const Double *p) {
    return this->der1st(x, p[0], p[1], p[2]);
}

Double AdvDampedHarmonicOscillator::der2nd(const Double x, const Double *p) {
    return this->der2nd(x,  p[0], p[1], p[2]);
}

Double *AdvDampedHarmonicOscillator::gradient(const Double x, const Double *p) {

    Double *g = new Double[NumberOfParamsForDampedHarmonicOscillator];
    g[0]=eval( x, 1.0,  p[1], p[2]);
    g[1]=derW0(x, p[0], p[1], p[2]);
    g[2]=derG( x, p[0], p[1], p[2]);

    return g;
}
#endif // HAVE_DIFFERENTIAL_MEMBER
#endif // USE_POINTER

#ifdef USE_VECTOR
Double AdvDampedHarmonicOscillator::eval(const Double x, const std::vector<Double> &p) {
    return this->eval(x, p[0], p[1], p[2]);
}

#ifdef HAVE_DIFFERENTIAL_MEMBER
Double AdvDampedHarmonicOscillator::der1st(const Double x, const std::vector<Double> &p) {
    return this->der1st(x, p[0], p[1], p[2]);
}

Double AdvDampedHarmonicOscillator::der2nd(const Double x, const std::vector<Double> &p) {
    return this->der2nd(x,  p[0], p[1], p[2]);
}

std::vector<Double> AdvDampedHarmonicOscillator::gradient(const Double x, const std::vector<Double> &p) {
    std::vector<Double> *g = new std::vector<Double>(NumberOfParamsForDampedHarmonicOscillator);
    g->push_back(eval( x, 1.0,  p[1], p[2]));
    g->push_back(derW0(x, p[0], p[1], p[2]));
    g->push_back(derG( x, p[0], p[1], p[2]));

    return *g;
    
}
#endif // USE_VECTOR
#endif // HAVE_DIFFERENTIAL_MEMBER
