#ifndef _INCLUDED_L3_OBJ_H
#define _INCLUDED_L3_OBJ_H

// *Material Point* Copyright (C) Krzysztof Bosak, 1999-11-05...1999-11-17
// *Galaxy* Copyright (C) Krzysztof Bosak, 1999-11-06...1999-11-19
// All rights reserved.
// kbosak@box43.pl
// http://www.kbosak.prv.pl

#include <stdlib.h>
#include "l3_array.h"
#include "l3_vec.h"
#include "l3_tool.h"

class MaterialPoint
{
private:
	Vector3D _r;
	Vector3D _v;
	double _mass;

public:
	inline MaterialPoint()
		: _mass(1)
	{
	}
	inline MaterialPoint(const Vector3D &r, const Vector3D &v, double mass)
		: _r(r),
		_v(v)
	{
		SetMass(mass);
	}
	inline void SetMass(double mass)
	{
		_mass=mass;
		assert(_mass>0);
		if(_mass<=0)
		{
			cerr<<"Mass of material point must be positive.\n";
			exit(1);
		}
	}
	inline const Vector3D& Position() const
	{// Position of object
		return _r;
	}
	inline const Vector3D& Velocity() const
	{// Velocity of object
		return _v;
	}
	inline Vector3D& Position()
	{// Position of object, changes allowed
		return _r;
	}
	inline Vector3D& Velocity()
	{// Velocity of object, changes allowed
		return _v;
	}
	inline double Mass() const
	{// Mass of object
		return _mass;
	}
	inline double Distance(const MaterialPoint &other) const
	{// Distance to other material point
		return (Position()-other.Position()).Length();
	}
	inline double DistancePow2(const MaterialPoint &other) const
	{// Distance^2 to other material point
		return (Position()-other.Position()).LengthPow2();
	}
};

