//physik.h
#pragma once

float abstand_zur_dreiecksflaeche(Vec3 q1, Vec3 p1, Vec3 p2, Vec3 p3);

uint32 numDinge=0;

class Ding {
 uint32 id=0;
 char name[64]="unnamed";
 Klon* klon;//Zeiger auf Daten von entsprechendem Klon von einem Modell
 Animobjekt* animobjekt; //oder Zeiger auf animiertes Objekt (dann klon auf NULL setzen)
 //TODO: Reibung ist eigentlich vom beruehrenden Objekt abhaengig
 float haftreibung=0.4f, gleitreibung=0.2f; //TODO: realistische Werte?
public:
 Box* thiskollbox; //TODO
 glm::vec3 daempfung=glm::vec3(1.0f);
 float masse=70.0f;
 glm::vec3 laufrichtung; //Einheitsvektor in Bewegungsrichtung
 glm::vec3 kraft=glm::vec3(0.0f); //summierte aktuelle Kraefte
 glm::vec3 veloc=glm::vec3(0.0f); //aktuelle Geschwindigkeit
 glm::vec3 accel=glm::vec3(0.0f); //aktuelle Beschleunigung
 glm::vec3 position=glm::vec3(0.0f); //aktuelle Position
 glm::vec3 userpos=glm::vec3(0.0f); //Position in User-Koordinaten
 Ding() {reset_motion(); id=numDinge++; klon=NULL; animobjekt=NULL;}
 Ding(const char *nam) {mystrncpy(name,nam,64); reset_motion(); id=numDinge++;}
 void reset_motion() {veloc=accel=glm::vec3(0.0f);} //Bewegung auf 0 setzen
 void setName(const char *nam) {mystrncpy(name,nam,64);}
 void setMasse(float m) {masse=m;}
 void setPointer(Klon* kl, Animobjekt *an) {klon=kl; animobjekt=an;}
 Animobjekt* getAnimobjekt() {return animobjekt;}
 void print();//test
 glm::vec3 kraefte_berechnen(float laufkraft);
 bool kollision(uint32 id2) {return false;}//TODO: auf Kollision mit Ding[id2] testen
 void neue_position(glm::vec3 posdiff);
};

std::vector<Ding> dinge;

// Spezialfaelle:
#define WELT dinge[0]
#define LINK dinge[1]
#define WELT_ID 0
#define LINK_ID 1

void Ding::print()//test
{
 printf("Ding: name=\"%s\" id=%d masse=%f a=%f v=%f\n",
	name, id, masse, absvec3(accel), absvec3(veloc));
}

bool zusammenstossend(Box *box1, Box *box2)
{
 float mindist=0.1; //TODO
 if(box1->minv.y > box2->maxv.y+mindist) return false;
 if(box2->minv.y > box1->maxv.y+mindist) return false;
 if(box1->minv.x > box2->maxv.x+mindist) return false;
 if(box2->minv.x > box1->maxv.x+mindist) return false;
 if(box1->minv.z > box2->maxv.z+mindist) return false;
 if(box2->minv.z > box1->maxv.z+mindist) return false;
 return true;
}

void printbox(Box *box)
{
 printf(" minv={%f %f %f} maxv={%f %f %f}\n",box->minv.x,box->minv.y,box->minv.z,
	box->maxv.x,box->maxv.y,box->maxv.z);
}

float nearest_triangle_ydistance(Objekte* obj, uint32 modelid, uint32 klonid, glm::vec3 position);

