// $Source: /usr/local/SimSoft/Sfx3D/src/gsl/fts.cpp,v $
// $Revision: 1.3 $
// $Author: baum $
// $Date: 2001/12/04 16:27:05 $
//
// Login name of the user who locked the revision
// $Locker:  $
// Tag name used to checkout the file
// $Name:  $
// State assigned to revision
// $State: Exp $
//
//
// $Log: fts.cpp,v $
// Revision 1.3  2001/12/04 16:27:05  baum
// -Verschiedene Bugs entfernt
//
// Revision 1.2  2001/11/29 15:18:09  baum
// -Realzeitsteuerung eingebaut
// -Handsteuerung eingebaut
//

#include <float.h>
#include <limits.h>

#include "MathDefs.h"
#include "Token.h"

#include "Vec2D.h"
#include "Vec3D.h"

#define _FTSWAGEN_CPP
#include "modell.h"

#include "sfx.h"
#include "transp.h"
#include "baustein.h"
#include "ereignis.h"
#include "arbplan.h"
#include "regal.h"
#include "fts.h"
#include "ftsver.h"   //*/

#include "regalver.h" //*/
#include "drehpuff.h"
#include "route.h"
#include "realzeit.h"

//---------------------------------------------------------
// Klasse typFtsWagen
//---------------------------------------------------------

typFtsWagen::typFtsWagen (unsigned n, unsigned b)
 : typBaustein            (n, b),

   //***** gewaehlte Vorgabewerte:
   ObjFahrRichtung        (),
   ObjFahrGeschwindigkeit (60.0),  // in m/min !!
   ObjFahrBeschleunigung  (0.0),   // in m/min2 !! (war 7200)
   ObjFahrVerzoegerung    (0.0),   // in m/min2 !! (war 3600)
   ObjFahrweg             (0.0),
   ObjHubGeschwindigkeit  (0.0),   // in m/min !!  (war 30)
   ObjHubBeschleunigung   (0.0),   // in m/min2 !! (war 5040)
   ObjHubVerzoegerung     (0.0),   // in m/min2 !! (war 2520)
   ObjHubweg              (0.0),
   ObjKnotenFahrweg       (0.0),
   ObjMotorleistung 			(0.0),
	 ObjAkkuIstKap		 			(0.0),
	 ObjAkkuSollKap		 			(0.0),
	 ObjAkkuMaxKap		 			(0.0),

   //*****Ende Vorgabewerte

   ObjHubGeo              (NULL),
   ObjVerwalter           (NULL), //*/
   ObjLadePool						(NULL), //*/

   ObjZielKoord           (),
   ObjGabelKoord          (),
	 //ObjZielWinkel					(),

   ObjFindeMeldungFunktion (typFtsWagen::findeFifoMeldung)
 {
		Vorschau = 40.0;   		// Vorgabewert
		SicherAbstand = 7.0; 	// Sicherheitsabstand zu anderen Wagen
		ObjLadeflag = 0;
 		set_NotRueckgabezaehler();
		
}

typFtsWagen::~typFtsWagen () {
  if (ObjHubGeo) delete ObjHubGeo;
}

void typFtsWagen::typTGSlesen (InputFile& file) {
  typBaustein::typTGSlesen (file);
  TGBPermPattern* myTGBPattern = MySFXModel->TGBLibrary ()->findPerm (ObjBasistyp, ObjVariante);
  const DynArray<GeoObject>& go (myTGBPattern->AllGeometries ());

  go.First ();
  if (go.Next ()) {
    ObjHubGeo = new GeoObject ();
    GeoObject* geo = new GeoObject ();
    geo->copyContents (*(go ()));
    geo->setParent (*ObjHubGeo);
    geo->setSelectionToMum ();
    ObjHubGeo->setParent (ObjGeometry);
    ObjHubGeo->setSelectionToMum ();
    //ObjHubGeo->copyContents (*(go ()));
    //ObjHubGeo->setParent (ObjGeometry);
    //ObjHubGeo->setSelectionToMum ();
    Vec3D v (ObjPalPlaetze[0]->Position ());
    ObjHubGeo->setPos (ObjGeometry.PtToLocal (v), 0);
  }
  if (ObjPalPlaetze.Count () != 1)
    sim_error ("FTSWAGEN", "typFtsWagen::typTGSlesen",
               "Verteilwagenvariante mit != 1 Palettenplatz", ObjVariante, FATAL);
}

void typFtsWagen::typTTPlesen (InputFile& file) {
  SFXToken tok;

  tok.nextToken (file);
  ObjAufnahmezeit = SimTime (atof (tok ()) * SimTime::SecondsPerMinute ());

  tok.nextToken (file);
  ObjFahrGeschwindigkeit = atof (tok ());
	tok.nextToken (file);
  ObjMotorleistung = atof (tok());
	tok.nextToken (file);
	tok.nextToken (file);
  ObjAkkuIstKap = atof (tok());
	tok.nextToken (file);
  ObjAkkuMaxKap = atof (tok());
	tok.nextToken (file);
  ObjAkkuSollKap = atof (tok());
  file.jump (200); //EOL
  ObjHubGeschwindigkeit = ObjFahrGeschwindigkeit / 2;   //**Vorgabewert!
  ObjPhysKapazitaet = ObjKapazitaet = 1;
  ObjGabelKoord = ObjPalPlaetze [0]->Position ();
}

