//Jacek Matulewski, e-mail: jacek@fizyka.umk.pl

#ifndef UKLADYPUNKTOWMATERIALNYCH2_H
#define UKLADYPUNKTOWMATERIALNYCH2_H

#include "PunktMaterialny.h"

class Pudlo : public ObszarZabroniony
{
	private:
		double minX,maxX,minY,maxY,minZ,maxZ;

	public:
		Wektor PobierzSrodek()
		{
			return Wektor((minX+maxX)/2.0,(minY+maxY)/2.0,(minZ+maxZ)/2.0);
		}

		Wektor PobierzRozmiar()
		{
			return Wektor(maxX-minX,maxY-minY,maxZ-minZ);
		}

		bool CzyWObszarzeZabronionym(Wektor polozenie,Wektor poprzedniePolozenie,double margines,Wektor* normalna)
		{			
			double _minX=minX+margines,_maxX=maxX-margines;
			double _minY=minY+margines,_maxY=maxY-margines;
			double _minZ=minZ+margines,_maxZ=maxZ-margines;
			bool wynik=polozenie.X<_minX || polozenie.X>_maxX || polozenie.Y<_minY || polozenie.Y>_maxY || polozenie.Z<_minZ || polozenie.Z>_maxZ;
			if(wynik && normalna!=NULL)
			{	
				//Wektor przemieszczenie=polozenie-poprzedniePolozenie;
				if(polozenie.X<_minX) normalna->X=1;
				if(polozenie.X>_maxX) normalna->X=-1;
				if(polozenie.Y<_minY) normalna->Y=1;
				if(polozenie.Y>_maxY) normalna->Y=-1;
				if(polozenie.Z<_minZ) normalna->Z=1;
				if(polozenie.Z>_maxZ) normalna->Z=-1;
			}
			return wynik;
		}

		Pudlo(double wspolczynnikOdbicia,double wspolczynnikTarcia,double minX,double maxX,double minY,double maxY,double minZ,double maxZ)
			:ObszarZabroniony(wspolczynnikOdbicia,wspolczynnikTarcia),
			 minX(minX),maxX(maxX),minY(minY),maxY(maxY),minZ(minZ),maxZ(maxZ)
		{	
		}
};

class ZbiorPunktowMaterialnychZeZderzeniami : public ZbiorPunktowMaterialnychZObszaremZabronionym
{
	private:
		double e;

	public:
		ZbiorPunktowMaterialnychZeZderzeniami(int ilosc,double wspolczynnikRestytucji)
			:ZbiorPunktowMaterialnychZObszaremZabronionym(ilosc),
			 e(wspolczynnikRestytucji)
		{			
		}

	private:
		void KolizjeCentralne(double krokCzasowy)
		{
			for(int i=0;i<ilosc;i++)
			{
				PunktMaterialny* punktI=PobierzPunktMaterialny(i);
				double mI=punktI->Masa();
				for(int j=i+1;j<ilosc;j++)
				{										
					PunktMaterialny* punktJ=PobierzPunktMaterialny(j);

					double nastepnaOdleglosc=(punktI->NastepnePolozenie()-punktJ->NastepnePolozenie()).Dlugosc();
					if(nastepnaOdleglosc<=(punktI->Promien()+punktJ->Promien()))
					{						
						double mJ=punktJ->Masa();
						double odwSumyMas=1/(mI+mJ);
			
						Wektor uI=((e+1)*mJ*punktJ->Predkosc()+punktI->Predkosc()*(mI-e*mJ))*odwSumyMas;
						Wektor uJ=((e+1)*mI*punktI->Predkosc()+punktJ->Predkosc()*(mJ-e*mI))*odwSumyMas;
						punktI->UstawPredkosc(uI);
						punktJ->UstawPredkosc(uJ);						
						punktI->PrzygotujRuch(Sila(i),krokCzasowy,algorytmEulera);
						punktJ->PrzygotujRuch(Sila(j),krokCzasowy,algorytmEulera);
					}
				}
			}
		}

