#include "AdvTrapezoid.hh"
#include <cfloat>

/**
 *  constructor
 */
AdvTrapezoid::AdvTrapezoid() : AdvFuncBase(*(new string("torapezoid")), *(new string("trp")), NumberOfParamForTrapezoid) {
}

/**
 * destructor
 */
AdvTrapezoid::~AdvTrapezoid() {
}


Double AdvTrapezoid::eval(const Double x, const Double t0, const Double t1, const Double t2, const Double t3, const Double h) {
    assert(t0 < t1);
    assert(t1 < t2);
    assert(t2 < t3);

    if      (            x < t0 ) { return 0.0;                }
    else if ( t0 <= x && x < t1 ) { return h*(x - t0)/(t1-t0); }
    else if ( t1 <= x && x < t2 ) { return h;                  }
    else if ( t2 <= x && x < t3 ) { return h*(t3 - x)/(t3-t2); }
    else   /* t3 <= x          */ { return 0.0;                }
}

#ifdef HAVE_DIFFERENTIAL_MEMBER
Double AdvTrapezoid::der1st(const Double x, const Double t0, const Double t1, const Double t2, const Double t3, const Double h) {
    assert(t0 < t1);
    assert(t1 < t2);
    assert(t2 < t3);

    if      (           x < t0 ) { return  0.0;           }
    else if ( x == t0          ) { return  0.5*h/(t1-t0); }
    else if ( t0 < x && x < t1 ) { return      h/(t1-t0); }
    else if ( x == t1          ) { return  0.5*h/(t1-t0); }
    else if ( t1 < x && x < t2 ) { return  0.0;           }
    else if ( x == t2          ) { return -0.5*h/(t3-t2); }
    else if ( t2 < x && x < t3 ) { return -1.0*h/(t3-t2); }
    else if ( x == t3          ) { return -0.5*h/(t3-t2); }
    else   /* t3 < x          */ { return  0.0;           }
}

Double AdvTrapezoid::der2nd(const Double x, const Double t0, const Double t1, const Double t2, const Double t3, const Double h) {
    assert(t0 < t1);
    assert(t1 < t2);
    assert(t2 < t3);

    if      (           x < t0 ) { return  0.0;           }
    else if ( x == t0          ) { return  0.5*h/(t1-t0); }
    else if ( t0 < x && x < t1 ) { return      h/(t1-t0); }
    else if ( x == t1          ) { return  0.5*h/(t1-t0); }
    else if ( t1 < x && x < t2 ) { return  0.0;           }
    else if ( x == t2          ) { return -0.5*h/(t3-t2); }
    else if ( t2 < x && x < t3 ) { return -1.0*h/(t3-t2); }
    else if ( x == t3          ) { return -0.5*h/(t3-t2); }
    else   /* t3 < x          */ { return  0.0;           }
}

Double AdvTrapezoid::derH(const Double x, const Double t0, const Double t1, const Double t2, const Double t3, const Double h) {
    assert(t0 < t1);
    assert(t1 < t2);
    assert(t2 < t3);

    if      (            x < t0 ) { return 0.0;            }
    else if ( t0 <= x && x < t1 ) { return (x-t0)/(t1-t0); }
    else if ( t1 <= x && x < t2 ) { return 1.0;            }
    else if ( t2 <= x && x < t3 ) { return (t3-x)/(t3-t2); }
    else    /*t3 <= x            */ { return 0.0;          }
}

Double AdvTrapezoid::derT0(const Double x, const Double t0, const Double t1, const Double t2, const Double t3, const Double h) {
    assert(t0 < t1);
    assert(t1 < t2);
    assert(t2 < t3);

    if      (            x < t0 ) { return 0.0;                       }
    else if ( t0 <= x && x < t1 ) { return -h*(t1-x)/pow(t1-t0, 2.0); }
    else if ( t1 <= x && x < t2 ) { return 0.0;                       }
    else if ( t2 <= x && x < t3 ) { return 0.0;                       }
    else    /*t3 <= x            */ { return 0.0;                     }
}

Double AdvTrapezoid::derT1(const Double x, const Double t0, const Double t1, const Double t2, const Double t3, const Double h) {
    assert(t0 < t1);
    assert(t1 < t2);
    assert(t2 < t3);

    if      (            x < t0 ) { return 0.0;                       }
    else if ( t0 <= x && x < t1 ) { return -h*(x-t0)/pow(t1-t0, 2.0); }
    else if ( t1 <= x && x < t2 ) { return 0.0;                       }
    else if ( t2 <= x && x < t3 ) { return 0.0;                       }
    else    /*t3 <= x          */ { return 0.0;                       }

}