ostream& typFtsWagen::typTTPschreiben (ostream& ostrm) const {
  ostrm.precision (2);
  ostrm << "TTP> ";
  Id_Schreiben (ostrm);
  ostrm << " ";
  ostrm.width (6);
  ostrm << ObjAufnahmezeit.inMinutes () << " ";
  ostrm.width (6);
  ostrm << ObjFahrGeschwindigkeit << " ";
  ostrm.width (6);
  ostrm << ObjHubGeschwindigkeit << endl;
  return ostrm;
}

void typFtsWagen::typTBVlesen (InputFile& file) {
  typBaustein::typTBVlesen (file);
/*
  if ((ObjZielBaustein->Basistyp () == tVerteilwagen)
      || (ObjZielBaustein->Basistyp () == tLaufkran) || (ObjZielBaustein->Basistyp () == tMehrfachLader) || (ObjZielBaustein->Basistyp () == tFtsWagen))
    sim_error ("FTSWAGEN", "typFtsWagen::typTBVlesen",
               "Veknuepfung mit Verteil-/FtsWagen, MehrfachLader oder Laufkran nicht erlaubt", ObjNummer, FATAL);
*/
}

/*void typFtsWagen::typTSTlesen (InputFile& file) {
  typBaustein::typTSTlesen (file);
  setStrategie (ObjStrategie);
}*/

/* unsigned typFtsWagen::setStrategie (unsigned stratNr) {
  ObjStrategie = stratNr;
  switch (ObjStrategie) {
    case 0:  ObjFindeMeldungFunktion = typFtsWagen::findeFifoMeldung; 
             break;
    case 1:  ObjFindeMeldungFunktion = typFtsWagen::findeKurzwegMeldung; 
             break;
    case 2:  ObjFindeMeldungFunktion = typFtsWagen::findeWartezeitMeldung;
             break;
    case 3:  ObjFindeMeldungFunktion = typFtsWagen::findeMinAvisIstMeldung;
             break;
    case 4:  ObjFindeMeldungFunktion = typFtsWagen::findeMitnahmeMeldung;
             break;
//  case 5: ObjFindeMeldungFunktion = typFtsWagen::findeTramMeldung; break;
    default: sim_error ("FTSWAGEN", "typFtsWagen::setStrategie",
                                  "Strategie ist nicht vorhanden, setzte FIFO",
                                   ObjNummer, WARNING);
             ObjFindeMeldungFunktion = typFtsWagen::findeFifoMeldung; 
             break;
  }
  return ObjStrategie;
  }*/

//*/
typTransportEinheit *typFtsWagen::gibTransportEinheit (typMeldung* zM) {
  if (zM->TransportEinheit () != ObjTransportEinheit)
    sim_error ("BAUSTEIN", "typFtsWagen::gibTransportEinheit",
               "FTS: falsche TransportEinheit angefordert", ObjNummer, FATAL);

  if (ObjBelGeschichte) ObjBelGeschichte->erfassen (ObjBelegung);

  ObjTransportEinheit->AktPlatz ()->setTransportEinheit (NULL);
  ObjTransportEinheit = NULL;
  setZustand (AKTIVIERT);
  planesofort (ETaabgeben, ObjNummer, zM->NachBaustein ()->Nummer ());
  return ObjTransportEinheit;
}

void typFtsWagen::a_aufnehmen () {
  // Erste Meldung wird aus Meldungsliste ausgehaengt
  typMeldung *zM = holeMeldung ();
  ObjZielBaustein->gibTransportEinheit (zM);
  aufnehmenEinleiten (ObjGabelKoord, ObjAufnahmezeit);
}

void typFtsWagen::e_aufnehmen () {
  setZustand (AKTIVIERT);
  typPlatz *p (ObjTransportEinheit->AktPlatz ());
  if (p->Buchung () != ObjTransportEinheit)
    sim_error ("FTSWAGEN", "typFtsWagen::e_aufnehmen",
               "TransportEinheit ist nicht auf Platz gebucht", ObjNummer, FATAL);
  p->setBuchung (NULL);
  ObjBuchung--;
  ObjTransportEinheit->positionieren (ObjGabelKoord);
  ObjTransportEinheit->drehen (ObjWinkel - ObjTransportEinheit->Winkel ());
	planesofort (ETeabgeben, ObjZielBaustein->Nummer (), ObjNummer);
	ObjZielBaustein = ObjTransportEinheit->nextBaustein ();
 	initZielKnoten ();


  if (!ObjTransportEinheit->ZielPlatz ()) {
    ObjTransportEinheit->setZielPlatz (ObjZielBaustein->waehlePlatz (ObjTransportEinheit));
  }
	 	ObjZielKoord = UebergabePos (*ObjZielBaustein);

  if ((p = ObjTransportEinheit->ZielPlatz ()) != NULL) {
    ObjZielKoord += Vec3D (0, 0, p->Position ().Z () - ObjZielKoord.Z ());
	}

   if (ObjIstKnoten != ObjZielKnoten)
    planesofort (ETafahren, ObjNummer, ObjZielBaustein->Nummer ());
  else planesofort (ETameldewarten, ObjNummer, 0);
}