glm::vec3 Ding::kraefte_berechnen(float laufkraft)
{
 //printf("Ding::kraefte_berechnen()\n");//test
 
 //laufrichtung = glm::normalize(laufrichtung);
 //kraft = laufrichtung * laufkraft;
 kraft = glm::vec3(0.0f);//provisorisch

 //Stichworte: Gravitation, Grafitation, Graphitation, Schwerkraft
 float gewicht=masse*ERDBESCHLEUNIGUNG; //Gewicht in Newton
 kraft.y -= gewicht;
 daempfung = glm::vec3(1.0f); //keine Daempfung solange keine Zusammenstoesse
 thiskollbox = (klon!=NULL) ? &klon->kollBox : &animobjekt->kollBox;
 
#ifdef KRAFT_VERSION
 //Kraefte zu Weltobjekten:
 const float abstossk1=5.56f, abstossk2=1.0f, kraftradius=0.3f; //TODO: gute Werte finden
 for(uint32 modelid=0; modelid<weltobjekte.numModels; modelid++)
  for(uint32 klonid=0; klonid < weltobjekte.klonlists[modelid]->numKlones; klonid++)
   {
    Box* kollbox = &weltobjekte.klonlists[modelid]->klones[klonid].kollBox;
    if(zusammenstossend(kollbox, thiskollbox))
     {
      //TODO: fuer realistische Kraefteberchnung waere viel kleinerer Zeitschritt noetig
      glm::vec3 k(0.0f);
      float abstand = thiskollbox->minv.y - kollbox->maxv.y;
      //provisorisch: nur y-Richtung beruecksichtigen
      if(abstand >= -2.5f && abstand <= 2.5f) //TODO
       {
	printf("Weltobjekt: modelid=%d klonid=%d  Abstand=%f\n",modelid,klonid,abstand);//test
	//TODO: naechstgelegenes Dreieck suchen, Abstand zu diesem berechnen
	abstand = nearest_triangle_ydistance(&weltobjekte, modelid, klonid, position);
	printf("  naechstes Dreieck Abstand=%f\n",abstand);//test
	abstand += kraftradius;
	float minabstand=kraftradius/2;
	if(abstand < minabstand)
	 {
	  printf("Warnung: zu kleiner Abstand fuer Kraftberechnung\n");//test
	  abstand = minabstand;
	  daempfung.y = 0.5f;
	 }
	else
	 {
	  if(veloc.y > 0.1f) daempfung.y = 0.8f;//TODO
	  //else if(abstand > 0.2f) daempfung.y = 0.8f;//TODO
	  //else if(abstand > 0.1) daempfung.y = 0.7f;//TODO
	  //else if(abstand > 0.05) daempfung.y = 0.6f;//TODO
	  else daempfung.y = 0.7f;//TODO
	 }
	k.y = abstossk1 * hoch4(abstossk2/abstand);
	//printf("    kollbox: "); printbox(kollbox);//test
	//printf("thiskollbox: "); printbox(thiskollbox);//test
	printf("Abstosskraft: k.y = %f  Gewicht = %f\n", k.y, gewicht);//test
	kraft += k;
       }
     }
    //else printf("nicht zusammenstossend\n");//test
   } //Ende Kraefte zu Weltobjekten
 
 //Kraefte zu andern Objekten:
 for(uint32 i=1;i<numDinge;i++)
  if(i!=id && dinge[i].kollision(id))
   {
    glm::vec3 k(0.0f);
    //TODO: Abstossungskraft zu dinge[i] berechnen
    kraft += k;
   }
 /*
 //TODO: Reibungskraefte nur in x-z-Richtung
 if(veloc!=glm::vec3(0.0f))
  {
   glm::vec3 reibungskraft(0.0f);
   float abskraft = absvec3(kraft);
   float absveloc = absvec3(veloc);
   if(absveloc>0.01f) reibungskraft = gleitreibung*gewicht*veloc;
   else               reibungskraft = haftreibung*gewicht*veloc;
   if(absvec3(reibungskraft) < abskraft)
         {kraft -= reibungskraft;}
   else  {kraft = glm::vec3(0.0f);}
  }
 */
#endif
 return kraft;
}

float xzabstand(glm::vec3& v1, glm::vec3& v2)
{
 float x=v2.x-v1.x, z=v2.z-v1.z;
 return sqrtf(x*x+z*z);
}

