// written in the D programming language

module samples.MagnetsElectric;

import chipmunkd.chipmunk;

import samples.ChipmunkDemo;

import std.math;
import core.stdc.string:strcmp;
import core.stdc.stdio:sprintf;

enum WIDTH = 600;
enum HEIGHT = 400;

enum SINGMAX = 10; // Maximum number of singularities per body
enum NMAG = 10; // Number of magnets
enum NCHG = 10; // Number of charged bodies
enum NMIX = 10; // Number of charged magnets

enum COU_MKS = 8.987551787e9; // Some physical constants
enum MAG_MKS = 1e-7;

// Prototypes
alias void function(DataforForce* data)SingForceFunc;

// Structures
// Singularities
struct ActorSingularity{
	// Number of singularities 
	int Nsing; 
	// Value of the singularities 
	cpFloat value[SINGMAX];
	// Type of the singularities 
	char type[SINGMAX][100]; 
	// Global position of the singularities
	cpVect Gpos[SINGMAX];
	// Local position of the singularities
	cpVect position[SINGMAX];
	// Angle of the singularities measured in the body axes
	cpFloat angle[SINGMAX];
	// Angle of the singularities measured from x
	cpFloat Gangle[SINGMAX];
	// Force function
	SingForceFunc force_func[SINGMAX];
	// Force function
	SingForceFunc torque_func[SINGMAX];
}
alias ActorSingularity Sing;

// Data for the force functions
struct DataforForce{
	//Everything in global coordinates
	// Position of the source
	cpVect p0;
	// Observed position
	cpVect p;
	// Relative position source-observed
	cpVect relp;
	// distance, disntace^2, ditance ^3
	cpFloat r[3];
	// angle of the source
	cpFloat ang0;
	// angle of the observed singularity
	cpFloat ang;
	// Foce value
	cpVect F;
	// Torque value
	cpFloat T;
}
alias DataforForce ForceData;

// Global Varibales
static cpSpace *space;


// **** Forces ****** // 
// Calculate the forces between two bodies. all this functions requieres 
// a pointer to an structure with the necessary fields.

// forces between charges
static void
CoulombForce(ForceData* data){
	data.F=cpvmult(cpvnormalize(data.relp),COU_MKS/data.r[1]);
}

// forces between magnets
static void
MagDipoleForce(ForceData* data){
	static cpFloat phi,alpha,beta,Fr,Fphi;
	
	// Angle of the relative position vector
	phi=cpvtoangle(data.relp);
	alpha=data.ang0;
	beta=data.ang;		

	alpha =phi - alpha;
	beta = phi - beta;
	
	
	// Components in polar coordinates
	Fr=(2.0e0*cos(alpha)*cos(beta) - sin(alpha)*sin(beta));
	Fphi=sin(alpha+beta);
//	printf("%g %g %g %g %g\n",phi,alpha,beta,Fphi);
	
	// Cartesian coordinates
	data.F=cpv(Fr*cos(phi)-Fphi*sin(phi),Fr*sin(phi)+Fphi*cos(phi));
	data.F=cpvmult(data.F,-3.e0*MAG_MKS/(data.r[1]*data.r[1]));
}

static void
MagDipoleTorque(ForceData* data){
	static cpFloat phi,alpha,beta;
	
	phi=cpvtoangle(data.relp);
	alpha=data.ang0;
	beta=data.ang;		
	alpha =phi - alpha;
	beta = phi - beta;

	// Torque. Though we could use a component of F to save some space, 
	// we use another variables for the sake of clarity.
	
	data.T=(MAG_MKS/data.r[2])*(3.0e0*cos(alpha)*sin(beta) + sin(alpha-beta));
}
// ******* // 

// This function fills the data structure for the force functions
// The structure Sing has the information about the singularity (charge or magnet)
static void
FillForceData(Sing* source,int inds, Sing* obs,int indo, ForceData* data)
{
	// Global Position and orientation of the source singularity
	 data.p0=source.Gpos[inds];
	 data.ang0=source.Gangle[inds]; 
	 
	// Global Position and orientation of the observed singularity
	 data.p=obs.Gpos[indo];
	 data.ang=obs.Gangle[indo];
	
	// Derived magnitudes
	 data.relp=cpvsub(data.p,data.p0); //Relative position
	 data.r[0]=cpvlength(data.relp); // Distance
	 data.r[1]=cpvlengthsq(data.relp); // Square Distance
	 data.r[2]=data.r[0]*data.r[1]; // Cubic distance
	 
     source.force_func[inds](data); // The value of the force
	 data.F= cpvmult(data.F,source.value[inds]*obs.value[indo]);
}