unsigned typFtsWagen::FtsZustand () {
  return ObjZustand;
}

void typFtsWagen::fahr_takt () {
  if (ObjZustand != FAHREN) return;
  if (ObjTaktSchritte > 0) {
    ObjTaktSchritte--;
	
    double d;
    Vec3D v;

    if (ObjFahrweg) {
      d = ObjTaktZeit.inMinutes () * ObjFahrGeschwindigkeit;
      if (fabs (ObjFahrweg) > d)  ObjFahrweg -= (d *= Sign (ObjFahrweg));
      else  {
        d = ObjFahrweg;
        ObjFahrweg = 0.0;
      }

      v = Vec3D (ObjFahrRichtung * d);
      bewegen (v);
      ObjGabelKoord += v;
    }


 		if (ObjHubweg) {
      d = ObjTaktZeit.inMinutes () * ObjHubGeschwindigkeit;
      if (fabs (ObjHubweg) > d)  ObjHubweg -= (d *= Sign (ObjHubweg));
      else  {
        d = ObjHubweg;
        ObjHubweg = 0.0;
      }
      v = Vec3D (0.0, 0.0, d);
      ObjGabelKoord += v;
      if (ObjHubGeo) ObjHubGeo->move (v, 0);
    }

    if (ObjPalPlaetze [0]->TransportEinheit ())
      ObjTransportEinheit->positionieren (ObjGabelKoord);
    if (ObjTaktSchritte)
      planedelta (ObjTaktZeit, ETfahrTakt, ObjNummer,
                  ObjZielBaustein->Nummer (), ObjTaktSchritte);
  }	
}