void Ding::neue_position(glm::vec3 posdiff)
{
 //TODO: verlaeufig nur fuer LINK
 position += posdiff;
 thiskollbox = (klon!=NULL) ? &klon->kollBox : &animobjekt->kollBox;
 Kollform* thiskollform = (klon!=NULL) ? &klon->kollform : &animobjekt->kollform;
 if(thiskollform->form!=FORM_ZYLINDER)
  {printf("Fehler: thiskollform->form!=FORM_ZYLINDER\n"); return;}//test
 thiskollform->fusspunkt = position;
 thiskollform->kopfpunkt = position;
 thiskollform->kopfpunkt.y += thiskollbox->getHoehe();
 thiskollform->radius = thiskollbox->getBreite()*0.5f;
 //printf("thiskollform->radius = %f\n",thiskollform->radius);//test
 //auf Zusammenstoesse zwischen LINK und allen andern Objekten testen:
 for(uint32 modelid=0; modelid<weltobjekte.numModels; modelid++)
  for(uint32 klonid=0; klonid < weltobjekte.klonlists[modelid]->numKlones; klonid++)
   {
    Box* kollbox = &weltobjekte.klonlists[modelid]->klones[klonid].kollBox;
    if(zusammenstossend(kollbox, thiskollbox)) //falls eventuell ein Zusammenstoss
     {
      Kollform* kollform = &weltobjekte.klonlists[modelid]->klones[klonid].kollform;
      if(kollform->form==FORM_KUGEL)
       {
	//printf("Testpunkt: Zusammenstoss mit Kugelfoermigem Objekt\n");//test
	//TODO
       }
      else if(kollform->form==FORM_ZYLINDER)
       {
	if(kollform->fusspunkt.y < thiskollform->kopfpunkt.y && kollform->kopfpunkt.y > thiskollform->fusspunkt.y &&
	   xzabstand(kollform->fusspunkt, thiskollform->fusspunkt) < kollform->radius+thiskollform->radius)
	 {
	  //printf("Testpunkt: Zusammenstoss mit Zylinderfoermigem Objekt\n");//test
	  position -= posdiff;
	  Vec3 q,q3;
	  q=thiskollform->fusspunkt; q.y=0.0f;
	  q3=kollform->fusspunkt; q3.y=0.0f;
	  float beta = atan2f(q3.z-q.z, q3.x-q.x); //Vorzeichen ist Glueckssache
	  //Szene um beta drehen:
	  q = ROTATEY(q, beta);
	  q3 = ROTATEY(q3, beta);
	  position = ROTATEY(position, beta);
	  posdiff = ROTATEY(posdiff, beta);
	  if(posdiff.x>0) posdiff.x=0;
	  position += posdiff;
	  //Szene um beta zurueckdrehen:
	  position = ROTATEY(position, -beta);
	 }
       }
      else if(kollform->form==FORM_QUADER)
       {
	if(kollform->fusspunkt.y < thiskollform->kopfpunkt.y && kollform->kopfpunkt.y > thiskollform->fusspunkt.y)
	 {
	  //printf("Testpunkt: Zusammenstoss mit Quaderfoermigem Objekt\n");//test
	  if(kollform->beta!=0) printf("TODO: gedrehter Quader\n");//test
	  Vec3 posdiffneu=posdiff;
	  float abstand1 = position.x - kollform->fusspunkt.x;
	  float abstand2 = position.z - kollform->fusspunkt.z;
	  float ab=abstand1/abstand2; if(ab<0) ab = -ab;
	  float r=thiskollform->radius;
	  if(ab > (kollform->laenge/2+r)/(kollform->breite/2+r))
	   {
	    if(abstand1>0 && abstand1 < kollform->laenge/2+r)
	     {
	      if(posdiffneu.x<0) posdiffneu.x=0;
	     }
	    else if(abstand1<0 && -abstand1 < kollform->laenge/2+r)
	     {
	      if(posdiffneu.x>0) posdiffneu.x=0;
	     }
	   }
	  else
	   {
	    if(abstand2>0 && abstand2 < kollform->breite/2+r)
	     {
	      if(posdiffneu.z<0) posdiffneu.z=0;
	     }
	    else if(abstand2<0 && -abstand2 < kollform->breite/2+r)
	     {
	      if(posdiffneu.z>0) posdiffneu.z=0;
	     }
	   }
	  position -= posdiff;
	  position += posdiffneu;
	 }
       }
     }
   }
}