class Galaxy
{
private:
	basearray<MaterialPoint> _star_array;
	double _gravity_constant;
	double _total_mass;
	double _deltat;
	int _adams_stage;

public:
	inline Galaxy(const basearray<MaterialPoint> star_array, double gravity_constant)
		: _star_array(star_array),
		_gravity_constant(gravity_constant),
		_deltat(-1),
		_adams_stage(0)
	{// Creates galaxy of numstars stars
		assert(_star_array.size()>=1);
		if(_star_array.size()<1)
		{
			cerr<<"At least 1 star is needed for single galaxy\n";
			exit(1);
		}
		assert(_gravity_constant>0);
		if(_gravity_constant<=0)
		{
			cerr<<"Gravity constant must be positive for gravitational interactions.\n";
			exit(1);
		}
		_total_mass=0;
		const int numstars=_star_array.size();
		const MaterialPoint *mp=&_star_array[0];
		for(int star=0; star<numstars; star++, mp++)
		{
			_total_mass+=mp->Mass();
		}
		Simplify();
	}
	inline MaterialPoint Star(int i) const
	{// Returns informations about selected star from galaxy
		return _star_array[i];
	}
	inline MaterialPoint& Star(int i)
	{// Allows modyfication of selected star from galaxy
		return _star_array[i];
	}
	inline double GravityConstant() const
	{// Returns gravity constant
		return _gravity_constant;
	}
	inline int NumberOfStars() const
	{// Returns number of stars in galaxy
		return _star_array.size();
	}
	inline double Distance(int i, int j) const
	{// Distance between two stars
		return _star_array[i].Distance(_star_array[j]);
	}
	inline double ForceLength(int i, int j) const
	{// Force of interaction between stars i and j
		assert(i!=j);
		const MaterialPoint &mpi=_star_array[i];
		const MaterialPoint &mpj=_star_array[j];
		return _gravity_constant * mpi.Mass() * mpj.Mass() / mpi.DistancePow2(mpj);
	}
	inline double PotentialEnergy() const
	{// Returns total potential energy of galaxy
		double energy=0;
		const int imax=_star_array.size();
		for(int i=0; i<imax; i++)
		{
			const MaterialPoint &mpi=_star_array[i];
			const double gravcon_mpimass=_gravity_constant * mpi.Mass();
			for(int j=i+1; j<imax; j++)
			{
				const MaterialPoint &mpj=_star_array[j];
				energy -= gravcon_mpimass * mpj.Mass() / mpi.Distance(mpj);
			}
		}
		return energy;
	}
	inline double KineticEnergy() const
	{// Returns total kinetic energy of galaxy
		double energy=0;
		const int numstars=_star_array.size();
		for(int star=0; star<numstars; star++)
		{
			const MaterialPoint &mp=_star_array[star];
			energy+=mp.Mass()*mp.Velocity().LengthPow2();
		}
		energy*=0.5;
		return energy;
	}
	inline double TotalEnergy() const
	{// Returns total energy of galaxy
		return KineticEnergy()+PotentialEnergy();
	}
	inline Vector3D Force(int i, int j) const
	{// Force of interaction between stars i and j
		assert(i!=j);
		const MaterialPoint &mpi=_star_array[i];
		const MaterialPoint &mpj=_star_array[j];
		const double dist=mpi.Distance(mpj);
		const double temp = _gravity_constant * mpi.Mass() * mpj.Mass() / (dist*dist*dist);
		return (mpj.Position()-mpi.Position())*temp;
	}
	inline const Vector3D TotalForce(int star) const // can be optimized
	{// Total force of interaction acting on star
		Vector3D total_force(0, 0, 0);
		const int numstars=_star_array.size();
		for(int another_star=0; another_star<numstars; another_star++)
		{
			if(star!=another_star)
			{
				total_force+=Force(star, another_star);
			}
		}
		return total_force;
	}
	inline const Vector3D Acceleration(int star) const
	{// Acceleration of given star
		const MaterialPoint &mpi=_star_array[star];
		Vector3D acceleration(0, 0, 0);
		const int numstars=_star_array.size();
		for(int another_star=0; another_star<numstars; another_star++)
		{
			if(star!=another_star)
			{
				const MaterialPoint &mpj=_star_array[another_star];
				const double dist=mpi.Distance(mpj);
				const double temp = _gravity_constant * mpj.Mass() / (dist*dist*dist);
				acceleration+=(mpj.Position()-mpi.Position())*temp;
			}
		}
		return acceleration;
	}
	inline const Vector3D CenterOfMass() const
	{// Position of center of mass
		Vector3D center(0, 0, 0);
		const int numstars=_star_array.size();
		for(int star=0; star<numstars; star++)
		{
			const MaterialPoint &mp=_star_array[star];
			center+=mp.Position()*mp.Mass();
		}
		return center/_total_mass;
	}
	inline Vector3D CenterOfMassVelocity() const
	{// Velocity of center of mass
		Vector3D center(0, 0, 0);
		const int numstars=_star_array.size();
		for(int star=0; star<numstars; star++)
		{
			const MaterialPoint &mp=_star_array[star];
			center+=mp.Velocity()*mp.Mass();
		}
		return center/_total_mass;
	}
	inline void Simplify()
	{// Modify position and velocity of center of mass to null vector, sorts by descending accelerations
		const Vector3D cenpos=CenterOfMass();
		const Vector3D cenvel=CenterOfMassVelocity();
		const int numstars=_star_array.size();
		for(int star=0; star<numstars; star++)
		{
			MaterialPoint &mp=_star_array[star];
			mp.Position()-=cenpos;
			mp.Velocity()-=cenvel;
		}
		/*
		for(int step=0; step<numstars; step++) // to be optimized: use heapsort?
		{
			for(int star=0; star<numstars-1; star++)
			{
				if(Acceleration(star).Length() < Acceleration(star+1).Length())
					PSwap(_star_array[star], _star_array[star+1]);
			}
		}*/
	}
	inline double Radius() const
	{// Calculates radius of galaxy, useful for graphics
		const Vector3D cenpos=CenterOfMass();
		double radius=0;
		const int numstars=_star_array.size();
		for(int star=0; star<numstars; star++)
		{
			const double temp=(_star_array[star].Position()-cenpos).Length();
			if(temp>radius)
			{
				radius=temp;
			}
		}
		return radius;
	}
	inline double AverageRadius() const
	{// Calculates average radius of galaxy, useful for graphics
		PAverage<double> average_radius;
		const Vector3D cenpos=CenterOfMass();
		const int numstars=_star_array.size();
		for(int star=0; star<numstars; star++)
		{
			average_radius.Add((_star_array[star].Position()-cenpos).Length());
		}
		return average_radius.Value();
	}
	inline double ZWidth() const
	{// Calculates width of galaxy by Z axis, useful for graphics
		const double cenpos=CenterOfMass().z;
		double zwidth=0;
		const int numstars=_star_array.size();
		for(int star=0; star<numstars; star++)
		{
			double temp=_star_array[star].Position().z-cenpos;
			if(temp<0)
			{
				temp=-temp;
			}
			if(temp>zwidth)
			{
				zwidth=temp;
			}
		}
		return zwidth;
	}
	inline double AverageZWidth() const
	{	// Calculates average width of galaxy by Z axis, useful for graphics
		const double cenpos=CenterOfMass().z;
		PAverage<double> average_zwidth;
		const int numstars=_star_array.size();
		for(int star=0; star<numstars; star++)
		{
			average_zwidth.Add((_star_array[star].Position().z-cenpos));
		}
		return average_zwidth.Value();
	}
	inline const basearray<Vector3D> ScaledPositions(int xmax, int ymax, double radius) const
	{	// Returns scaled positions of stars fitted to screen size and z-sorted
		// Radius parameter should be rather old and taken from average
		const int screen_size_half=(xmax<ymax ? xmax : ymax)<<1;
		const int numstars=_star_array.size();
		const double factor=screen_size_half/radius;
		const Vector3D center=CenterOfMass();
		basearray<Vector3D> pos(numstars);
		basearray<bool> tested(numstars);
		for(int s=0; s<numstars; s++)
		{
			tested[s]=false;
		}
		for(int star=0; star<numstars; star++)// to be optimized: use heapsort?
		{
			double maxz=-1E30;
			int maxindex=0;
			for(int lookstar=0; lookstar<numstars; lookstar++)
			{
				if(tested[lookstar]==false)
				{
					const double z=_star_array[lookstar].Position().z;
					if(z>maxz)
					{
						maxz=z;
						maxindex=lookstar;
					}
				}
			}
			pos[star]=(_star_array[maxindex].Position()-center)*factor;
			tested[maxindex]=true;
		}
		return pos;
	}
	inline void AdjustKineticEnergy(double factor)
	{// Multiplies Kinetic energy by factor, useful in artificial energy conservation, but it is very weak way of improwing solutions
		const double sqrt_factor=sqrt(factor);
		const int numstars=_star_array.size();
		for(int star=0; star<numstars; star++)
		{
			_star_array[star].Velocity()*=sqrt_factor;
		}
	}
	inline void EulerStep(double deltat)
	{// Evolution of galaxy using Euler method, O(1)
		assert(deltat>0);
		_deltat=deltat;
		static basearray<Vector3D> updated_pos(16);
		static basearray<Vector3D> updated_vel(16);
		const int numstars=_star_array.size();
		updated_pos.setsize(numstars);
		updated_vel.setsize(numstars);
		int star;
		MaterialPoint *mp;
		for(star=0, mp=&_star_array[0]; star<numstars; star++, mp++)
		{
			const Vector3D &vel=mp->Velocity();
			updated_pos[star]=mp->Position()+vel*_deltat;
			updated_vel[star]=vel+Acceleration(star)*_deltat;
		}
		for(star=0, mp=&_star_array[0]; star<numstars; star++, mp++)
		{
			mp->Position()=updated_pos[star];
			mp->Velocity()=updated_vel[star];
		}
	}
	inline void EulerModPCStep(double deltat)
	{// Evolution of galaxy using Euler method, O(1), semi-predictor-corrector scheme, (very stable - great surprise!)
		assert(deltat>0);
		_deltat=deltat;
		const int numstars=_star_array.size();
		MaterialPoint *mp=&_star_array[0];
		int star;
		for(star=0; star<numstars; star++)
		{
			Vector3D &vel=mp->Velocity();
			mp->Position()+=vel*_deltat;
			vel+=Acceleration(star)*_deltat;
			mp++;
		}
	}
	inline void EulerPCStep(double deltat)
	{// Evolution of galaxy using Euler method, O(1), predictor-corrector scheme
		assert(deltat>0);
		_deltat=deltat;
		const int numstars=_star_array.size();
		int star;
		MaterialPoint *mp;
		for(star=0, mp=&_star_array[0]; star<numstars; star++, mp++)
		{
			Vector3D &pos=mp->Position();
			pos+=mp->Velocity()*_deltat;
		}
		for(star=0, mp=&_star_array[0]; star<numstars; star++, mp++)
		{
			Vector3D &vel=mp->Velocity();
			vel+=Acceleration(star)*_deltat;
		}
	}
	inline void VerletStep(double updated_deltat)
	{// Evolution of galaxy using Verlet method, O(2)
		assert(updated_deltat>0);
		const int numstars=_star_array.size();
		static basearray<Vector3D> old_pos(16);
		old_pos.setsize(numstars);
		if(updated_deltat!=_deltat)
		{
			_deltat=updated_deltat;
			for(int star=0; star<numstars; star++)
			{
				const MaterialPoint &mp=_star_array[star];
				old_pos[star]=mp.Position();
			}
			EulerPCStep(_deltat);
			return;
		}
		static basearray<Vector3D> updated_pos(16);
		updated_pos.setsize(numstars);
		const double deltat2=_deltat*_deltat;
		const double deltat_x2=0.5/_deltat;
		int star;
		for(star=0; star<numstars; star++)
		{
			MaterialPoint &mp=_star_array[star];
			updated_pos[star]=Acceleration(star)*deltat2 + mp.Position()*2 - old_pos[star];
			mp.Velocity()=(updated_pos[star]-old_pos[star])*deltat_x2;
		}
		for(star=0; star<numstars; star++)
		{
			MaterialPoint &mp=_star_array[star];
			old_pos[star]=mp.Position();
			mp.Position()=updated_pos[star];
		}
	}
	inline void VerletModPCStep(double updated_deltat)
	{// Evolution of galaxy using Verlet method, O(2), semi-predictor-corrector scheme
		assert(updated_deltat>0);
		const int numstars=_star_array.size();
		static basearray<Vector3D> old_pos(16);
		old_pos.setsize(numstars);
		if(updated_deltat!=_deltat)
		{
			_deltat=updated_deltat;
			const MaterialPoint *mp=&_star_array[0];
			for(int star=0; star<numstars; star++, mp++)
			{
				old_pos[star]=mp->Position();
			}
			EulerPCStep(_deltat);
			return;
		}
		static basearray<Vector3D> unchanged_pos(16);
		unchanged_pos.setsize(numstars);
		int star;
		MaterialPoint *mp=&_star_array[0];
		for(star=0; star<numstars; star++, mp++)
		{
			unchanged_pos[star]=mp->Position();
		}
		const double deltat2=_deltat*_deltat;
		const double deltat_x2=0.5/_deltat;
		mp=&_star_array[0];
		for(star=0; star<numstars; star++, mp++)
		{
			Vector3D &pos=mp->Position();
			pos=Acceleration(star)*deltat2 + pos*2 - old_pos[star];
			mp->Velocity()=(pos-old_pos[star])*deltat_x2;
		}
		old_pos.swap(unchanged_pos);
	}
	inline void VerletPCStep(double updated_deltat)
	{// Evolution of galaxy using Verlet method, O(2), predictor-corrector scheme, best method so far
		assert(updated_deltat>0);
		const int numstars=_star_array.size();
		static basearray<Vector3D> old_pos(16);
		old_pos.setsize(numstars);
		if(updated_deltat!=_deltat)
		{
			_deltat=updated_deltat;
			const MaterialPoint *mp=&_star_array[0];
			for(int star=0; star<numstars; star++, mp++)
				old_pos[star]=mp->Position();
			EulerPCStep(_deltat);
			return;
		}
		static basearray<Vector3D> unchanged_pos(16);
		unchanged_pos.setsize(numstars);
		const double deltat2=_deltat*_deltat;
		int star;
		MaterialPoint *mp;
		for(star=0, mp=&_star_array[0]; star<numstars; star++, mp++)
		{
			Vector3D &pos=mp->Position();
			unchanged_pos[star]=pos;
			pos=Acceleration(star)*deltat2 + pos*2 - old_pos[star];
		}
		const double deltat_recip=1./_deltat;
		for(star=0, mp=&_star_array[0]; star<numstars; star++, mp++)
		{
			Vector3D &pos=mp->Position();
			pos=unchanged_pos[star];
			pos=Acceleration(star)*deltat2 + pos*2 - old_pos[star];
			//mp->Velocity()=(pos-old_pos[star])*deltat_x2; // old
			//mp->Velocity()=(unchanged_pos[star]-old_pos[star])*deltat_recip; // worse than old
			mp->Velocity()=(pos-unchanged_pos[star])*deltat_recip; // best
		}
		old_pos.swap(unchanged_pos);
	}
	inline void Adams2Step(double updated_deltat)
	{
		assert(updated_deltat>0);
		_deltat=updated_deltat;
		const double deltat_a=_deltat*1.5;
		const double deltat_b=-_deltat*2;
		const int numstars=_star_array.size();

		MaterialPoint *mp;
		int star;

		static basearray<Vector3D> a0(16);
		static basearray<Vector3D> a1(16);
		static basearray<Vector3D> v1(16);
		a0.setsize(numstars);
		a1.setsize(numstars);
		v1.setsize(numstars);

		if(_adams_stage<2)
		{
			if(_adams_stage==0)
			{
				for(star=0, mp=&_star_array[0]; star<numstars; star++, mp++)
				{
					v1[star]=mp->Velocity();
				}
			}
			else
			{
				for(star=0; star<numstars; star++)
				{
					a0[star]=Acceleration(star);
				}
			}
			EulerStep(_deltat);
			_adams_stage++;
			return;
		}

		a0.swap(a1);
		for(star=0; star<numstars; star++)
		{
			a0[star]=Acceleration(star);
		}
		for(star=0, mp=&_star_array[0]; star<numstars; star++, mp++)
		{
			mp->Position()+=deltat_a*mp->Velocity()+deltat_b*v1[star];
		}
		for(star=0, mp=&_star_array[0]; star<numstars; star++, mp++)
		{
			Vector3D &vel=mp->Velocity();
			v1[star]=vel;
			vel+=deltat_a*a0[star]+deltat_b*a1[star];
		}
	}
};

#endif //_INCLUDED_L3_OBJ_H