Double AdvTrapezoid::derT2(const Double x, const Double t0, const Double t1, const Double t2, const Double t3, const Double h) {
    assert(t0 < t1);
    assert(t1 < t2);
    assert(t2 < t3);

    if      (            x < t0 ) { return 0.0; }
    else if ( t0 <= x && x < t1 ) { return 0.0; }
    else if ( t1 <= x && x < t2 ) { return 0.0; }
    else if ( t2 <= x && x < t3 ) { return h*(t3-x)/pow(t3-t2, 2.0); }
    else    /*t3 <= x          */ { return 0.0; }
}

Double AdvTrapezoid::derT3(const Double x, const Double t0, const Double t1, const Double t2, const Double t3, const Double h) {
    assert(t0 < t1);
    assert(t1 < t2);
    assert(t2 < t3);

    if      (            x < t0 ) { return 0.0; }
    else if ( t0 <= x && x < t1 ) { return 0.0; }
    else if ( t1 <= x && x < t2 ) { return 0.0; }
    else if ( t2 <= x && x < t3 ) { return h*(x-t2)/pow(t3-t2, 2.0); }
    else    /*t3 <= x          */ { return 0.0; }
}
#endif // HAVE_DIFFERENTIAL_MEMBER

#ifdef USE_POINTER
/**
 *  evaluate the value of the function
 *
 *  \param[in] x aargument
 *  \param[in] p parameters for the function
 *               p[0]: height of the trapezoid
 *               p[1], p[2], p[3], p[4]
 */
Double AdvTrapezoid::eval(const Double x, const Double *p) {
    return eval(x, p[1], p[2], p[3], p[4], p[0]);
}

#ifdef HAVE_DIFFERENTIAL_MEMBER
/**
 *  evaluate the value of the 1st. derived coefficient
 *
 *  \param[in] x aargument
 *  \param[in] p parameters for the function
 *               p[0]: height of the trapezoid
 *               p[1], p[2], p[3], p[4]
 */
Double AdvTrapezoid::der1st(const Double x, const Double *p) {
    return der1st(x, p[1], p[2], p[3], p[4], p[0]);
}

/**
 *  evaluate the value of the 2nd. derivative coefficient
 *
 *  \param[in] x aargument
 *  \param[in] p parameters for the function
 *               p[0]: height of the trapezoid
 *               p[1], p[2], p[3], p[4]
 */
Double AdvTrapezoid::der2nd(const Double x, const Double *p) {
    return der1st(x, p[1], p[2], p[3], p[4], p[0]);
}

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

    Double *g = new Double[NumberOfParamForTrapezoid];

        g[0]=derH( x, p[1], p[2], p[3], p[4], p[0]);
        g[1]=derT0(x, p[1], p[2], p[3], p[4], p[0]);
        g[2]=derT1(x, p[1], p[2], p[3], p[4], p[0]);
        g[3]=derT2(x, p[1], p[2], p[3], p[4], p[0]);
        g[4]=derT3(x, p[1], p[2], p[3], p[4], p[0]);
    return g;
}

#endif // HAVE_DIFFERENTIAL_MEMBER
#endif // USE_POINTER

#ifdef USE_VECTOR
Double AdvTrapezoid::eval(const Double x, const vector<Double> &p) {
    return eval(x, p[1], p[2], p[3], p[4], p[0]);
}

#ifdef HAVE_DIFFERENTIAL_MEMBER
Double AdvTrapezoid::der1st(const Double x, const vector<Double> &p) {
    return der1st(x, p[1], p[2], p[3], p[4], p[0]);
}

Double AdvTrapezoid::der2nd(const Double x, const vector<Double> &p) {
    return der1st(x, p[1], p[2], p[3], p[4], p[0]);
}

vector<Double> AdvTrapezoid::gradient(const Double x, const vector<Double> &p) {

    vector<Double> *g = new vector<Double>();

    g->push_back(derH( x, p[1], p[2], p[3], p[4], p[0]));
    g->push_back(derT0(x, p[1], p[2], p[3], p[4], p[0]));
    g->push_back(derT1(x, p[1], p[2], p[3], p[4], p[0]));
    g->push_back(derT2(x, p[1], p[2], p[3], p[4], p[0]));
    g->push_back(derT3(x, p[1], p[2], p[3], p[4], p[0]));
    return *g;
}
#endif // HAVE_DIFFERENTIAL_MEMBER
#endif // USE_VECTOR
