#include "forcespr.h"

//template Array<SparsePaird>;

// r= rest length
// k= spring constant
// p,q = indices of particles (endpoints)

ForceSpr::ForceSpr(int p, int q, double k, double r) 
: Force(), _p(p), _q(q), _k(k), _r(r) 
{  
    // Make sure p<q, else swap
    assert(_p!=_q);
    if (_p > _q) 
    {
        int tmp = _p;
        _p = _q;
        _q = tmp;
    }
}


// returns force vector for this configuration (v,m)
SparseVecd ForceSpr::eval(const Vecd &x, const Vecd &v, const Vecd &m) const
{
    Vec3d pv( x[O(_p,0)], x[O(_p,1)], x[O(_p,2)] );
    Vec3d qv( x[O(_q,0)], x[O(_q,1)], x[O(_q,2)] );
    
    Vec3d  d    = pv-qv;
    double dlen = len(d);
    
    Vec3d  fq   = _k*(dlen-_r)*d/dlen;
    Vec3d  fp   = -fq;
    
    return SparseVecd(
        x.Elts(),
        O(_p,0), fp[0],
        O(_p,1), fp[1],
        O(_p,2), fp[2],
        O(_q,0), fq[0],
        O(_q,1), fq[1],
        O(_q,2), fq[2],
        VL_SV_END);
}

// delta from rest length
double ForceSpr::Displacement(const Vecd &x) const
{
    Vec3d pv( x[O(_p,0)], x[O(_p,1)], x[O(_p,2)] );
    Vec3d qv( x[O(_q,0)], x[O(_q,1)], x[O(_q,2)] );
    
    Vec3d  d    = pv-qv;
    double dlen = len(d);

    return (fabs(dlen-_r)/_r);
}

// derivative of force relative to position
// (if we want strings w/o compression, return 0 if
//  in compressive state)

Force::DblSparse_t ForceSpr::evalDx(const Vecd &x, const Vecd &v, const Vecd &m) const
{
	int n = x.Elts();

    // Get the two point positions
    const double x0(x[O(_p,0)]), x1(x[O(_p,1)]), x2(x[O(_p,2)]);
    const double y0(x[O(_q,0)]), y1(x[O(_q,1)]), y2(x[O(_q,2)]);
    const double k(_k);
    const double r(_r);
    
    // Holy shit, Maple
    Mat3d J;
    double t1 = x0*x0;
    double t4 = y0*y0;
    double t5 = y1*y1;
    double t8 = x1*x1;
    double t9 = y2*y2;
    double t12 = x2*x2;
    double t13 = t1-2.0*x0*y0+t4+t5-2.0*y1*x1+t8+t9-2.0*y2*x2+t12;
    double t14 = sqrt(t13);
    double t16 = r/t14;
    double t17 = y0-x0;
    double t18 = r*t17;
    double t20 = 1/t14/t13;
    double t26 = k*r;
    double t27 = t20*t17;
    double t28 = x1-y1;
    double t32 = x2-y2;
    double t47 = -t28*t20;
    double t51 = -r*t28;
    double t71 = -t32*t20;
    double t78 = -r*t32;
    
    J[0][0] = k*(-1.0+t16-t18*t20*t17);
    J[0][1] = t26*t27*t28;
    J[0][2] = t26*t27*t32;
    J[1][0] = -t26*t47*t17;
    J[1][1] = k*(-1.0+t16+t51*t28*t20);
    J[1][2] = t26*t47*t32;
    J[2][0] = -t26*t71*t17;
    J[2][1] = t26*t71*t28;
    J[2][2] = k*(-1.0+t16+t78*t32*t20);
    
    // This is the block at p,p in the matrix
    // also at block q,q.  Negative of this in p,q and q,p
    
    DblSparse_t ret(6);
    
    ret[0].first = O(_p,0);
    ret[1].first = O(_p,1);
    ret[2].first = O(_p,2);
    ret[3].first = O(_q,0);
    ret[4].first = O(_q,1);
    ret[5].first = O(_q,2);
    
	ret[0].second.SetSize(n);
    ret[0].second.SetElts(
        O(_p,0), J[0][0] ,
        O(_p,1), J[0][1] ,
        O(_p,2), J[0][2] ,
        O(_q,0), -J[0][0],
        O(_q,1), -J[0][1],
        O(_q,2), -J[0][2],
        VL_SV_END);
    
	ret[1].second.SetSize(n);
    ret[1].second.SetElts(
        O(_p,0), J[1][0] ,
        O(_p,1), J[1][1] ,
        O(_p,2), J[1][2] ,
        O(_q,0), -J[1][0],
        O(_q,1), -J[1][1],
        O(_q,2), -J[1][2],
        VL_SV_END);

    ret[2].second.SetSize(n);
    ret[2].second.SetElts(
        O(_p,0), J[2][0] ,
        O(_p,1), J[2][1] ,
        O(_p,2), J[2][2] ,
        O(_q,0), -J[2][0],
        O(_q,1), -J[2][1],
        O(_q,2), -J[2][2],
        VL_SV_END);
    
//	ret[3].second.SetSize(n);
    ret[3].second = -ret[0].second;
    /*ret[3].second.SetElts(
        O(_p,0), -J[0][0],
        O(_p,1), -J[0][1],
        O(_p,2), -J[0][2],
        O(_q,0), J[0][0] ,
        O(_q,1), J[0][1] ,
        O(_q,2), J[0][2] ,
        VL_SV_END);
    */
	//ret[4].second.SetSize(n);
    ret[4].second = -ret[1].second;
/*    ret[4].second.SetElts(
        O(_p,0), -J[1][0],
        O(_p,1), -J[1][1],
        O(_p,2), -J[1][2],
        O(_q,0), J[1][0] ,
        O(_q,1), J[1][1] ,
        O(_q,2), J[1][2] ,
        VL_SV_END); 
  */  
    ret[5].second = -ret[2].second;

    //ret[5].second.SetSize(n);

   /* ret[5].second.SetElts(
        O(_p,0), -J[2][0],
        O(_p,1), -J[2][1],
        O(_p,2), -J[2][2],
        O(_q,0), J[2][0] ,
        O(_q,1), J[2][1] ,
        O(_q,2), J[2][2] ,
        VL_SV_END);
    */
    return ret;
}

// how does it change with velocity it doesn't 
// (we'd use it for viscosity, but we dont care)
Force::DblSparse_t ForceSpr::evalDv(const Vecd &x, const Vecd &v, const Vecd &m) const
{
    return DblSparse_t();
}


