#ifndef ATOM_CSPRING_H
#define ATOM_CSPRING_H

#include "Globals.h"

class cSpring
{
	public:
		float   rest_length,
				stiffness;

		cPoint *p1, *p2;

		void update()
		{
			float	angle = atan2(p2->pos.y() - p1->pos.y(), p2->pos.x() - p1->pos.x()),

					l1 = dist( p1->pos, p2->pos ),
					l2 = dist( p1->pos + p1->vel, p2->pos + p2->vel ),

					dampen = ( ( l2-l1 )*(l1 - rest_length) < 0 )  ?  0.3 : 1.0;

			p2->force.x( p2->force.x() + -(stiffness * ( (p2->pos.x() - p1->pos.x()) - cos(angle)*rest_length )) * dampen );
			p2->force.y( p2->force.y() + -(stiffness * ( (p2->pos.y() - p1->pos.y()) - sin(angle)*rest_length )) * dampen );

			p1->force.x( p1->force.x() + (stiffness * ( (p2->pos.x() - p1->pos.x()) - cos(angle)*rest_length )) * dampen );
			p1->force.y( p1->force.y() + (stiffness * ( (p2->pos.y() - p1->pos.y()) - sin(angle)*rest_length )) * dampen );
		}

		void draw(BITMAP *bmp)
		{fastline(bmp, int(p1->pos.x()),int(p1->pos.y()), int(p2->pos.x()),int(p2->pos.y()), makecol(255,255,255));}

		void handle_col(cPoint *pnt)
		{
			vector norm = vector(-(p2->pos.y() - p1->pos.y()), p2->pos.x() - p1->pos.x() );

			float norm_length = sqrt(norm.x() * norm.x() + norm.y() * norm.y());

			norm.x( norm.x() / norm_length );
			norm.y( norm.y() / norm_length );

			vector	norm_vel = norm * dot(norm, pnt->vel),
					tan_vel = (pnt->vel - norm_vel);

			pnt->pos += (vector(-norm.y(), norm.x()) * 0.5);
			pnt->vel = tan_vel - norm_vel;
		}

