#include "vl/VLd.h"
#include "forcegrav.h"
#include "forcevisc.h"
#include "forcespr.h"
#include "odesolver.h"
#include "odeexpeul.h"
#include "odeexprk4.h"
#include "odeimpeul.h"
#include "particlesys.h"

#include "rcpointer.h"
#include <vector>
using namespace std;

SparseMatd tosparse(const Force::DblSparse_t &ds, int n)
{
	SparseMatd M(n,n);
	for (unsigned int i=0; i<ds.size(); i++)
	{
		M[ds[i].first] = ds[i].second;
	}

	return M;
}

int test1()
{
	Vecd x(3,vl_zero), v(3,vl_zero);
	Vecd m(1,vl_one);
	x[1] = 10;
	v[0] = 15;

	cout << x << v << endl;

	ForceGrav fg(Vec3d(0,-9.8,0));
	ForceVisc fv(0.1);
	
	cout << fg.eval(x,v,m) << endl;
	cout << fv.eval(x,v,m) << endl;
	cout << "anal sex!" << endl;

	cout << tosparse( fv.evalDv(x,v,m),3 ) << endl;
	return 0;
}

int test2()
{
	Vecd x(6,vl_zero), v(6,vl_zero);
	Vecd m(2,vl_one);

	x[3] = x[4] = x[5] = 1;

	cout << x << v << endl;

	ForceSpr fs(0,1,4,10);

	cout << fs.eval(x,v,m) << endl;
	cout << tosparse( fs.evalDx(x,v,m), 6 ) << endl;

//	fs.evalDx(x,v,m)


	return 0;
}

class Func : public OdeFunc2
{
public:
    Vecd operator()(const Vecd &x, const Vecd &v) const
    {
        return -x;
    }

    SparseMatd dx(const Vecd &x, const Vecd &v) const { return SparseMatd(); }
    SparseMatd dv(const Vecd &x, const Vecd &v) const { return SparseMatd(); }
};

class Func2 : public OdeFunc2
{
    Vecd operator()(const Vecd &x, const Vecd &v) const
    {
        return -x;
    }

    SparseMatd dx(const Vecd &x, const Vecd &v) const 
    { 
        SparseMatd M(3,3);
        M.MakeDiag(-1);
        return M;
    }

    SparseMatd dv(const Vecd &x, const Vecd &v) const 
    { return SparseMatd(3,3); }
};

class Func3 : public OdeFunc1
{
public:
    Vecd operator()(const Vecd &x) const
    {
        Vecd ret(2);
        ret[0] = x[1];
        ret[1] = -x[0];
        return ret;
    }

    SparseMatd dx(const Vecd &x) const
    {
        SparseMatd ret(2,2);
        ret[0].SetElts(1,1,VL_SV_END);
        ret[1].SetElts(0,-1,VL_SV_END);
        return ret;
    }
};

int test3()
{
    double h=0.05;
  	Vecd x(3,vl_zero), v(3,vl_zero);
    x[1] = 1;
    //x[4] = 1;
	Vecd m(1,vl_one);

    Func2 f;
    OdeImpEul2 solver;

    for (int i=0; i<1000000; i++)
    {
      //  if (i % 1000 == 0) cout << i << '\t' << x << endl;
        Vecd xout, vout;
        solver.step(xout,vout, f, x,v,h);
        x=xout;
        v=vout;
    }

    return 0;


}

int test4()
{
    double h=0.1;
  	Vecd x(2,vl_zero);
    x[0] = 100000000;

    Func3 f;
    OdeImpEul1 solver;

    for (int i=0; i<1000; i++)
    {
        //if (i==29) { i=29; }
            
         /*if (i % 1000 == 0)*/// cout << i << '\t' << x << endl;
        Vecd xout;
        solver.step(xout,f,x,h);
        x = xout;
    }
	
    return 0;
}

int test5()
{
	RcPointer<Force> fsp = new ForceSpr(0,1,0.1,1);
/*	vector<RcPointer<Force> > forces;
	forces.push_back(fsp);
	vector<Vec3d> pos,vel;
	pos.push_back(Vec3d(0,0,0));
	pos.push_back(Vec3d(1,1,1));
	vel.push_back(Vec3d(0,0,0));
	vel.push_back(Vec3d(0,0,0));

	vector<double> mass;
	mass.push_back(1);
	mass.push_back(1);

	ParticleSys ps(pos,vel,mass,forces, new OdeImpEul2);


//	(*fsp).evalDv(x,v,m);*/

	Vecd x(6), v(6), m(2);
	m[0] = m[1] = 1;
	x.MakeZero();
	v.MakeZero();

	(*fsp).evalDx(x,v,m);

	






	return 0;
}

extern int glut_main(int argc, char *argv[]);


int main(int argc, char *argv[])
{
/*
	int tmpDbgFlag;
	
   _CrtSetReportMode( _CRT_WARN, _CRTDBG_MODE_FILE );
   _CrtSetReportFile( _CRT_WARN, _CRTDBG_FILE_STDOUT );
   _CrtSetReportMode( _CRT_ERROR, _CRTDBG_MODE_FILE );
   _CrtSetReportFile( _CRT_ERROR, _CRTDBG_FILE_STDOUT );
   _CrtSetReportMode( _CRT_ASSERT, _CRTDBG_MODE_FILE );
   _CrtSetReportFile( _CRT_ASSERT, _CRTDBG_FILE_STDOUT );

*/
	/*
	* Set the debug-heap flag to keep freed blocks in the
	* heap's linked list - This will allow us to catch any
	* inadvertent use of freed memory
	*/
/*	tmpDbgFlag = _CrtSetDbgFlag(_CRTDBG_REPORT_FLAG);
	tmpDbgFlag |= _CRTDBG_DELAY_FREE_MEM_DF;
	tmpDbgFlag |= _CRTDBG_LEAK_CHECK_DF;
	_CrtSetDbgFlag(tmpDbgFlag);



	_CrtMemState s1,s2,s3;
	_CrtMemCheckpoint(&s1);
*/	
    return glut_main(argc, argv);
	test5();

/*	_CrtMemCheckpoint(&s2);
	_CrtMemDifference(&s3,&s1,&s2);
	_CrtMemDumpStatistics(&s3);
*/
	return 0;



  //  return test3();
    return test2();
    return 0;
}
