// written in the D programming language

module samples.MagnetsElectric;

import dchip.all;

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.0*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*)cpcalloc(1,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*)cpcalloc(1,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*)cpcalloc(1,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);

    // 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()
{
    ChipmunkDemoFreeSpaceChildren(space);
    cpSpaceFree(space);
}

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