bool box_unterhalb(glm::vec3 position, float mindist, Box& box)
{
 if(box.minv.y > position.y) return false;
 if(box.minv.x > position.x+mindist) return false;
 if(box.minv.z > position.z+mindist) return false;
 if(box.maxv.x < position.x-mindist) return false;
 if(box.maxv.z < position.z-mindist) return false;
 return true;
}

float bodenhoehe_berechnen(glm::vec3 position)
{
 glm::vec3 position2 = position;
 position2.y += 2.0f; //nur Flaechen unterhalb dieser Position suchen
 glm::vec3 p0(0.0f), p1(0.0f), p2(0.0f), p3(0.0f);
 glm::vec4 p41, p42, p43;
 uint32 index;
 float minabst=1e6f;
 Objekte* obj = &weltobjekte;
 float mindist = LINK.thiskollbox->getBreite()/4;
 for(uint32 modelid=0; modelid<weltobjekte.numModels; modelid++)
  {
   Modeldata* mod = obj->modeldatas[modelid];
   //TODO: wenn Modell Hoehenformel enthaelt, dann diese verwenden statt nach Dreiecken suchen
   for(uint32 klonid=0; klonid < obj->klonlists[modelid]->numKlones; klonid++)
    if(box_unterhalb(position2, mindist, obj->klonlists[modelid]->klones[klonid].kollBox))
    {
     glm::mat4 matrix = obj->klonlists[modelid]->klones[klonid].modelMatrix;
     //naechstgelegenes Dreieck unterhalb position2 suchen:
     for(uint32 m=0; m < mod->numMeshes; m++)
      {
       Meshdata* mes = mod->meshdatas[m];
       for(uint32 i=0; i < mes->numIndices; i+=3)
	{
	 index = mes->indices[i];
	 if(mes->vertices[index].normal.y > 0.0f)
	  {
	   p41 = matrix * glm::vec4(mes->vertices[index].position, 1.0f);
	   index = mes->indices[i+1];
	   p42 = matrix * glm::vec4(mes->vertices[index].position, 1.0f);
	   index = mes->indices[i+2];
	   p43 = matrix * glm::vec4(mes->vertices[index].position, 1.0f);
	   float abstand = abstand_zur_dreiecksflaeche(position2, p41, p42, p43);
	   if(abstand<minabst) {minabst=abstand; p1=p41; p2=p42; p3=p43;}
	  }
	}
      }
    }
  }
 float bodenhoehe=0.0f;
 if(minabst==1e6f)
  {
   //printf("Error: keinen Boden gefunden\n");//test
   //bodenhoehe = -100.0; //TODO: Tiefster Punkt der Welt
   bodenhoehe = -1.75; //test
  }
 else
  {
   bodenhoehe = position2.y - minabst;
  }
 return bodenhoehe;
}

