#include "cloth.h"

#include "forcespr.h"
#include "forcegrav.h"
#include "forcevisc.h"

#include "vl/VLd.h"
#include "GL/gl.h"
#include <vector>
using namespace std;


// convert cloth indices i,j into array indices
// All *invalid* indices return -1
static inline int IDX(int i, int j, int n)
{
    if (j<0 || j>=n || i<0 || i>=n) return -1;
    return j*n+i;
}

Cloth::Cloth(int n, 
             double structk, 
             double sheark, 
             double flexk,
             double grav,
             double visc,
             const RcPointer<OdeSolver2> &solver)
             : _n(n)
{
    int i,j;

    vector<Vec3d> pos(n*n);
    vector<Vec3d> vel(n*n, Vec3d(0,0,0)); // zero initial velocity
    vector<RcPointer<Force> > forces;
    vector<double> mass(n*n);

    for (j=0; j<n; j++)
    {
        for (i=0; i<n; i++)
        {
            // 12       13      14      15
            // 8        9       10      11
            // 4(0,1)   5       6       7
            // 0(0,0)   1(1,0)  2(2,0)  3(3,0)

            //   u
            //  lor
            //   d  --- o=ij d=ij-1
            int o = IDX(i,j,n);
            int u = IDX(i,j+1,n);
            //int d = IDX(i,j-1,n);
            //int l = IDX(i-1,j,n);
            int r = IDX(i+1,j,n);
            int ul = IDX(i-1,j+1,n);
            int ur = IDX(i+1,j+1,n);
            //int dl = IDX(i-1,j-1,n);
            //int dr = IDX(i+1,j-1,n);
            int uu = IDX(i,j+2,n);
            int rr = IDX(i+2,j,n);

            // Initialize positions of particles
            pos[o] = Vec3d((double)i/(n-1), 0, (double)j/(n-1));

            //cout << pos[o] << endl;
            //cout << "*** " << i << ' ' << j << endl;

            // Initialize structural springs
            double structlen = 1.0/(n-1);
            // right structural spring
            if (r != -1) {
                forces.push_back(new ForceSpr(o,r,structk,structlen));
                // cout << "added structural " << o << ' ' << r << endl;
            }
            // up structural spring
            if (u != -1) {
                forces.push_back(new ForceSpr(o,u,structk,structlen));
                // cout << "added structural " << o << ' ' << u << endl;
            }

            // Initialize shear springs
            double shearlen = sqrt((double)2)/(n-1);

            // upright shear
            if (ur != -1) {
                forces.push_back(new ForceSpr(o,ur,sheark,shearlen));
                // cout << "added shear " << o << ' ' << ur << endl;
            }

            // upleft shear
            if (ul != -1) {
                forces.push_back(new ForceSpr(o,ul,sheark,shearlen));
                // cout << "added shear " << o << ' ' << ul << endl;
            }

            // Initialize flex springs
            double flexlen = 2.0/(n-1);

            // upup flex
            if (uu != -1) {
                forces.push_back(new ForceSpr(o,uu,flexk,flexlen));
                // cout << "added flex " << o << ' ' << uu << endl;
            }

            // rightright flex
            if (rr != -1) {
                forces.push_back(new ForceSpr(o,rr,flexk,flexlen));
                // cout << "added flex " << o << ' ' << rr << endl;
            }
        }
    }

    forces.push_back(new ForceGrav(Vec3d(0,-grav,0)));
    forces.push_back(new ForceVisc(visc));

    fill(mass.begin(), mass.end(), 1.0/(n*n));

    _ps = new ParticleSys(pos, vel, mass, forces, solver);

}

void Cloth::fix(int i, int j)
{
    _ps->setMass(IDX(i,j,_n), HUGE_VAL);
	_ps->setVelocity(IDX(i,j,_n), Vec3d(0,0,0));
}

void Cloth::unfix(int i, int j)
{
    _ps->setMass(IDX(i,j,_n), 1.0/(_n*_n));
}