		void check_col(cSpring *spring)
		{
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
			float	x1 = p1->pos.x(),
					y1 = p1->pos.y(),

					x2 = p2->pos.x(),
					y2 = p2->pos.y(),

					x3 = spring->p1->pos.x(),
					y3 = spring->p1->pos.y(),

					x4 = spring->p1->pos.x() + spring->p1->vel.x(),
					y4 = spring->p1->pos.y() + spring->p1->vel.y();

			float	ua = ( (x4-x3)*(y1-y3) - (y4-y3)*(x1-x3) ) /
						 ( (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1) ),

					ub = ( (x2-x1)*(y1-y3) - (y2-y1)*(x1-x3) ) /
						 ( (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1) );

			if( spring->p1 != p1 && spring->p1 != p2 )
				if((ua >= 0 && ua <= 1) && (ub >= 0 && ub <= 1))
				{	spring->p1->pos.x( x1 + ua*(x2 - x1) );
					spring->p1->pos.y( y1 + ua*(y2 - y1) );

						{handle_col(spring->p1);}
				}


           	x3 = spring->p2->pos.x();
			y3 = spring->p2->pos.y();

			x4 = spring->p2->pos.x() + spring->p2->vel.x();
			y4 = spring->p2->pos.y() + spring->p2->vel.y();

			ua = ( (x4-x3)*(y1-y3) - (y4-y3)*(x1-x3) ) /
				 ( (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1) );

			ub = ( (x2-x1)*(y1-y3) - (y2-y1)*(x1-x3) ) /
				 ( (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1) );

			if( spring->p2 != p1 && spring->p2 != p2 )
				if((ua >= 0 && ua <= 1) && (ub >= 0 && ub <= 1))
				{	spring->p2->pos.x( x1 + ua*(x2 - x1) );
					spring->p2->pos.y( y1 + ua*(y2 - y1) );

						{handle_col(spring->p2);}
				}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

			float	a = spring->p1->pos.x() - p1->pos.x(),
					b = spring->p1->pos.y() - p1->pos.y(),
					c = p2->pos.x() - p1->pos.x(),
					d = p2->pos.y() - p1->pos.y();

			// Point 1's direction from the line
			float dir1a = (a*d)-(b*c);
				if(dir1a >= 0) dir1a = 1;
				else dir1a = -1;

			a = spring->p1->pos.x() + spring->p1->vel.x() - p1->pos.x();
			b = spring->p1->pos.y() + spring->p1->vel.y() - p1->pos.y();

			// And with velocity
			float dir1b = (a*b)-(b*c);
				if(dir1b >= 0) dir1b = 1;
				else dir1b = -1;

			a = spring->p2->pos.x() - p1->pos.x();
			b = spring->p2->pos.y() - p1->pos.y();

			// Point 2's direction
			float dir2a = (a*d)-(b*c);
				if(dir2a >= 0) dir2a = 1;
				else dir2a = -1;

			a = spring->p2->pos.x() + spring->p2->vel.x() - p1->pos.x();
			b = spring->p2->pos.y() + spring->p2->vel.y() - p1->pos.y();

			// And after velocity
			float dir2b = (a*d)-(b*c);
				if(dir2b >= 0) dir2b = 1;
				else dir2b = -1;

			// If point 1's direction is different from point 2's (after veloctiy)
			// And if this point isn't attached to the spring that's checking for collision
			// Move point 2 to the point of collision between the line segment (spring)
			// And the line between points 1 and 2.
			if( dir2a != dir2b && spring->p2 != p1 && spring->p2 != p2)
				if(spring->p2->checked == false && spring->p2->mass > 0)
					{spring->p2->checked = true;

						x3 = spring->p2->pos.x();
						y3 = spring->p2->pos.y();

						x4 = spring->p2->pos.x() + spring->p2->vel.x();
						y4 = spring->p2->pos.y() + spring->p2->vel.y();

						ua = ( (x4-x3)*(y1-y3) - (y4-y3)*(x1-x3) ) /
							 ( (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1) );

						ub = ( (x2-x1)*(y1-y3) - (y2-y1)*(x1-x3) ) /
							 ( (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1) );

                        if((ua > 0 && ua < 1) && (ub > 0 && ub < 1))
							spring->p2->pos.x( x1 + ua*(x2 - x1) );
							spring->p2->pos.y( y1 + ua*(y2 - y1) );

					}
			// An effort to keep the system from repeatedly updating particles
			// attached to more than one spring.
			spring->p2->checked = true;

			// If point 2's direction is different from point 1's (after veloctiy)
			// And if this point isn't attached to the spring that's checking for collision
			// Move point 2 to the point of collision between the line segment (spring)
			// and the line between points 1 and 2.
			if( dir1a != dir1b && spring->p1 != p1 && spring->p1 != p2)
				if(spring->p1->checked == false && spring->p1->mass > 0)
					{spring->p1->checked = true;

						x3 = spring->p1->pos.x();
						y3 = spring->p1->pos.y();

						x4 = spring->p1->pos.x() + spring->p1->vel.x();
						y4 = spring->p1->pos.y() + spring->p1->vel.y();

						ua = ( (x4-x3)*(y1-y3) - (y4-y3)*(x1-x3) ) /
							 ( (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1) );

						ub = ( (x2-x1)*(y1-y3) - (y2-y1)*(x1-x3) ) /
							 ( (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1) );

                        if((ua > 0 && ua < 1) && (ub > 0 && ub < 1))
							spring->p1->pos.x( x1 + ua*(x2 - x1) );
							spring->p1->pos.y( y1 + ua*(y2 - y1) );
					}
			spring->p1->checked = true;
		}


		cSpring(cPoint *pA, cPoint *pB, float len, float stif)
			{rest_length = len; stiffness = stif;
				p1 = pA; p2 = pB;}

		~cSpring(){};
};

std::vector <cSpring*> vSpring;

#endif