float abstand_zur_dreiecksflaeche(Vec3 q1, Vec3 p1, Vec3 p2, Vec3 p3)
{
 static int ner=0; //11; //test
 if(ner!=0) ner--;//test
 //Senkrechter Abstand von q1 zur Dreiecksflaeche berechnen
 const float nein=1e6f; //grosser Wert falls q1 nicht ueber der Flaeche liegt
 float abstand=nein;
 Vec3 h;
 if(p2.y < p1.y && p2.y <= p3.y)
  {h=p1; p1=p2; p2=p3; p3=h;} //umsortieren so dass p1.y am kleinsten ist
 else if(p3.y < p1.y && p3.y < p2.y)
  {h=p3; p3=p2; p2=p1; p1=h;} //umsortieren so dass p1.y am kleinsten ist
 if(ner!=0) printf("Umsortiert: p1.y am kleinsten\n");//test
 //verschieben so dass p1 auf 0 0 0 liegt:
 p2 -= p1;
 p3 -= p1;
 q1 -= p1;
 if(ner!=0)//test
  {
   p1 -= p1;
   printf("Verschoben: p1 sollte jetzt 0 0 0 sein\n");//test
   printf("  p1 = {%f %f %f}\n",p1.x,p1.y,p1.z);
   printf("  p2 = {%f %f %f}\n",p2.x,p2.y,p2.z);
   printf("  p3 = {%f %f %f}\n",p3.x,p3.y,p3.z);
   printf("  q1 = {%f %f %f}\n",q1.x,q1.y,q1.z);
  }
 float alfa = atan2f(p2.x, p2.z); //drehen so dass p2 auf z-Achse zu liegen kommt

 p2 = ROTATEY(p2, -alfa);
 p3 = ROTATEY(p3, -alfa);
 q1 = ROTATEY(q1, -alfa);
 if(ner!=0)
  {
   printf("Rotiert: p2.x sollte jetzt 0.0 sein\n");//test
   printf("  p2 = {%f %f %f}\n",p2.x,p2.y,p2.z);//test
   printf("  p3 = {%f %f %f}\n",p3.x,p3.y,p3.z);//test
   printf("  q1 = {%f %f %f}\n",q1.x,q1.y,q1.z);//test
  }
#ifdef _DEBUG
 if(p2.x>1e-4f || p2.x < -1e-4f)//test
  {
   printf("Fehler: p2 = {%f %f %f} sollte fuer x 0.0 ergeben\n",p2.x,p2.y,p2.z);
   p2 = ROTATEY(p2, 2.0f*alfa);
   printf(" in andere Richtung rotiert: p2 = {%f %f %f}\n",p2.x,p2.y,p2.z);
   return nein;//test
  }
#endif
 if(q1.x > p3.x || q1.x < 0.0f) return nein;
 if(q1.x == 0.0f && q1.z == 0.0f) {abstand=q1.y; return nein;}
 if(q1.x == 0.0f && q1.z == p2.z) {abstand=q1.y-p2.y; return nein;}
 if(q1.x == p3.x && q1.z == p3.z) {abstand=q1.y-p3.y; return nein;}
#ifdef _DEBUG
 if(p3.x <= 0.0f)
  {
   if(ner!=0)
    {
     printf("Error: falsches Dreieck? p3.x sollte > 0.0 sein\n");//test
     printf("  p1 = {%f %f %f}\n",p1.x,p1.y,p1.z);//test
     printf("  p2 = {%f %f %f}\n",p2.x,p2.y,p2.z);//test
     printf("  p3 = {%f %f %f}\n",p3.x,p3.y,p3.z);//test
    }
   return nein;
  }
#else
 if(p3.x <= 0.0f) return nein;
#endif
 float A = p2.y/p2.z; //Achsenabschnitt auf z-Achse
 float B = (p3.y-A*p3.z)/p3.x; //Achsenabschnitt auf x-Achse
 if(ner!=0) printf("A=%f B=%f\n",A,B);//test
 if(p3.z > 0.0f && p3.z < p2.z) //1.Fall
  {
   if(ner!=0) printf("Testpunkt 2: 1.Fall\n");//test
   if(q1.z > p2.z || q1.z < 0.0f) return nein;
   if(!(q1.x/q1.z < p3.x/p3.z && q1.x/(p2.z-q1.z) < p3.x/(p2.z-p3.z))) return nein;
  }
 else if(p3.z <= 0.0f) //2.Fall
  {
   if(ner!=0) printf("Testpunkt 2: 2.Fall\n");//test
   if(q1.z < p3.z || q1.z > p2.z) return nein;
   if(!(-q1.z/q1.x < -p3.z/p3.x && q1.x/(p2.z-q1.z) < p3.x/(p2.z-p3.z))) return nein;
  }
 else //if(p3.z >= p2.z) //3.Fall
  {
   if(ner!=0) printf("Testpunkt 2: 3.Fall\n");//test
   if(q1.z > p3.z || q1.z < 0.0f) return nein;
   if(q1.x/q1.z > p3.x/p3.z) return nein;
   if(p3.z!=p2.z && q1.z > p2.z && q1.x/(q1.z-p2.z) < p3.x/(p3.z-p2.z)) return nein;
  }
 float y = A*q1.z + B*q1.x; //Formel fuer Flaeche im Raum
 if(ner!=0) printf("q1.y=%f y=%f\n",q1.y,y);//test
 abstand = q1.y - y;
 if(abstand<0) return nein;
 return abstand;
}
