/*
	SuperCollider real time audio synthesis system
    Copyright (c) 2002 James McCartney. All rights reserved.
	http://www.audiosynth.com

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
*/

// some basic physical modeling ugens - julian rohrhuber 1/04
// these are very simple implementations with cartoonification aspects.


#include "SC_PlugIn.h"

static InterfaceTable *ft;

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


struct Spring : public Unit
{
	float m_pos;
	float m_vel;
};
/*
struct Friction : public Unit
{
	float m_pos;
	float m_vel;
	int m_state;
};
*/

struct Ball : public Unit
{
	float m_pos;
	float m_vel;
	float m_prev;
};

struct TBall : public Unit
{
	double m_pos;
	float m_vel;
	double m_prev;
};



extern "C"
{
	void load(InterfaceTable *inTable);


	void Spring_Ctor(Spring *unit);
	void Spring_next(Spring *unit, int inNumSamples);

	//void Friction_Ctor(Friction *unit);
	//void Friction_next(Friction *unit, int inNumSamples);

	void Ball_Ctor(Ball *unit);
	void Ball_next(Ball *unit, int inNumSamples);

	void TBall_Ctor(TBall *unit);
	void TBall_next(TBall *unit, int inNumSamples);
}

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


void Spring_Ctor(Spring *unit)
{
	SETCALC(Spring_next);
	unit->m_vel = 0.f;
	unit->m_pos = 0.f;
	Spring_next(unit, 1);

}

// in, spring, damping

void Spring_next(Spring *unit, int inNumSamples)
{
	float pos = unit->m_pos;
	float vel = unit->m_vel;
	float *out = ZOUT(0); 		// out force
	float *in = ZIN(0); 		// in force
	float spring = ZIN0(1);		// spring constant
	float damping = 1.f - ZIN0(2);// damping
	float c = SAMPLEDUR;
	float rc = SAMPLERATE;
	spring = spring * c;
	LOOP1(inNumSamples,
		float force = ZXP(in) * c - pos * spring;
		vel = (force + vel) * damping;
		pos += vel;
		ZXP(out) = force * rc;
	);

	unit->m_pos = pos;
	unit->m_vel = vel;
}


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

/*
void Friction_Ctor(Friction *unit)
{
	SETCALC(Friction_next);
	unit->m_vel = 0.f;
	unit->m_pos = 0.f;
	unit->m_state = 0;
	Friction_next(unit, 1);

}

// in, spring, damping

void Friction_next(Friction *unit, int inNumSamples)
{
	float pos = unit->m_pos;
	float vel = unit->m_vel;
	int state = unit->m_state;
...
void Index_next_a(Index *unit, int inNumSamples)
{
	// get table
	GET_TABLE
		float *table = bufData;
		int32 maxindex = tableSize - 1;

	float *out = ZOUT(0);
	float *in = ZIN(1);

	LOOP1(inNumSamples,
		int32 index = (int32)ZXP(in);
		index = sc_clip(index, 0, maxindex);
		ZXP(out) = table[index];
	);

}
...

	float *out = ZOUT(0); 			// out force
	float *in = ZIN(0); 			// in displacement
	float spring = ZIN0(1);			// spring constant
	float damping = 1.f - ZIN0(2);	// friction -> replace by buffer number for friction table
	float c = SAMPLEDUR;
	float rc = SAMPLERATE;
	spring *= c;
	LOOP1(inNumSamples,
		float inpos = ZXP(in) * c;
		float force = inpos + pos * spring;
		state = (vel > damping) ?  1 : 0; //  friction table here
		if(state) { // stuck
			vel = 0.f;
			pos += inpos;
		} else {   // loose
			vel += force;
			pos -= vel;
		}
		ZXP(out) = force * rc;
	);

	unit->m_pos = pos;
	unit->m_vel = vel;
	unit->m_state = state;

}

#define GET_TABLE \
		float fbufnum = ZIN0(0); \
		if (fbufnum != unit->m_fbufnum) { \
			uint32 bufnum = (uint32)fbufnum; \
			World *world = unit->mWorld; \
			if (bufnum >= world->mNumSndBufs) bufnum = 0; \
			unit->m_buf = world->mSndBufs + bufnum; \
		} \
		SndBuf *buf = unit->m_buf; \
        if(!buf) { \
			ClearUnitOutputs(unit, inNumSamples); \
			return; \
		} \
		float *bufData __attribute__((__unused__)) = buf->data; \
		if (!bufData) { \
			ClearUnitOutputs(unit, inNumSamples); \
			return; \
		} \
		int tableSize = buf->samples;

*/
//////////////////////////////////////////////////////////////////////////////////////////