		void Kolizje(double krokCzasowy)
		{
			for(int i=0;i<ilosc;i++)
			{
				PunktMaterialny* punktI=PobierzPunktMaterialny(i);
				double mI=punktI->Masa();
				for(int j=i+1;j<ilosc;j++)
				{										
					PunktMaterialny* punktJ=PobierzPunktMaterialny(j);

					double nastepnaOdleglosc=(punktI->NastepnePolozenie()-punktJ->NastepnePolozenie()).Dlugosc();
					if(nastepnaOdleglosc<=(punktI->Promien()+punktJ->Promien()))
					{						
						Wektor n=punktI->Polozenie()-punktJ->Polozenie();
						n.Normuj();

						double mJ=punktJ->Masa();
						double masaZred=1/(1/mI+1/mJ);

						double Dvn=(punktI->Predkosc()-punktJ->Predkosc())*n;

						double J=-masaZred*(e+1)*Dvn;
			
						Wektor uI=punktI->Predkosc()+n*J/mI;
						Wektor uJ=punktJ->Predkosc()-n*J/mJ;
						punktI->UstawPredkosc(uI);
						punktJ->UstawPredkosc(uJ);						
						punktI->PrzygotujRuch(Sila(i),krokCzasowy,algorytmEulera);
						punktJ->PrzygotujRuch(Sila(j),krokCzasowy,algorytmEulera);
					}
				}
			}
		}

	protected:
		void PoPrzygotowaniuRuchu(double krokCzasowy)
		{
			//KolizjeCentralne(krokCzasowy);
			Kolizje(krokCzasowy);
		}
};

#define _USE_MATH_DEFINES
#include <math.h>
#include <time.h>

class ZderzajaceSiePunkty  : public ZbiorPunktowMaterialnychZeZderzeniami
{
	public:
		ZderzajaceSiePunkty(int ilosc,double wspolczynnikRestytucji,Wektor polozenieMin,Wektor polozenieMax,double predkoscMin,double predkoscMax,double masaMin,double masaMax)
			:ZbiorPunktowMaterialnychZeZderzeniami(ilosc,wspolczynnikRestytucji)
		{
			obszarZabroniony=new Pudlo(1,0,1.1*polozenieMin.X,1.1*polozenieMax.X,1.1*polozenieMin.Y,1.1*polozenieMax.Y,-1,1);
			
			srand((unsigned)time(NULL));
			for(int i=0;i<ilosc;++i)
			{
				bool wynikTestuNakrywania=false;
				do
				{
					double polozenieX=(double)rand()/(RAND_MAX+1)*(polozenieMax.X-polozenieMin.X)+polozenieMin.X;
					double polozenieY=(double)rand()/(RAND_MAX+1)*(polozenieMax.Y-polozenieMin.Y)+polozenieMin.Y;
					double polozenieZ=(double)rand()/(RAND_MAX+1)*(polozenieMax.Z-polozenieMin.Z)+polozenieMin.Z;				
					PunktMaterialny* punkt=PobierzPunktMaterialny(i);				
					punkt->UstawPolozenie(Wektor(polozenieX,polozenieY,polozenieZ));
					double predkoscKatPhi=(double)rand()/(RAND_MAX+1)*2*M_PI;					
					double szybkosc=(double)rand()/(RAND_MAX+1)*(predkoscMax-predkoscMin)+predkoscMin;
					punkt->UstawPredkosc(szybkosc*Wektor(cos(predkoscKatPhi),sin(predkoscKatPhi),0));
					double masa=(double)rand()/(RAND_MAX+1)*(masaMax-masaMin)+masaMin;
					punkt->UstawMase(masa);				
					punkt->UstawKolor(i/(float)ilosc,1-i/(float)ilosc,1);

					//test nakladania sfer
					wynikTestuNakrywania=false;
					for(int j=0;j<i;++j)
					{
						PunktMaterialny* punktJ=PobierzPunktMaterialny(j);
						if((punkt->Polozenie()-punktJ->Polozenie()).Dlugosc()<=(punkt->Promien()+punktJ->Promien()))
							wynikTestuNakrywania=true;
					}
				}
				while(wynikTestuNakrywania);
			}						
		}

	protected:
		Wektor Sila(int indeks) const
		{
			return Wektor(0,0,0);
		}
};