void typFtsWagen::a_fahren () {
  double fahrZeit = 0.0;
 	double hubZeit = 0.0;
 	double zeit = 0.0;

  ObjNaechsterIstKnoten = NULL;
  finde_naechstenKnoten (ObjIstKnoten, ObjLetzterIstKnoten, 0.0);

  if (ObjNaechsterIstKnoten != NULL) {
	  ObjFahrRichtung = ObjNaechsterIstKnoten->Position () - ObjIstKnoten->Position ();
		ObjFahrweg = ObjFahrRichtung.len (); // Abstand finden, bevor Einheitsvektor geholt wird  	
  	ObjFahrRichtung = ObjFahrRichtung.unit ();

    set_NotRueckwaertszaehler ();
		set_NotRueckgabezaehler ();
    ObjNaechsterIstKnoten->setBelegt (this);
    ObjIstKnoten->setBelegt (NULL); 								// gebe Knoten wieder frei

    DrehInPosition ();
	  fahrZeit = Zeit_ermitteln (fabs (ObjFahrweg), ObjFahrGeschwindigkeit,
	                               ObjFahrBeschleunigung, ObjFahrVerzoegerung);

    setZustand (FAHREN);
    //===============================================================
    if (MySFXModel->RealTime ())  RzsSenden (ObjNummer, ETafahren, (int) ObjFahrweg);

    if (MySFXModel->Cycle ()) { //TAKTEN
      //===============================================================
      if (ObjNaechsterIstKnoten == ObjZielKnoten) {
      	set_Hubweg ();
	fahrZeit = Zeit_ermitteln (fabs (ObjFahrweg), ObjFahrGeschwindigkeit,
		ObjFahrBeschleunigung, ObjFahrVerzoegerung);
		hubZeit = Zeit_ermitteln (fabs (ObjHubweg), ObjHubGeschwindigkeit,
		ObjHubBeschleunigung, ObjHubVerzoegerung);      	
		zeit = (hubZeit > fahrZeit) ? hubZeit : fahrZeit;
  			//29.09.===============================================================
       } else {
  	if (ObjGabelKoord != ObjPalPlaetze [0]->Position ()) {
  	  ObjGabelKoord = ObjPalPlaetze [0]->Position ();
  	}
	zeit = fahrZeit;
       }
	    //===============================================================
      ObjTaktZeit = (ClassFahrSchritt * SimTime::SecondsPerMinute ()) / ObjFahrGeschwindigkeit;
			ObjTaktSchritte = (unsigned) ((zeit - (ObjTaktZeit.inMinutes () / ClassTaktFaktor)) / ObjTaktZeit.inMinutes ());

      if (MySFXModel->Debug ()) {
         dprintf ("Fahren %3d: Fahrweg=%5.2f Hubweg=%5.2f Schritte=%1d Ziel = %1d zum %s\n",
                  ObjNummer, ObjFahrweg, ObjHubweg, ObjTaktSchritte,
                  ObjZielBaustein->Nummer (), ObjPalPlaetze [0]->TransportEinheit () ? "bringen" : "holen");
         dprintf ("            fahrZeit=%8.5f HubZeit=%8.5f zeit=%8.5f TaktZeit=%8.5f\n",
                  fahrZeit, hubZeit, zeit, ObjTaktZeit.inMinutes ());
         dprintf ("            Gabel= (%6.3f/%6.3f/%6.3f), Ziel= (%6.3f/%6.3f/%6.3f)\n",
                  ObjGabelKoord.X (), ObjGabelKoord.Y (), ObjGabelKoord.Z (),
                  ObjZielKoord.X (), ObjZielKoord.Y (), ObjZielKoord.Z ());
      }
      if (ObjTaktSchritte > 0)  
        fahr_takt ();
    } else {
      zeit = fahrZeit;
    }
  	// 29.09.
    planedelta (zeit, ETefahren, ObjNummer, ObjZielBaustein->Nummer ());
    if (((!MySFXModel->Cycle ()) || (ObjTaktSchritte == 0)) && (ObjFahrweg != 0.0)) {
    //if (ObjFahrweg != 0.0) {
    	//===============================================================
      Vec3D weg (ObjNaechsterIstKnoten->Position () - ObjGeometry.Pos ());
      positionieren (ObjGeometry.Pos () + weg.projOn (ObjFahrRichtung));
      //ObjGabelKoord = ObjZielKoord;
      ObjGabelKoord = ObjPalPlaetze [0]->Position ();
      if (ObjHubGeo) 
        ObjHubGeo->setPos (ObjGeometry.PtToLocal ((weg = ObjGabelKoord)), 0);
       if (ObjPalPlaetze [0]->TransportEinheit ())  
  	    ObjTransportEinheit->positionieren (ObjGabelKoord);
    }
  }else {
		NotRueckwaertszaehler--;
		NotRueckgabezaehler--;
		if (NotRueckwaertszaehler == 0) {
			ObjLetzterIstKnoten = ObjIstKnoten;
			set_NotRueckwaertszaehler();
		}

		if (NotRueckgabezaehler == 0) {
      if (ObjMeldungen.Count () != 0 ){
				typMeldung* zM = ObjMeldungen.First ();				
        typFtsWagen* frei = ObjVerwalter->findeFreienWagen(zM);
				zM = holeMeldung();
				frei->empfangeMeldung(zM);
				planesofort(ETaktivieren,frei->Nummer(),ObjNummer);
        if (ObjBuchung != 0) ObjBuchung =0;
	    }
			set_NotRueckgabezaehler ();
			if (ObjLadePlatz) ((typFtsLadePlatz*)ObjLadePlatz)->loescheBuchung();
			schickeLaden();
    }
		else {
			setZustand(WARTEN);
			planedelta (SimTime(10), ETafahren, ObjNummer, ObjZielBaustein->Nummer ());
    }
  }
	ObjAkkuIstKap -= zeit*ObjMotorleistung/24;
}


void typFtsWagen::e_fahren() {
	ObjLetzterIstKnoten = ObjIstKnoten;
	ObjIstKnoten = ObjNaechsterIstKnoten;
  setZustand (AKTIVIERT);

  //===============================================================

	if (ObjIstKnoten != ObjZielKnoten) {
	/** Wenn freiwillig zum Laden, wird das Laden unterbrochen wenn eine neue Meldung eintrifft **/
			if (!ObjMeldungen.isEmpty () && ObjLadeflag == 1) {
				ObjLadeflag = 0;
				setZustand(RUHE);
				if (ObjLadePlatz)((typFtsLadePlatz*)ObjLadePlatz)->loescheBuchung();
				planesofort (ETaktivieren, ObjNummer, ObjNummer);
			}
			else
    	planesofort(ETafahren, ObjNummer, ObjZielBaustein->Nummer ()); // weiterfahren
	}
  else {   		

		if (ObjZielBaustein->Basistyp () == 29 )
			planesofort(ETaladen, ObjNummer,ObjNummer);
		
		else {
		  WagenAusrichten ();
  		/*if (ObjZielKoord != ObjGabelKoord) {
    		//Vec3D v (ObjZielKoord - ObjGabelKoord);
		  	
				//v = (v - ObjGabelKoord).projOn (ObjFahrRichtung);		
    		ObjGabelKoord = ObjZielKoord;
    		//if (ObjHubGeo) ObjHubGeo->setPos (ObjGeometry.PtToLocal ((v = ObjGabelKoord)), 0);
    		//if (ObjPalPlaetze [0]->TransportEinheit ())  ObjTransportEinheit->positionieren (ObjZielKoord);
  		}*/


   		ObjZielKnoten = NULL;
   		ObjLetzterIstKnoten = ObjIstKnoten;

  		if (typTransportEinheit* zTE = ObjPalPlaetze [0]->TransportEinheit ()) {
	    	if (ObjPalPlaetze [0]->TransportEinheit () && ObjTransportEinheit->ZielPlatz ()) {
    			ObjZielBaustein = NULL;
    			planesofort (ETameldewarten, ObjNummer, 0);
  			}
  			else {
  				// Wo geht dieser Aufruf hin?
      		planesofort (ETameldewarten, ObjNummer, ObjNummer);
      	}
  		}
  		else {
	  		planesofort (ETaktivieren, ObjNummer, ObjNummer);
  		}
  	}
	}	
}