// Calculation of the interaction
static void
LRangeForceApply(cpBody *a, cpBody *b){
	
	Sing* aux = cast(Sing*)a.data;
	Sing* aux2 = cast(Sing*)b.data;
	cpVect delta;
	// General data needed to calculate interaction
	static ForceData fdata;
	fdata.F=cpvzero;
	
	// Calculate the forces between the charges of different bodies
	for (int i=0; i<aux.Nsing; i++)
	{
		for (int j=0; j<aux2.Nsing; j++)
		{
			if(!strcmp(aux.type[i].ptr,aux2.type[j].ptr))
			{
				//printf("%s %s\n",aux.type[i],aux2.type[j]);
				FillForceData (aux2,j,aux,i,&fdata);
				
				//Force applied to body A
				delta=cpvsub(aux.Gpos[i], a.p);
				cpBodyApplyForce(a,fdata.F, delta);
				
	 			if(aux.torque_func[i] != null)
				{
					//Torque on A
					aux.torque_func[i](&fdata);
					a.t += aux.value[i]*aux2.value[j]*fdata.T;
					
				}
			}
		}
	}
}

// function for the integration of the positions
// The following functions are variations to the starndrd integration in Chipmunk
// you can go ack to the standard ones by doing the appropiate changes.
static void
ChargedBodyUpdatePositionVerlet(cpBody *_body, cpFloat dt)
{
    // Long range interaction
    cpArray *bodies = space.bodies;
	static cpBody* B;
	Sing* aux=cast(Sing*)_body.data;
	Sing* aux2;

	// General data needed to calculate interaction
	static ForceData fdata;
	fdata.F=cpvzero;
	
	for(int i=0; i< bodies.num; i++)
	{
	  B=cast(cpBody*)bodies.arr[i];
	  aux2=cast(Sing*)B.data;
	  
	  if(B != _body)
	  {
        // Calculate the forces between the singularities of different bodies
        LRangeForceApply(_body, B);
	  }
	}
	
	cpVect dp = cpvmult(cpvadd(_body.v, _body.v_bias), dt);
	dp = cpvadd(dp,cpvmult(cpvmult(_body.f, _body.m_inv), 0.5e0*dt*dt));
	_body.p = cpvadd(_body.p, dp);

	cpBodySetAngle(_body, cast(float)(_body.a + (_body.w + _body.w_bias)*dt 
				   + 0.5*_body.t*_body.i_inv*dt*dt));

	// Update position of the singularities
	aux = cast(Sing*)_body.data;
	for (int i=0; i<aux.Nsing; i++)
	{
        aux.Gpos[i]=cpvadd(_body.p,cpvrotate(cpv(aux.position[i].x,
										  aux.position[i].y), _body.rot));
		aux.Gangle[i]= aux.angle[i] + _body.a;
	}
	
            
 	_body.v_bias = cpvzero;
	_body.w_bias = 0.0f;
}

// function for the integration of the velocities
static void
ChargedBodyUpdateVelocityVerlet(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt)
{
	_body.v = cpvadd(_body.v, cpvmult(cpvadd(gravity, cpvmult(_body.f, _body.m_inv)), 0.5e0*dt));
	_body.w = _body.w + _body.t*_body.i_inv*0.5e0*dt;
	
	_body.f = cpvzero;
	_body.t = 0.0e0;
	
	// Long range interaction
    cpArray *bodies = space.bodies;
	static cpBody* B;

	// General data needed to calculate interaction
	static ForceData fdata;
	fdata.F=cpvzero;
	
	for(int i=0; i< bodies.num; i++)
	{
	  B=cast(cpBody*)bodies.arr[i];
	  
	  if(B != _body)
	  {
        // Calculate the forces between the singularities of different bodies
        LRangeForceApply(_body, B);
	  }
	}
	_body.v = cpvadd(cpvmult(_body.v,damping), cpvmult(cpvadd(gravity, cpvmult(_body.f, _body.m_inv)), 0.5e0*dt));
	_body.w = _body.w*damping + _body.t*_body.i_inv*0.5e0*dt;
}