class ProblemBilardzisty : public ZbiorPunktowMaterialnychZeZderzeniami
{
	public:
		ProblemBilardzisty(double promien,double parametrZderzenia)
			:ZbiorPunktowMaterialnychZeZderzeniami(2,1)
		{			
			PunktMaterialny* punkt1=PobierzPunktMaterialny(0);
			punkt1->UstawPolozenie(Wektor(-2,parametrZderzenia,0));
			punkt1->UstawPredkosc(Wektor(1,0,0));
			punkt1->UstawMase(1);
			punkt1->UstawPromien(promien);
			punkt1->UstawKolor(0,1,0);

			PunktMaterialny* punkt2=PobierzPunktMaterialny(1);
			punkt2->UstawPolozenie(Wektor(0,0,0));
			punkt2->UstawPredkosc(Wektor(0,0,0));
			punkt2->UstawMase(1);
			punkt2->UstawPromien(promien);
			punkt2->UstawKolor(1,0,0);
		}

	private:
		Wektor Sila(int indeks) const
		{
			return Wektor(0,0,0);
		}
};

class PunktyPoruszajaceSiePionowoIPoziomo : public ZbiorPunktowMaterialnychZeZderzeniami
{
	public:
		PunktyPoruszajaceSiePionowoIPoziomo(int ilosc1,int ilosc2,Wektor polozenieMin,Wektor polozenieMax,double predkoscMin,double predkoscMax,double masaMin,double masaMax)
			:ZbiorPunktowMaterialnychZeZderzeniami(ilosc1+ilosc2,1)
		{
			obszarZabroniony=new Pudlo(1,0,1.1*polozenieMin.X,1.1*polozenieMax.X,1.1*polozenieMin.Y,1.1*polozenieMax.Y,-1,1);

			srand((unsigned)time(NULL));
			for(int grupa=1;grupa<=2;grupa++)
			{				
				int offset=0;
				int ilosc=ilosc1;
				if (grupa==2) 
				{
					offset=ilosc1;
					ilosc=ilosc2;
				}
				for(int i=0;i<ilosc;++i)
				{
					bool wynikTestuNakrywania=false;
					do
					{
						double polozenieX=(double)rand()/(RAND_MAX+1)*(polozenieMax.X-polozenieMin.X)+polozenieMin.X;
						double polozenieY=(double)rand()/(RAND_MAX+1)*(polozenieMax.Y-polozenieMin.Y)+polozenieMin.Y;
						double polozenieZ=(double)rand()/(RAND_MAX+1)*(polozenieMax.Z-polozenieMin.Z)+polozenieMin.Z;						
						PunktMaterialny* punkt=PobierzPunktMaterialny(i+offset);
						punkt->UstawPolozenie(Wektor(polozenieX,polozenieY,polozenieZ));
						double predkoscKatPhi=(double)rand()/(RAND_MAX+1)*2*M_PI;						
						if (grupa==1) predkoscKatPhi=0;
						if (grupa==2) predkoscKatPhi=M_PI/2;
						double szybkosc=(double)rand()/(RAND_MAX+1)*(predkoscMax-predkoscMin)+predkoscMin;
						punkt->UstawPredkosc(szybkosc*Wektor(cos(predkoscKatPhi),sin(predkoscKatPhi),0));
						double masa=(double)rand()/(RAND_MAX+1)*(masaMax-masaMin)+masaMin;
						punkt->UstawMase(masa);				
						if(grupa==1) punkt->UstawKolor(1,1,0); else punkt->UstawKolor(0,1,0);

						//test nakladania sfer
						wynikTestuNakrywania=false;
						for(int j=0;j<i;++j)
						{
							PunktMaterialny* punktJ=PobierzPunktMaterialny(j);
							if((punkt->Polozenie()-punktJ->Polozenie()).Dlugosc()<=(punkt->Promien()+punktJ->Promien()))
								wynikTestuNakrywania=true;
						}
					}
					while(wynikTestuNakrywania);
				}						
			}
		}

	private:
		Wektor Sila(int indeks) const
		{
			return Wektor(0,0,0);
		}
};