void Ball_Ctor(Ball *unit)
{
	SETCALC(Ball_next);
	unit->m_vel = 0.f;
	unit->m_pos = ZIN0(0);
	unit->m_prev = ZIN0(0);
	Ball_next(unit, 1);
}


void Ball_next(Ball *unit, int inNumSamples)
{
	float *out = ZOUT(0);
	float *in = ZIN(0); 			// floor position
	float g_in = ZIN0(1); 			// gravity
	float damping = 1 - ZIN0(2);	// damping
	float k = ZIN0(3);				// friction

	float pos = unit->m_pos;
	float vel = unit->m_vel;
	float prev_floor = unit->m_prev;
	float c = SAMPLEDUR;
	float maxvel = c * 1000.f;
	float minvel = 0.f - maxvel;
	float inter = c * 1000.f;
	RGen& rgen = *unit->mParent->mRGen;
	float g = c * g_in;
#ifdef _MSC_VER
	k *= g_in; // stickyness proportional to gravity
#else //#ifdef _MSC_VER
	k = (double) k * (double) g_in; // stickyness proportional to gravity
#endif //#ifdef _MSC_VER

	LOOP1(inNumSamples,
		float floor = ZXP(in);
		float floorvel;
		float dither;
		vel -= g;
		pos += vel;
		float dist = pos - floor;
		floorvel = floor - prev_floor;
		floorvel = sc_clip(floorvel, minvel, maxvel);
		float vel_diff = floorvel - vel;
		if(sc_abs(dist) < k) { // sticky friction: maybe vel dependant?

			if(sc_abs(dist) < (k*0.005)) {
				vel = 0.f;
				pos = floor + g;
			} else {
				vel = vel_diff * inter + vel;
				pos = (floor - pos) * inter + pos;
			}

		} else if(dist <= 0.f) {

			pos = floor - dist;
			vel = vel_diff;
			vel *= damping;

			dither = rgen.frand() * 0.00005f * g_in; // dither to reduce jitter
			//if(sc_abs(dist) < 0.000001) { vel += dither; }
			vel += dither;
		}
		prev_floor = floor;
		ZXP(out) = pos;
	);

	unit->m_pos = pos;
	unit->m_vel = vel;
	unit->m_prev = prev_floor;
}


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

void TBall_Ctor(TBall *unit)
{
	SETCALC(TBall_next);
	unit->m_vel = 0.f;
	unit->m_pos = ZIN0(0);
	unit->m_prev = ZIN0(0);
	TBall_next(unit, 1);
}

void TBall_next(TBall *unit, int inNumSamples)
{
	float *out = ZOUT(0);
	float *in = ZIN(0); 		// floor position
	float g_in = ZIN0(1); 		// gravity
	float damping = 1 - ZIN0(2);// damping
	float k = ZIN0(3); 			// friction

	double pos = unit->m_pos;
	float vel = unit->m_vel;
	double prev_floor = unit->m_prev;
	float c = SAMPLEDUR;
	float maxvel = c * 1000.f;
	float minvel = 0.f - maxvel;
	float inter = c * 10000.f;
	RGen& rgen = *unit->mParent->mRGen;
	float g = c * g_in;
#ifdef _MSC_VER
	k *= g_in; // stickyness proportional to gravity
#else //#ifdef _MSC_VER
	k = (double) k * (double) g_in; // stickyness proportional to gravity
#endif //#ifdef _MSC_VER

	LOOP1(inNumSamples,
		double floor = ZXP(in);
		float floorvel;
		float outval = 0.f;
		float dither;
		vel -= g;
		pos += vel;
		double dist = pos - floor;
		floorvel = floor - prev_floor;
		floorvel = sc_clip(floorvel, minvel, maxvel);
		float vel_diff = floorvel - vel;
		if(sc_abs(dist) < k) { // sticky friction: vel dependant?
					if(sc_abs(dist) < (k*0.005)) {
					vel = 0.f;
					pos = floor + g;
				} else {
					vel = vel_diff * inter + vel;
					pos = (floor - pos) * inter + pos;
				}
		} else if(dist <= 0.f) {
					pos = floor - dist;
					vel = floorvel - vel;
					vel *= damping;
					outval = vel;
					dither = rgen.frand() * 0.001f * g_in; // dither to reduce sampling jitter
					//if(sc_abs(dist) < 0.003) { vel += dither; }
					vel += dither;

		}
		prev_floor = floor;
		ZXP(out) = outval;
	);

	unit->m_pos = pos;
	unit->m_vel = vel;
	unit->m_prev = prev_floor;
}




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

PluginLoad(PhysicalModeling)
{
	ft = inTable;

	DefineSimpleUnit(Spring);
//	DefineSimpleUnit(Friction);
	DefineSimpleUnit(Ball);
	DefineSimpleUnit(TBall);
}