void typFtsWagen::e_abgeben () {
  // Extra fuer den FtsWagen geschrieben

  ObjGabelKoord = ObjPalPlaetze [0]->Position (); // rudi 18.09.00
  decBelegung ();
	if ((ObjAkkuIstKap < ObjAkkuSollKap)) {
		ObjLadeflag = 2;
		schickeLaden ();
	}
	/** Der Wagen wird immer zum Ladenen geschickt damit er den Weg und den letzten Baustein freigibt **/
	else {
	  setZustand (RUHE);
  	planesofort (ETaktivieren, ObjVerwalter->Nummer (), ObjNummer);
  	ObjLadeflag = 1;
		schickeLaden ();
	}
}

/* lar 25.09.00 */

void typFtsWagen::a_laden () {
	double LadeZeit = ObjLadePool->Laden(this);
	planedelta (SimTime(LadeZeit),ETeladen, ObjNummer, ObjNummer);
  setZustand(LADEN);
}

void typFtsWagen::e_laden () {
	ObjAkkuIstKap = ObjAkkuMaxKap;
	((typFtsLadePlatz*)ObjLadePlatz)->loescheBuchung();
  setZustand(RUHE);
	planesofort(ETaktivieren, ObjNummer,ObjNummer);
}

bool typFtsWagen::hasKapazitaet (typTransportEinheit*) const {
  return (ObjBelegung + ObjBuchung) < ObjKapazitaet;
}

typPlatz* typFtsWagen::waehlePlatz (typTransportEinheit* zTE) {
  typPlatz* p (ObjPalPlaetze [0]);
  if (!p->isVerfuegbar ())  return (p->Buchung () == zTE) ? p : (typPlatz*) NULL;
  p->setBuchung (zTE);
  ObjBuchung++;
  return p;
}
// folgende Zeilen sind notwendig, da sonst Fehlermeldung (s. baustein.cpp)
typBaustein* typFtsWagen::setVerwalter (typBaustein* newVerwalter) {
  if (newVerwalter->Basistyp () != tFtsVerwalter)
    sim_error ("BAUSTEIN", "typFtsVerwalter::setVerwalter",
               "FtsVerwalter kann nur von FtsVerwalter verwaltet werden", ObjNummer, FATAL);
  return (ObjVerwalter = (typFtsVerwalter*) newVerwalter);
}


Vec3D typFtsWagen::UebernahmePos (const typBaustein& von) const {
  Vec3D v (von.UebergabePos (*this));
  double zielZ (v.Z ());
  //Vec3D Richtung (von.Position () - ObjGeometry.Pos ());
	v = (v - ObjGabelKoord).projOn (ObjFahrRichtung);
  v += ObjGabelKoord;
  return Vec3D (v.X (), v.Y (), zielZ);
}

Vec3D typFtsWagen::UebergabePos (const typBaustein& nach) const {
  Vec3D v (nach.UebernahmePos (*this));
  double zielZ (v.Z ());
  //Vec3D Richtung (nach.Position () - ObjGeometry.Pos ());
	v = (v - ObjGabelKoord).projOn (ObjFahrRichtung);
  v += ObjGabelKoord;
  return Vec3D (v.X (), v.Y (), zielZ);
}

const double& typFtsWagen::setGeschwindigkeit (const double& wert) {
  if (wert <= 0.0)
    sim_error ("FTSWAGEN", "typFtsWagen::setGeschwindigkeit",
               "negative Fahrgeschwindigkeit", ObjNummer, FATAL);
  return ObjFahrGeschwindigkeit = wert;
}

const double& typFtsWagen::setFahrBeschleunigung (const double& wert) {
  if (wert <= 0.0)
    sim_error ("FTSWAGEN", "typFtsWagen::setFahrBeschleunigung",
               "negative Fahrbeschleunigung", ObjNummer, FATAL);
  return ObjFahrBeschleunigung = wert;
}

const double& typFtsWagen::setFahrVerzoegerung (const double& wert) {
  if (wert <= 0.0)
    sim_error ("FTSWAGEN", "typFtsWagen::setFahrVerzoegerung",
               "negative Fahrverzoegerung", ObjNummer, FATAL);
  return (ObjFahrVerzoegerung = wert);
}