class RuchyBrowna  : public ZderzajaceSiePunkty
{
	public:
		RuchyBrowna(int ilosc,int iloscKropliTluszczu,Wektor polozenieMin,Wektor polozenieMax,double predkoscMin,double predkoscMax,double masaMin,double masaMax)
			:ZderzajaceSiePunkty(ilosc+iloscKropliTluszczu,1,polozenieMin,polozenieMax,predkoscMin,predkoscMax,masaMin,masaMax)
		{	
			for(int i=0;i<ilosc;i++) PobierzPunktMaterialny(i)->UstawPromien(0.05);

			for(int i=0;i<iloscKropliTluszczu;i++)
			{
				//nie sprawdzam czy nie pokrywaja sie z innymi punktami i kroplami (zakladam, ze w rzeczywistosci moga)
				PunktMaterialny* ostatniPunkt=PobierzPunktMaterialny(ilosc+i);
				ostatniPunkt->UstawKolor(1,1,0,0.75);
				ostatniPunkt->UstawMase(10);			
				ostatniPunkt->UstawPromien(0.3);			
			}
		}
};


//------------------------------------------------------------------

class Woda : public ZbiorPunktowMaterialnych
{
	private:
		double polozenieYMin,polozenieYMax;
		double polozenieZMin,polozenieZMax;
		double predkoscXMin,predkoscXMax;
		Wektor g; //przyspieszenie ziemskie
		
	public:
		Woda(int ilosc,Wektor przyspieszenieZiemskie)
			:ZbiorPunktowMaterialnych(ilosc),
			 polozenieYMin(-0.75),polozenieYMax(0.75),
			 polozenieZMin(-0.75),polozenieZMax(0.75),
			 predkoscXMin(0.1),predkoscXMax(0.2),
			 g(przyspieszenieZiemskie)
		{						
			for(int i=0;i<ilosc;++i)
			{
				double polozenieY=(double)rand()/(RAND_MAX+1)*(polozenieYMax-polozenieYMin)+polozenieYMin;
				double polozenieZ=(double)rand()/(RAND_MAX+1)*(polozenieZMax-polozenieZMin)+polozenieZMin;
				double predkoscX=(double)rand()/(RAND_MAX+1)*(predkoscXMax-predkoscXMin)+predkoscXMin;
				PunktMaterialny* punkt=PobierzPunktMaterialny(i);
				punkt->UstawPolozenie(Wektor(-1,polozenieY,polozenieZ));
				punkt->UstawPredkosc(Wektor(predkoscX,0,0));
				punkt->UstawKolor(0,0.75,1,0.5);
			}			
		}

	public:
		//metoda przedeklarowana jako publiczna
		Wektor Sila(int indeks) const
		{
			return PobierzPunktMaterialny(indeks)->Masa()*g;
		}
};

class Material : public Siatka
{
	public:
		Material(int Nx,int Ny,double wspolczynnikSprezystosci,double wspolczynnikTlumienia,double wspolczynnikTlumieniaOscylacji,double wspolczynnikSztywnosci,Wektor przyspieszenieZiemskie,double dlugoscX,double dlugoscY)
			:Siatka(Nx,Ny,wspolczynnikSprezystosci,wspolczynnikTlumienia,wspolczynnikTlumieniaOscylacji,wspolczynnikSztywnosci,przyspieszenieZiemskie,dlugoscX,dlugoscY,NULL)
		{			
			const double cos45=cos(M_PI/4);			
			//w klasie Siatka zakres dostepnosci pol zostal zmieniony z prywatnego na chroniony 
			for(int ix=0;ix<Nx;++ix)
				for(int iy=0;iy<Ny;++iy)
				{
					int i=ix+Nx*iy;
					PunktMaterialny* punkt=PobierzPunktMaterialny(i);					
					punkt->UstawPolozenie(Wektor(cos45*(-dlugoscX/2+ix*lx),-dlugoscY/2+iy*ly,cos45*(-dlugoscX/2+ix*lx)));
					punkt->UstawKolor(1,0,1);					
				}			
			
			ZerujPredkoscSrednia();

			//linka
			for(int ix=0;ix<Nx;++ix)
				for(int iy=0;iy<Ny;++iy)
				{
					int i=ix+Nx*iy;
					if(iy==Ny-1) UstawWiezy(i,true);
				}
		}