static void 
update(int ticks)
{
	enum int steps = 10;
	enum cpFloat dt = 1.0/60.0/cast(cpFloat)steps;
	
	cpArray *bodies = space.bodies;

	for(int i=0; i< bodies.num; i++)
		cpBodyResetForces(cast(cpBody*)bodies.arr[i]);
	
	for(int i=0; i<steps; i++){
		cpSpaceStep(space, dt);
	}
	
}

static void
make_mag(cpVect p, cpFloat ang, cpFloat mag)
{
	int nverts=6;
	cpVect verts[] = [
		cpv(-10,-10),
		cpv(-10, 10),
		cpv( 10, 10),
		cpv( 15, 5),
		cpv( 15, -5),
		cpv( 10,-10)
	];

	cpBody *_body = cpBodyNew(1.0, cpMomentForPoly(1.0f, nverts, verts.ptr, cpvzero));
	_body.p = p;
	_body.v = cpvzero;
	cpBodySetAngle(_body, ang);
	_body.w = 0.0e0;
	
    // Load the singularities
    Sing *magnet=cast(Sing*)cpmalloc(Sing.sizeof);
	magnet.Nsing=1; 
	magnet.value[0]=mag;
	sprintf(magnet.type[0].ptr,"magdipole"); 

	// The position and angle could be different form the one of the body
	magnet.position[0]=cpvzero;
	magnet.Gpos[0]=cpvadd(p,magnet.position[0]);
	magnet.angle[0]=0.0f;
	magnet.Gangle[0]=ang;
	
	magnet.force_func[0]=&MagDipoleForce;
	magnet.torque_func[0]=&MagDipoleTorque;

	_body.data=magnet;
	
    _body.position_func=&ChargedBodyUpdatePositionVerlet;
    _body.velocity_func=&ChargedBodyUpdateVelocityVerlet;
	cpSpaceAddBody(space, _body);
	
	cpShape *shape = cpPolyShapeNew(_body, nverts, verts.ptr, cpvzero);
	shape.e = 0.0; shape.u = 0.7;
	cpSpaceAddShape(space, shape);
}

static void
make_charged(cpVect p, cpFloat chg)
{
	int nverts=4;
	cpVect verts[] = [
		cpv(-10,-10),
		cpv(-10, 10),
		cpv( 10, 10),
		cpv( 10,-10)
	];

	cpBody *_body = cpBodyNew(1.0, cpMomentForPoly(1.0, nverts, verts.ptr, cpvzero));
	_body.p = p;
	_body.v = cpvzero;
	cpBodySetAngle(_body, 0.0f);
	_body.w = 0.0e0;
	
    // Load the singularities
    Sing *charge=cast(Sing*)cpmalloc(Sing.sizeof);;
	charge.Nsing=1; 
	charge.value[0]=chg;
	sprintf(charge.type[0].ptr,"electrical\0"); 

	// The position and angle could be different form the one of the body
	charge.position[0]=cpvzero;
	charge.Gpos[0]=cpvadd(p,charge.position[0]);
	charge.Gangle[0]=0;
	charge.angle[0]=0.0f;
	
	charge.force_func[0]=&CoulombForce;
	charge.torque_func[0]=null;
	
	_body.data=charge;
	
    _body.position_func=&ChargedBodyUpdatePositionVerlet;
    _body.velocity_func=&ChargedBodyUpdateVelocityVerlet;
	cpSpaceAddBody(space, _body);
	
	cpShape *shape = cpPolyShapeNew(_body, nverts, verts.ptr, cpvzero);
	shape.e = 0.0; shape.u = 0.7;
	cpSpaceAddShape(space, shape);
}
void 
make_mix(cpVect p, cpFloat ang, cpFloat mag,cpFloat chg)
{
	int nverts=5;
	cpVect verts[] = [
		cpv(-10,-10),
		cpv(-10, 10),
		cpv( 10, 10),
		cpv( 20, 0),		
		cpv( 10,-10)
	];

	cpBody *_body = cpBodyNew(1.0, cpMomentForPoly(1.0, nverts, verts.ptr, cpvzero));
	_body.p = p;
	_body.v = cpvzero;
	cpBodySetAngle(_body, ang);
	_body.w = 0.0e0;
	
    // Load the singularities
    Sing *mix=cast(Sing*)cpmalloc(Sing.sizeof);;
	mix.Nsing=2; 
	mix.value[0]=mag;
	mix.value[1]=chg;
	sprintf(mix.type[0].ptr,"magdipole\0");
	sprintf(mix.type[1].ptr,"electrical\0");  

	// The position and angle could be different form the one of the body
	mix.position[0]=cpvzero;
	mix.Gpos[0]=cpvadd(p,mix.position[0]);
	mix.position[1]=cpvzero;
	mix.Gpos[1]=cpvadd(p,mix.position[1]);
	mix.Gangle[0]=ang;
	mix.Gangle[1]=ang;	
	mix.angle[0]=0.0f;
	mix.angle[1]=0.0f;
	
	mix.force_func[0]=&MagDipoleForce;
	mix.force_func[1]=&CoulombForce;
	mix.torque_func[0]=&MagDipoleTorque;
	mix.torque_func[1]=null;	
	
	_body.data=mix;
	
    _body.position_func=&ChargedBodyUpdatePositionVerlet;
    _body.velocity_func=&ChargedBodyUpdateVelocityVerlet;
	cpSpaceAddBody(space, _body);
	
	cpShape *shape = cpPolyShapeNew(_body, nverts, verts.ptr, cpvzero);
	shape.e = 0.0; shape.u = 0.7;
	cpSpaceAddShape(space, shape);
}