bool Cloth::isFixed(int i, int j) const
{
	return _ps->getMasses()[IDX(i,j,_n)] == HUGE_VAL;
}

void Cloth::setPos(int i, int j, const Vec3d &v)
{
    _ps->setPosition(IDX(i,j,_n), v);

}

Vec3d Cloth::getPos(int i, int j) const
{
    return _ps->getPositions()[IDX(i,j,_n)];

}
    
void Cloth::draw(bool shade) const
{
    int i,j,n;
    n=_n;
    vector<Vec3d> pos = _ps->getPositions();

	if (!shade)
	{

		glBegin(GL_LINES);
		for (j=0; j<n; j++)
		{
			for (i=0; i<n; i++)
			{
				// 12       13      14      15
				// 8        9       10      11
				// 4(0,1)   5       6       7
				// 0(0,0)   1(1,0)  2(2,0)  3(3,0)

				//   u
				//  lor
				//   d  --- o=ij d=ij-1
				int o = IDX(i,j,n);
				int u = IDX(i,j+1,n);
				int r = IDX(i+1,j,n);
          
				// right structural spring
				if (r != -1)
				{
					glVertex3dv(pos[o].Ref());
					glVertex3dv(pos[r].Ref());
				}

				// up structural spring
				if (u != -1) 
				{
					glVertex3dv(pos[o].Ref());
					glVertex3dv(pos[u].Ref());
				}
			}
		}
		glEnd();
	}
	else
	{
		vector<Vec3d> norms(pos.size(),Vec3d(0,0,0));

		for (j=0; j<n; j++)
		{
			for (i=0; i<n; i++)
			{
				// 12       13      14      15
				// 8        9       10      11
				// 4(0,1)   5       6       7
				// 0(0,0)   1(1,0)  2(2,0)  3(3,0)

				//   u
				//  lor
				//   d  --- o=ij d=ij-1
				int o = IDX(i,j,n);
				int u = IDX(i,j+1,n);
				int d = IDX(i,j-1,n);
				int l = IDX(i-1,j,n);
				int r = IDX(i+1,j,n);

				Vec3d uv = (u != -1) ? norm(pos[u]-pos[o]) : Vec3d(0,0,0);
				Vec3d lv = (l != -1) ? norm(pos[l]-pos[o]) : Vec3d(0,0,0);
				Vec3d rv = (r != -1) ? norm(pos[r]-pos[o]) : Vec3d(0,0,0);
				Vec3d dv = (d != -1) ? norm(pos[d]-pos[o]) : Vec3d(0,0,0);
			
				if (u>=0 && l>=0) norms[o] += norm(cross(uv,lv));
				if (l>=0 && d>=0) norms[o] += norm(cross(lv,dv));
				if (d>=0 && r>=0) norms[o] += norm(cross(dv,rv));
				if (r>=0 && u>=0) norms[o] += norm(cross(rv,uv));
				
				norms[o].Normalise();
			}
		}

		glBegin(GL_QUADS);
		for (j=0; j<n-1; j++)
		{
			for (i=0; i<n-1; i++)
			{
				int o = IDX(i,j,n);
				int u = IDX(i,j+1,n);
				int r = IDX(i+1,j,n);
				int ur = IDX(i+1,j+1,n);

				glNormal3dv(norms[o].Ref());
				glVertex3dv(pos[o].Ref());
				glNormal3dv(norms[r].Ref());
				glVertex3dv(pos[r].Ref());
				glNormal3dv(norms[ur].Ref());
				glVertex3dv(pos[ur].Ref());
				glNormal3dv(norms[u].Ref());
				glVertex3dv(pos[u].Ref());
			}
		}
		glEnd();
	}

}
    
void Cloth::step(double h, double max_allowed_displacement)
{
    _ps->step(h, max_allowed_displacement);
}

double Cloth::max_displacement()
{
  return (_ps->max_displacement());
}