const double& typFtsWagen::setHubGeschwindigkeit (const double& wert) {
  if (wert <= 0.0)
    sim_error ("FTSWAGEN", "typFtsWagen::setHubGeschwindigkeit",
               "negative Hubgeschwindigkeit", ObjNummer, FATAL);
  return ObjHubGeschwindigkeit = wert;
}

const double& typFtsWagen::setHubBeschleunigung (const double& wert) {
  if (wert <= 0.0)
    sim_error ("FTSWAGEN", "typFtsWagen::setHubBeschleunigung",
               "negative Hubbeschleunigung", ObjNummer, FATAL);
  return ObjHubBeschleunigung = wert;
}

const double& typFtsWagen::setHubVerzoegerung (const double& wert) {
  if (wert <= 0.0)
    sim_error ("FTSWAGEN", "typFtsWagen::setHubVerzoegerung",
               "negative Hubverzoegerung", ObjNummer, FATAL);
  return (ObjHubVerzoegerung = wert);
}

void typFtsWagen::individuellAktivieren (typBaustein* vonWem) {
	if ((ObjZustand == RUHE) || (ObjZustand == POLLING) || (vonWem == this)
      || ((ObjZustand == WARTEDP) && (ObjZielBaustein == vonWem))/* || (ObjTankflag == 1)*/)  {
	  //===============================================================
  	if (ObjPalPlaetze [0]->TransportEinheit ()) {
  		if (!ObjTransportEinheit->ZielPlatz ())
    		if (ObjTransportEinheit->setZielPlatz (ObjZielBaustein->waehlePlatz (ObjTransportEinheit))) {
      		ObjZielKoord += Vec3D (0, 0, ObjTransportEinheit->ZielPlatz ()->Position ().Z ()
          										- ObjZielKoord.Z ());
        	if (ObjIstKnoten != ObjZielKnoten)
        		planesofort (ETafahren, ObjNummer, ObjZielBaustein->Nummer ());
        	else  planesofort (ETameldewarten, ObjNummer, 0);
      	}
      	else  planesofort (ETameldewarten, ObjNummer, 0);
	   		return;
  	}
  	else {
  		setZustand (RUHE);
    	// Wenn der Wagen keine TE hat, wird Verwalter aufgefordert
    	// was zu machen
    	if (ObjVerwalter) {
    		ObjVerwalter->aktivieren (ObjNummer);
    	}
  	}    //ObjPalPlaetze else
  	typMeldung* zM = findeAusfuehrbareMeldung ();
  	if (zM) {
			ObjTransportEinheit = zM->TransportEinheit ();
  		ObjZielBaustein = zM->VonBaustein ();
			initZielKnoten ();
    	Vec3D v (UebernahmePos (*ObjZielBaustein));
    	ObjZielKoord = Vec3D (v.X (), v.Y (), ObjTransportEinheit->AktPlatz ()->Position ().Z ());
		
			if (ObjIstKnoten != ObjZielKnoten) {
    		setZustand (AKTIVIERT);
      	planesofort (ETafahren, ObjNummer, ObjZielBaustein->Nummer ());
    	}
    	else {
    		wartenaufnehmen (ObjTransportEinheit, ObjZielBaustein);
    		if (ObjGabelKoord != ObjZielKoord)
    			ObjGabelKoord = ObjZielKoord; // rudi 12.10.
    	}
  	}
  	else {
  		if (!(ObjMeldungen.isEmpty ())) {
  		setZustand (RUHE);
  		planedelta (SimTime(100), ETaktivieren, ObjNummer, ObjNummer);
			}
			else {
	 			setZustand (RUHE);
	 			ObjVerwalter->aktivieren (ObjNummer);
			}
 		}
 	}
}

typMeldung* typFtsWagen::findeAusfuehrbareMeldung () {
  return ObjFindeMeldungFunktion (this);
}

typMeldung* typFtsWagen::findeFifoMeldung (typFtsWagen* who) {
  typTransportEinheit* zBuch = who->ObjPalPlaetze [0]->Buchung ();
  for (LCursor<typMeldung> c (who->ObjMeldungen); c; c++) {
    typTransportEinheit* zTE = c ()->TransportEinheit ();
    if (!zBuch || (zTE == zBuch)) {
      if (!zTE->BuchPlatz ()) {
        typBaustein* zNext = zTE->nextBaustein ();
        if (zNext->hasKapazitaet (zTE))  zTE->setBuchPlatz (zNext->waehlePlatz (zTE));
      }
//      if (((who->ObjEingaenge.Count () > 1) || (who->ObjAusgaenge.Count () > 1)) && (!zTE->BuchPlatz ())) continue;
      if (!zBuch)  zTE->setZielPlatz (who->waehlePlatz (zTE));
      return &(who->ObjMeldungen.prepend (*(who->ObjMeldungen.remove (c))));
    }
  }
  return NULL;
}