	public:
		//przedeklarowana jako publiczna
		Wektor Sila(int indeks) const
		{
			return Siatka::Sila(indeks);
		}
};

class WodaUderzajacaWMaterial
{
	private:
		//ZbiorPunktowMaterialnych* woda;
		//Siatka* material;
		Woda* woda;
		Material* material;		

	public:
		Woda* PobierzWode() const
		{
			return woda;
		}

		Material* PobierzMaterial() const
		{
			return material;
		}

		WodaUderzajacaWMaterial(Woda* woda,Material* material)
		{
			this->woda=woda;
			this->material=material;
		}
	
		void KrokNaprzod(double krokCzasowy,Algorytm algorytm=algorytmVerleta) const
		{
			woda->PrzygotujRuch(krokCzasowy,algorytm);
			material->PrzygotujRuch(krokCzasowy,algorytm);

			PoPrzygotowaniuRuchu(krokCzasowy);
				
			material->WykonajRuch();	
			woda->WykonajRuch();
		}

		void Kolizje(double krokCzasowy) const
		{						
			int Nx=material->PobierzNx(), Ny=material->PobierzNy();
			
			//petla po komorkach siatki
			for(int jx=0;jx<Nx-1;++jx)
				for(int jy=0;jy<Ny-1;++jy)
				{
					//w kazdej iteracji dwa trojkaty (czworokat)
					int indeksyWezlow[5]={-1,jx+Nx*jy,(jx+1)+Nx*jy,jx+Nx*(jy+1),(jx+1)+Nx*(jy+1)};
					PunktMaterialny* wezel[5];
					for(int i=1;i<=4;++i) wezel[i]=material->PobierzPunktMaterialny(indeksyWezlow[i]);

					Wektor t1=wezel[1]->Polozenie(); //pozycjaGornyLewy
					Wektor t2=wezel[2]->Polozenie(); //pozycjaGornyPrawy
					Wektor t3=wezel[3]->Polozenie(); //pozycjaDolnyLewy
					Wektor t4=wezel[4]->Polozenie(); //pozycjaDolnyPrawy

					//box
					Wektor min,max;
					min.X=min(min(t1.X,t2.X),min(t3.X,t4.X));
					min.Y=min(min(t1.Y,t2.Y),min(t3.Y,t4.Y));
					min.Z=min(min(t1.Z,t2.Z),min(t3.Z,t4.Z));
					max.X=max(max(t1.X,t2.X),max(t3.X,t4.X));
					max.Y=max(max(t1.Y,t2.Y),max(t3.Y,t4.Y));
					max.Z=max(max(t1.Z,t2.Z),max(t3.Z,t4.Z));

					bool wezlyDoOdswiezenia=false;					
					Wektor nowePredkosciWezlow[5];						
					
					//trojkat A
					Wektor uA=t2-t1;
					Wektor vA=t3-t1;
					Wektor nA=uA^vA; //normalna nieunormowana
					double tnA=(t1+t2+t3)*nA/3.0;
					//trojkat B
					Wektor uB=t3-t4;
					Wektor vB=t2-t4;
					Wektor nB=uB^vB; //normalna nieunormowana
					double tnB=(t2+t3+t4)*nB/3.0;

					//iloczyny skalarne
					double uuA=uA*uA;
					double vvA=vA*vA;
					double uvA=uA*vA;
					double mianownikA=uvA*uvA-uuA*vvA;

					double uuB=uB*uB;
					double vvB=vB*vB;
					double uvB=uB*vB;
					double mianownikB=uvB*uvB-uuB*vvB;					
					
					//petla po kroplach
					for(int i=0;i<woda->LiczbaPunktow();++i)
					{
						PunktMaterialny* kropla=woda->PobierzPunktMaterialny(i);
						Wektor p1=kropla->Polozenie();
						Wektor p2=kropla->NastepnePolozenie();					

						//kropla zachacza o box
						if((p1.X<min.X && p2.X<min.X) || (p1.X>max.X && p2.X>max.X) ||
						   (p1.Y<min.Y && p2.Y<min.Y) || (p1.Y>max.Y && p2.Y>max.Y) ||
						   (p1.Z<min.Z && p2.Z<min.Z) || (p1.Z>max.Z && p2.Z>max.Z)) continue;

						double p1nA=p1*nA;
						double p2nA=p2*nA;

						bool uderzenie=false;
						Wektor normalna;

						//trojkat A						
						if( (p1nA-tnA)*(p2nA-tnA)<0 || (p2nA-tnA<1E-3) ) //po dwoch stronach plaszczyzny trojkata A lub p2 nalezy do plaszczyzny
						{
							double k=(tnA-p1nA)/(p2nA-p1nA);
							Wektor w=p1+k*(p2-p1)-t1; //w=p-t1
							//w*n powinno byc b. bliskie zera, bo w nalezy do plaszczyzny trojkata A

							double wu=w*uA;
							double wv=w*vA;

							//wspolrzedne barycentryczne
							double u2=(uvA*wv-vvA*wu)/mianownikA;
							double u3=(uvA*wu-uuA*wv)/mianownikA;
							double u1=1-u2-u3;

							if(u1>0 && u2>0 && u3>0) 
							{
								uderzenie=true;
								normalna=nA;
								//kropla->UstawKolor(1,1,0); //uderzenie w trokat t1-t2-t3
							}
						}

						if(!uderzenie)
						{
							double p1nB=p1*nB;
							double p2nB=p2*nB;

							//trojkat B
							if( (p1nB-tnB)*(p2nB-tnB)<0 || (p2nB-tnB<1E-3) ) //po dwoch stronach plaszczyzny trojkata B lub p2 nalezy do plaszczyzny
							{
								double k=(tnB-p1nB)/(p2nB-p1nB);
								Wektor w=p1+k*(p2-p1)-t4; //w=p-t4
								//w*n powinno byc b. bliskie zera, bo w nalezy do plaszczyzny trojkata B

								double wu=w*uB;
								double wv=w*vB;

								//wspolrzedne barycentryczne
								double u2=(uvB*wv-vvB*wu)/mianownikB;
								double u3=(uvB*wu-uuB*wv)/mianownikB;
								double u1=1-u2-u3;

								if(u1>0 && u2>0 && u3>0) 
								{
									uderzenie=true;
									normalna=nB;
									//kropla->UstawKolor(0,1,0); //uderzenie w trokat t4-t3-t2
								}
							}
						}

						//reakcja na uderzenie
						if(uderzenie)
						{							
							normalna.Normuj();

							wezlyDoOdswiezenia=true;

							const double e=0;

							double usrednionaMasaKomorki=0;
							Wektor usrednionaPredkoscKomorki;
							for(int j=1;j<=4;++j) 
							{
								usrednionaMasaKomorki+=wezel[j]->Masa();
								usrednionaPredkoscKomorki+=wezel[j]->Predkosc();
							}
							usrednionaMasaKomorki/=4.0;
							usrednionaPredkoscKomorki/=4.0;

							double masaKropli=kropla->Masa();							
							double masaZred=1/(1/masaKropli+1/usrednionaMasaKomorki);
							double Dvn=(kropla->Predkosc()-usrednionaPredkoscKomorki)*normalna;
							double J=-masaZred*(e+1)*Dvn;
							Wektor uKropli=kropla->Predkosc()+normalna*J/masaKropli;																																			
							for(int j=1;j<=4;++j) nowePredkosciWezlow[j]-=normalna*J/wezel[j]->Masa();

							kropla->UstawPredkosc(uKropli);
							kropla->PrzygotujRuch(woda->Sila(i),krokCzasowy,algorytmEulera);	
							
						} //czy uderzenie
					} //petla po kroplach

					if(wezlyDoOdswiezenia)
						for(int j=1;j<=4;++j) 
						{
							nowePredkosciWezlow[j]+=wezel[j]->Predkosc();
							wezel[j]->UstawPredkosc(nowePredkosciWezlow[j]);				
							wezel[j]->PrzygotujRuch(material->Sila(indeksyWezlow[j]),krokCzasowy,algorytmEulera);				
							wezel[j]->UstawKolor(0.5,0,0.5);
						}

				} //petla po komorkach siatki
		}

		void PoPrzygotowaniuRuchu(double krokCzasowy) const
		{
			//detekcja kolizji punktow z trojkatami
			Kolizje(krokCzasowy);
		}
};

#endif