static cpSpace* 
init()
{
	cpResetShapeIdCounter();
	space = cpSpaceNew();
	space.iterations = 5;
	space.gravity = cpvzero; //cpv(0,-100);
	
	cpSpaceResizeActiveHash(space, 30.0, 2999);

	// Screen border
/*	shape = cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(-320,240), 0.0f);
	shape.e = 1.0; shape.u = 1.0;
	cpSpaceAddShape(space, shape);

	shape = cpSegmentShapeNew(staticBody, cpv(320,-240), cpv(320,240), 0.0f);
	shape.e = 1.0; shape.u = 1.0;
	cpSpaceAddShape(space, shape);

	shape = cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 0.0f);
	shape.e = 1.0; shape.u = 1.0;
	cpSpaceAddShape(space, shape);

	// Reference line
	// Does not collide with other objects, we just want to draw it.
	shape = cpSegmentShapeNew(staticBody, cpv(-320,0), cpv(320,0), 0.0f);
	shape.collision_type = 1;
	cpSpaceAddShape(space, shape);
	// Add a collision pair function to filter collisions
	cpSpaceAddCollisionPairFunc(space, 0, 1, null, null);
*/		
	
	//srand(cast(uint) time(null));
    cpVect p;
	cpFloat ang;
	
	// Create magnets
	for(int i=0; i<NMAG; i++)
	{
	  p.x=(2.0e0*frand() - 1.0e0)*WIDTH/2.0f;
  	  p.y=(2.0e0*frand() - 1.0e0)*HEIGHT/2.0f;
  	  ang=(2.0e0*frand() - 1.0e0)*3.1415;
	  make_mag(p, ang,1.0e7);
	}
	
	// Create charged objects
	for(int i=0; i<NCHG; i++)
	{
	  p.x=(2.0e0*frand() - 1.0e0)*WIDTH/2.0f;
  	  p.y=(2.0e0*frand() - 1.0e0)*HEIGHT/2.0f;
  	  ang=(2.0e0*frand() - 1.0e0)*3.1415;
	  make_charged(p,1.0e-3*pow(-1.0,i%2));
	}
		
	// Create charged magnets objects
	for(int i=0; i<NMIX; i++)
	{
      p.x=(2.0e0*frand() - 1.0e0)*WIDTH/2.0f;
  	  p.y=(2.0e0*frand() - 1.0e0)*HEIGHT/2.0f;
  	  ang=(2.0e0*frand() - 1.0e0)*3.1415;
	  make_mix(p, ang,1.0e7*pow(-1.0,i%2), 1.0e-3*pow(-1.0,i%2));
	}
	
	return space;
}

static void
destroy()
{
	cpSpaceFreeChildren(space);
	cpSpaceFree(space);
}

chipmunkDemo MagnetsElectric = {
	"Magnets and Electric Charges (By: Juan Pablo Carbajal)",
	null,
	&init,
	&update,
	&destroy,
};