typMeldung* typFtsWagen::findeKurzwegMeldung (typFtsWagen* who) {
  typTransportEinheit* zBuch = who->ObjPalPlaetze [0]->Buchung ();
  typMeldung* zM;

  for (unsigned i = who->ObjMeldungen.Count (); i; i--) {
    Vec3D min (DBL_MAX, DBL_MAX, DBL_MAX);
    LCursor<typMeldung> c (who->ObjMeldungen, 1); // toEnd
    for (unsigned j = 1; j <= i; c--, j++) {
      Vec3D v (c ()->TransportEinheit ()->AktPlatz ()->Position () - who->ObjGabelKoord);
      if (v.len () < min.len ()) {
        min = v;
        zM = c ();
      }
    }
    typTransportEinheit* zTE = zM->TransportEinheit ();
    if (!zBuch || (zTE == zBuch)) {
      if (!zTE->BuchPlatz ()) {
        typBaustein* zNext = zTE->nextBaustein ();
        if (zNext->hasKapazitaet (zTE))  zTE->setBuchPlatz (zNext->waehlePlatz (zTE));
      }
      if (((who->ObjEingaenge.Count () > 1) || (who->ObjAusgaenge.Count () > 1)) && (!zTE->BuchPlatz ())) continue;
      if (!zBuch)  zTE->setZielPlatz (who->waehlePlatz (zTE));
      return &(who->ObjMeldungen.prepend (*(who->ObjMeldungen.remove (*zM))));
    }
    who->ObjMeldungen.prepend (*(who->ObjMeldungen.remove (*zM)));
  }
  return NULL;
}

double typFtsWagen::Zeit_ermitteln (const double& weg, const double& geschw,
                                  const double& beschl, const double& verz) const {
  if (beschl && verz) {
    double f = (1 / beschl) + (1 / verz);  // Rechenfaktor f. Beschleunigungen
    double v = sqrt (2 * weg / f);         // erreichbare Geschwindigkeit
    if (v > geschw)                        // Trapezrechnung
      return (geschw * f) + ((weg - (geschw * geschw / 2 * f)) / geschw);
    else
      return v * f;                     // Dreieckrechnung
  }
  else
    return weg / geschw;                // Rechteckrechnung
}

void typFtsWagen::DrehInPosition () {
  Angle ang1 (EX.angle (ObjFahrRichtung.scale (Vec3D (1.0, 1.0, 0.0))));
  Angle ObjZielWinkel = (ObjFahrRichtung.Y () < 0.0) ? (Angle (360) - ang1) : ang1;

  Angle drehWinkel = ObjZielWinkel - ObjWinkel;
  			
	//drehen (ang);
  drehen (drehWinkel);
  typTransportEinheit* zTE = ObjPalPlaetze [0]->TransportEinheit ();
  if (zTE){
		zTE->drehen (drehWinkel);
		zTE->positionieren (ObjPalPlaetze [0]->Position ());
  }
}

double typFtsWagen::finde_naechstenKnoten(typNetzKnoten *IstKn, typNetzKnoten *letzterKn, double Weg){
  double min_q_abstand = DBL_MAX;

  for (IstKn->nachfolger.First (); IstKn->nachfolger (); IstKn->nachfolger++) {
    typNetzKnoten* knotenfinder = IstKn->nachfolger ();
    double KnWeg = AbstandIstZielKn (IstKn, knotenfinder);
    if ((knotenfinder != letzterKn)           //kein Rueckwaertsgang
	&& (knotenfinder != ObjIstKnoten)           //keine Schleife
	&& (!knotenfinder->Besucht ()  )
	&& (!knotenfinder->Gesperrt () )){
//	&& (!knotenfinder->Belegt () || (Weg > SicherAbstand) )) { //kein Dejavu (roter Faden)

			if ((knotenfinder->Belegt () != NULL) && (IstKn == ObjIstKnoten)) {
				if ((knotenfinder->Belegt ()->Zustand () == FAHREN) ) {
					ObjNaechsterIstKnoten = NULL;
					return min_q_abstand;
				}
      }
      else {
      if ((knotenfinder != ObjZielKnoten) && (Weg < Vorschau)) {
				knotenfinder->setBesucht (true);  // "Roten Faden" legen
				KnWeg += finde_naechstenKnoten (knotenfinder, IstKn, Weg + KnWeg);
      }
      else KnWeg += AbstandIstZielKn (knotenfinder, ObjZielKnoten);

      	if (KnWeg < min_q_abstand) {
					min_q_abstand = KnWeg;
					// Folgende Abfrage ist wichtig fuer die Zuweisung des richtigen Knotens.
					// Der rekursive Aufruf dieser Funktion geht auch den gleichen Weg zurueck. Somit
					// wird gewaehrleistet, dass erst in der obersten Ebene die Zuweisung des Knotens erfolgt.
					if (IstKn == ObjIstKnoten)      	 				// nur in erster Ebene der Knoten
	  			ObjNaechsterIstKnoten = knotenfinder; // fuer das fragende FTS
      	}
 	    	knotenfinder->setBesucht (false);         // "Roten Faden" einrollen
    	}
    }
  }
  return min_q_abstand;
}

double typFtsWagen::AbstandIstZielKn (typNetzKnoten* IstKn, typNetzKnoten* ZielKn) {
	// berechnet den Abstand zweier Knoten
	Vec3D Abstand (ZielKn->Position () - IstKn->Position ());
	double VekAbstand = Abstand.len ();
 	return VekAbstand;
}

void typFtsWagen::set_NotRueckwaertszaehler (){
	if (TransportEinheit () != NULL) NotRueckwaertszaehler = 10;
  else NotRueckwaertszaehler = 13;
}


void typFtsWagen::set_NotRueckgabezaehler (){
   NotRueckgabezaehler = 15;
}



/*** lar 14.09.00 ***/
void typFtsWagen::initIstKnoten () {
	ObjIstKnoten = ObjVerwalter->suchKnoten((typBaustein*)this);	
	ObjIstKnoten->setBelegt(this);
	ObjLetzterIstKnoten = ObjIstKnoten;
	positionieren (ObjIstKnoten->Position ());
}

void typFtsWagen::initZielKnoten () {
	ObjZielKnoten = ObjVerwalter->suchKnoten ((typBaustein*)ObjZielBaustein);
}
/*** lar ***/


void typFtsWagen::WagenAusrichten () {
	// Vektor v zwischen Wagen und ZielBaustein
  Vec3D v (ObjZielBaustein->Position () - ObjGeometry.Pos ());
  double abs = v.len (); // Hypotenuse
  Angle ang (ObjFahrRichtung.angle (v.scale (Vec3D (1.0, 1.0, 0.0))));
	//Länge aweg zwischen Wagen und EndHaltepunkt
	double aweg = (ang.cos () * abs);
	
	typTransportEinheit* zTE = ObjPalPlaetze [0]->TransportEinheit ();
	// Der Wagen richtet sich am Baustein nicht richtig aus. Wenn man genauer hinschaut,
	// gleitet eine kleine TE diagonal zum FtsWagen oder zum Baustsin.
	double dx = ObjZielBaustein->Position ().X () - ObjGeometry.Pos ().X ();
	double dy = ObjZielBaustein->Position ().Y () - ObjGeometry.Pos ().Y ();
	
	// 0.1 ist Toleranzbereich	
	if ((fabs (dx) < 0.1) || (fabs (dy) < 0.1)) {
		Angle deltaWinkel;
  	Angle wagenEX (EX.angle (ObjFahrRichtung.scale (Vec3D (1.0, 1.0, 0.0))));
  	Angle wagenEY (EY.angle (ObjFahrRichtung.scale (Vec3D (1.0, 1.0, 0.0))));

		if (wagenEY > 90)
			if (wagenEX > 90)
  			deltaWinkel = (ang < 90.0) ? ((Angle) 90.0 - ang) : (ang - (Angle) 90.0);
  		else
  			deltaWinkel = (ang < 90.0) ? (ang -(Angle) 90.0) : ((Angle) 90.0 - ang);
		else
			if (wagenEX < 90)
				deltaWinkel = (ang < 90.0) ? ((Angle) 90.0 - ang) : (ang - (Angle) 90.0);
			else
				deltaWinkel = (ang < 90.0) ? (ang - (Angle) 90.0) : ((Angle) 90.0 - ang);
			
 		drehen (deltaWinkel);
  	if (zTE)
  		zTE->drehen (deltaWinkel);
 	}		
	else {
		bewegen (ObjFahrRichtung * aweg);
 	
 		if (zTE)
			zTE->bewegen (ObjFahrRichtung * aweg);
  }	
}


/* lar 26.09.00 */
void typFtsWagen::schickeLaden () {
	if (ObjLadePool != NULL) {
		setZustand (AKTIVIERT);
    ObjLadePlatz = ObjLadePool->gibLadeplatz(this);
		if (ObjLadePlatz!=NULL) {
			ObjZielBaustein = ObjLadePlatz;
			((typFtsLadePlatz*)ObjLadePlatz)->setBuchung();
			initZielKnoten();
			planesofort (ETafahren, ObjNummer, ObjZielBaustein->Nummer ());
  	}
		else {
				setZustand (RUHE);
    }
	
  }
}


void typFtsWagen::setLadePoolVerwalter (typFtsLadePool* newVerwalter) {
	ObjLadePool = newVerwalter;
}

void typFtsWagen::set_Hubweg () {
  Vec3D v (ObjZielKoord - ObjGabelKoord);
	v = v.projOn (ObjFahrRichtung);
	ObjHubweg = v.Z ();
	if (ObjHubweg > 0.0)
		ObjZielKoord = ObjGabelKoord + Vec3D (v.X (), v.Y (), ObjHubweg);
}

