// $Source: /usr/local/SimSoft/Sfx3D/src/gsl/staufoer.cpp,v $
// $Revision: 1.2 $
// $Author: baum $
// $Date: 2001/11/29 15:18:10 $
//
// 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: staufoer.cpp,v $
// Revision 1.2  2001/11/29 15:18:10  baum
// -Realzeitsteuerung eingebaut
// -Handsteuerung eingebaut
//

#include <limits.h>

#include "MathDefs.h"

#include "Vec3D.h"
#include "HMatrx3D.h"
#include "Token.h"
#include "List.h"
#include "SimTime.h"

#define _STAUFOER_CPP
#include "modell.h"
#include "staufoer.h"

#include "sfx.h"
#include "transp.h"
#include "baustein.h"
#include "ereignis.h"
#include "realzeit.h"
#include "arbplan.h"

struct StauDaten {
  StauDaten (typTransportEinheit* TE, const double& pos, const double& lastPos,
             const double& len);
  typTransportEinheit* TE;
  double Pos, LastPos, Len, Speed;
  SimTime ZielZeit;
};

StauDaten::StauDaten (typTransportEinheit* TE, const double& pos, const double& lastPos,
                      const double& len)
 : TE (TE), Pos (pos), LastPos (lastPos),
  Len (len), Speed (0.0), ZielZeit ()
  { }

//---------------------------------------------------------
// Klasse typStaufoerderer
//---------------------------------------------------------

typStaufoerderer::typStaufoerderer (unsigned n, unsigned b)
: typBaustein       (n,b),
  ObjLaenge         (0.0),
  ObjFoerderGeschw  (0.0),
  ObjEffGeschw      (0.0),
  ObjStauGrenze     (0.0),
  ObjRichtung       (),
  ObjAufnahmeDaten  (NULL),
  ObjTeile          (),
  ObjFTaktTime      (),
  ObjFTaktDelta     (0.0),
  ObjAbgabeLaenge   (0.0)
  { }

typStaufoerderer::~typStaufoerderer () {
  while (ObjTeile.Last ()) delete ObjTeile.removeLast ();
}

/*virtual*/ void typStaufoerderer::typTGSlesen (InputFile& file) {
  typBaustein::typTGSlesen (file);
  if (ObjPalPlaetze.Count () != 2)
    sim_error ("STAUFOER", "typStaufoerderer::typTGSlesen",
        "Staufoerderervariante mit != 2 Palettenplaetzen", ObjVariante, FATAL);
}

/*virtual*/ void typStaufoerderer::typTTPlesen     (InputFile& file) {
  SFXToken tok;

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

  tok.nextToken (file);
  ObjFoerderGeschw = atof (tok ());
  ObjEffGeschw = ObjFoerderGeschw;
  ObjFTaktDelta = ClassNehmSchritt / ObjFoerderGeschw;

  tok.nextToken (file);
  ObjLaenge = atof (tok ());

  tok.nextToken (file);
  ObjPhysKapazitaet = ObjKapazitaet = atoi (tok ());

  file.jump (100); //EOL
}

/*virtual*/ const double& typStaufoerderer::setGeschwindigkeit (const double& wert) {
  if (wert <= 0.0)
    sim_error ("BAUSTEIN", "typStaufoerderer::setGeschwindigkeit",
               "negative Foerdergeschwindigkeit", ObjNummer, FATAL);
  return ObjFoerderGeschw = wert;
}

typTransportEinheit* typStaufoerderer::TransportEinheit () const {
   return ObjTeile.isEmpty () ? ObjTransportEinheit : ObjTeile.First ()->TE;
}

/*virtual*/ typTransportEinheit *typStaufoerderer::gibTransportEinheit (typMeldung* zM) {
  typBaustein* ausgang = zM->NachBaustein ();
  typTransportEinheit *zTE = zM->TransportEinheit ();
  ObjAbgabeLaenge = zTE->Laenge (ObjRichtung).len ();
  if (ausgang->Aufnahmezeit () > 0)
    ObjEffGeschw = Min (ObjFoerderGeschw,
              ObjAbgabeLaenge / ausgang->Aufnahmezeit ().inMinutes ());
  if (!ObjTeile.isEmpty () && (zTE == ObjTeile.First ()->TE))
    delete ObjTeile.removeFirst ();
  else sim_error ("STAUFOER", "typStaufoerderer::gibTransportEinheit",
                  "falsches TransportEinheit angefordert", ObjNummer, FATAL);
  decBelegung ();
  planesofort (ETaabgeben, ObjNummer, ausgang->Nummer ());
  return zTE;
}

/*virtual*/ void typStaufoerderer::a_aufnehmen () {
  typMeldung *zM = holeMeldung ();
  ObjZielBaustein->gibTransportEinheit (zM);
  Vec3D TELaenge = ObjTransportEinheit->Laenge (ObjRichtung);
  ObjTransportEinheit->positionieren (ObjPalPlaetze [0]->Position () - (TELaenge * 0.5));
  Vec3D zielPos (ObjPalPlaetze [0]->Position () + (TELaenge * 0.5));
  double aufnahmeWeg = (zielPos - ObjTransportEinheit->Position ()).len ();

  ObjAufnahmeDaten = new StauDaten (ObjTransportEinheit,
    ObjLaenge - TELaenge.len (), ObjLaenge + aufnahmeWeg - TELaenge.len (), TELaenge.len ());
  ObjAufnahmeDaten->Speed    = ObjFoerderGeschw;
  ObjAufnahmeDaten->ZielZeit = (aufnahmeWeg /  ObjFoerderGeschw) * SimTime::SecondsPerMinute ();

  setZustand (FOERDERN);
  ObjTransportEinheit->schaltePlatz ();
  ObjTransportEinheit->AktPlatz ()->setTransportEinheit (ObjTransportEinheit);
  incBelegung ();
  planedelta (ObjAufnahmeDaten->ZielZeit, ETeaufnehmen, ObjNummer, ObjZielBaustein->Nummer ());
  if (MySFXModel->Cycle ()) {
    ObjTaktSchritte = (unsigned) ((aufnahmeWeg - (ClassNehmSchritt / ClassTaktFaktor)) / ClassNehmSchritt);
    if (ObjTaktSchritte > 0) {
      ObjTaktZeit = (ObjAufnahmeDaten->ZielZeit.inSeconds () / aufnahmeWeg) * ClassNehmSchritt;
      aufn_takt ();
    }
    else positioniereAufAnfang (*ObjAufnahmeDaten);
  }
                               else positioniereAufAnfang (*ObjAufnahmeDaten);
}

/*virtual*/ void typStaufoerderer::e_aufnehmen () {
  typPlatz *p (ObjTransportEinheit->AktPlatz ());
  if (p->Buchung () != ObjTransportEinheit)
    sim_error ("typStaufoerderer", "::e_aufnehmen",
               "TransportEinheit ist nicht auf Platz gebucht", ObjNummer, FATAL);
  //  ObjTransportEinheit->faerbeGestalt (default);
  if (MySFXModel->Debug ())
    dprintf ("Aufn_Ende %1d -> %1d: xW=%5.2f yW=%5.2f z=%5.2f\n",
             ObjZielBaustein->Nummer (), ObjNummer, ObjTaktZiel.X (), ObjTaktZiel.Y (), ObjTaktZiel.Z ());

  ObjAufnahmeDaten->ZielZeit = 0;
  ObjTeile.append (*ObjAufnahmeDaten);
  if (ObjAufnahmeDaten->Pos > 0.0) {
    positioniereAufAnfang (*ObjAufnahmeDaten);
    planesofort (ETafoerdern, ObjNummer, ObjTransportEinheit->ID());
  }
  else {
    positioniereAufEnde (*ObjAufnahmeDaten);
    ObjAufnahmeDaten->TE->Anmelden ();
  }
  if (hasMeldungen ()) planesofort (ETaktivieren, ObjNummer, ObjNummer);
  planesofort (ETeabgeben, ObjZielBaustein->Nummer (), ObjNummer);

  p->setBuchung (NULL);
  ObjBuchung--;
  p->setTransportEinheit (NULL);
  ObjAufnahmeDaten    = NULL;
  ObjTransportEinheit = NULL;
  ObjZielBaustein     = NULL;
  setZustand (FOERDERN);
}

/*virtual*/ void typStaufoerderer::a_foerdern () {
  LCursor<StauDaten> c (ObjTeile);
  SimTime eZeit (INT_MAX);
  bool mussFoerdern = false;
  ObjStauGrenze = 0.0;
  double weg = 0.0;
  int z = 0; //rei050525
  for (StauDaten *s, *p = NULL; (s = c ()) != NULL; c++)  {
    if ((s->ZielZeit < Modellzeit ()) && (s->Pos > ObjStauGrenze)) {
      mussFoerdern = true; z++;
      s->Speed = ((!p && (ObjAbgabeLaenge == s->Pos)) || (p && ((ObjStauGrenze + weg) == s->Pos)))
                 ? ObjEffGeschw : ObjFoerderGeschw;
      weg = s->Pos - ObjStauGrenze;
      s->ZielZeit = Modellzeit ()
                    + SimTime ((weg / s->Speed) * SimTime::SecondsPerMinute ());
      //rei050525 if (eZeit > s->ZielZeit)  eZeit = s->ZielZeit;
      if (z == 1)  eZeit = s->ZielZeit;
      s->Pos = ObjStauGrenze;
      if (ObjFTaktTime >= Modellzeit ()) {
        weg = (ObjFTaktTime - Modellzeit ()).inMinutes () * s->Speed;
        if (weg > 0.0)  bewegeTE (*s, weg);
      }
      else
        if (!MySFXModel->Cycle ()) bewegeTE (*s, weg);
    }
    ObjStauGrenze += s->Len;
    p = s;
  }
  if (mussFoerdern) {
    setZustand (FOERDERN);
    if ((MySFXModel->Cycle ()) && (ObjFTaktTime < Modellzeit ()))
      fahr_takt ();
    planeein (eZeit, ETefoerdern, ObjNummer, notiz::AktInfo2());
    if (MySFXModel->RealTime ())  RzsSenden (ObjNummer, ETafoerdern, 1);
  }
}

/*virtual*/ void typStaufoerderer::e_foerdern () {
  ObjStauGrenze = 0.0;
  SimTime eZeit (INT_MAX);
  bool nochmalStarten = false;
  LCursor<StauDaten> c (ObjTeile);
  for (StauDaten* s; (s = c ()) != NULL; c++) {
    if (s->ZielZeit == Modellzeit ())  {
      s->Speed = 0.0;
      s->ZielZeit = 0;
      if (s->Pos == 0.0) {
        positioniereAufEnde (*s);
        s->TE->Anmelden ();
      }
      else positioniereTE (*s);
      if (s->Pos > ObjStauGrenze) {
        nochmalStarten = true;
      }
    }
    ObjStauGrenze += s->Len;
  }
  if (nochmalStarten) planesofort (ETafoerdern, ObjNummer, -1);
  else  if (!ObjTransportEinheit && ObjTeile.isEmpty ()) setZustand (RUHE);
  if (!ObjTransportEinheit && (ObjStauGrenze < ObjLaenge)) aktiviereEingaenge ();
}

/*virtual*/ void typStaufoerderer::a_abgeben () {
  setZustand (FOERDERN);
  if (!ObjTransportEinheit) planesofort (ETaktivieren, ObjNummer, ObjNummer);
  if (ObjTeile.isEmpty ()) ObjStauGrenze = 0;
  else planesofort (ETafoerdern, ObjNummer, -3);
}

/*virtual*/ void typStaufoerderer::e_abgeben () {
  ObjEffGeschw = ObjFoerderGeschw;
  ObjAbgabeLaenge = 0.0;
  if (!ObjTransportEinheit && ObjTeile.isEmpty ()) setZustand (RUHE);
  aktiviereEingaenge ();
}

/*virtual*/ void typStaufoerderer::aufn_takt () {
  if (!ObjTransportEinheit || !ObjAufnahmeDaten || !ObjTaktSchritte) return;
  ObjTaktSchritte--;
  bewegeTE (*ObjAufnahmeDaten, ObjAufnahmeDaten->Speed * ObjTaktZeit.inMinutes ());
  planedelta (ObjTaktZeit, ETaufnTakt, ObjNummer);
}

/*virtual*/ void typStaufoerderer::fahr_takt () {
  if (ObjZustand != FOERDERN)  return;

  SimTime nextTakt (ObjFTaktDelta * SimTime::SecondsPerMinute ());
  nextTakt += Modellzeit ();
  LCursor<StauDaten> c (ObjTeile);
  bool nochmaltakten = false;
  for (StauDaten* s; (s = c ()) != NULL; c++) {
    if (s->ZielZeit >= nextTakt) {
      bewegeTE (*s, s->Speed * ObjFTaktDelta);
      nochmaltakten = true;
    }
  }
  if (nochmaltakten && MySFXModel->Cycle ())  {
    ObjFTaktTime = nextTakt;
    planeein (ObjFTaktTime, ETfahrTakt, ObjNummer);
  }
}

/*virtual*/ bool typStaufoerderer::hasKapazitaet (typTransportEinheit* zTE) const {
  if (ObjPalPlaetze [0]->Buchung () || (ObjBelegung >= ObjKapazitaet))  return false;
  return zTE ? passtDrauf (zTE) : (ObjStauGrenze < ObjLaenge);
}

/*virtual*/ typPlatz* typStaufoerderer::waehlePlatz (typTransportEinheit* zTE) {
  typPlatz* p (ObjPalPlaetze[0]);
  if (p->Buchung ())  
    return (p->Buchung () == zTE) ? p : (typPlatz*) NULL;
  if (passtDrauf (zTE)) {
    p->setBuchung (zTE);
    ObjBuchung++;
    return p;
  }
  else return NULL;
}

/*virtual*/ Vec3D typStaufoerderer::UebernahmePos (const typBaustein&) const {
  return (ObjPalPlaetze [0]->Position ());
}

/*virtual*/ Vec3D typStaufoerderer::UebergabePos (const typBaustein&) const {
  return (ObjPalPlaetze [1]->Position ());
}

/*virtual*/ unsigned typStaufoerderer::AnzahlArbeitsplanVorhanden
                     (unsigned arbNr, unsigned istv /*= 0*/) const {
  unsigned sumTeile = 0;
  if (ObjTransportEinheit && (ObjTransportEinheit->Baustein () == (typBaustein*) this)
      && ObjTransportEinheit->hatArbeitsplan (arbNr, istv))   sumTeile++;
  for (LCursor<StauDaten> c (ObjTeile); c; c++)
    if (c ()->TE->hatArbeitsplan (arbNr, istv))  sumTeile++;
  return sumTeile;
}

/*virtual*/ unsigned typStaufoerderer::AnzahlArtikelVorhanden
                     (unsigned artNr, unsigned istv /*= 0*/) const {
  unsigned sumTeile = 0;
  if (ObjTransportEinheit && (ObjTransportEinheit->Baustein () == (typBaustein*) this)
      && ObjTransportEinheit->hatArtikel (artNr, istv))   sumTeile++;
  for (LCursor<StauDaten> c (ObjTeile); c; c++)
    if (c ()->TE->hatArtikel (artNr, istv))  sumTeile++;
  return sumTeile;
}

/*virtual*/ double typStaufoerderer::MengeArbeitsplanVorhanden
                   (unsigned arbNr, unsigned istv /*= 0*/) const {
  double sumMenge = 0.0;
  if (ObjTransportEinheit && (ObjTransportEinheit->Baustein () == (typBaustein*) this)
      && ObjTransportEinheit->hatArbeitsplan (arbNr, istv))   sumMenge += ObjTransportEinheit->Menge ();
  for (LCursor<StauDaten> c (ObjTeile); c; c++)
    if (c ()->TE->hatArbeitsplan (arbNr, istv))  sumMenge += c ()->TE->Menge ();
  return sumMenge;
}

/*virtual*/ double typStaufoerderer::MengeArtikelVorhanden
                   (unsigned artNr, unsigned istv /*= 0*/) const {
  double sumMenge = 0.0;
  if (ObjTransportEinheit && (ObjTransportEinheit->Baustein () == (typBaustein*) this)
      && ObjTransportEinheit->hatArtikel (artNr, istv))   sumMenge += ObjTransportEinheit->Menge ();
  for (LCursor<StauDaten> c (ObjTeile); c; c++)
    if (c ()->TE->hatArtikel (artNr, istv))  sumMenge += c ()->TE->Menge ();
  return sumMenge;
}

/*virtual*/ void typStaufoerderer::individuellAktivieren (typBaustein* vonWem) {
  if (!ObjTransportEinheit || (ObjZielBaustein == vonWem))  {
    typMeldung* zM = findeAusfuehrbareMeldung ();
    if (zM)  {
      ObjTransportEinheit = zM->TransportEinheit ();
      ObjZielBaustein = zM->VonBaustein ();
      wartenaufnehmen (ObjTransportEinheit, ObjZielBaustein);
    }
  }
}

typMeldung* typStaufoerderer::findeAusfuehrbareMeldung () {
  typTransportEinheit* zBuch = ObjPalPlaetze [0]->Buchung ();
  for (LCursor<typMeldung> c (ObjMeldungen); c; c++) {
    typTransportEinheit* zTE = c ()->TransportEinheit ();
    if ((!zBuch || (zTE == zBuch))
        && (ObjBelegung < ObjKapazitaet) && passtDrauf (zTE)) {
      if (!zBuch)  zTE->setZielPlatz (waehlePlatz (zTE));
      return &(ObjMeldungen.prepend (*(ObjMeldungen.remove (c))));
    }
  }
  return NULL;
}

/*virtual*/ bool typStaufoerderer::passtDrauf (typTransportEinheit *zTE) const {
  return (zTE->Laenge (ObjRichtung).len () <= (ObjLaenge - ObjStauGrenze)
          || (!ObjBelegung));
}

/*virtual*/ void typStaufoerderer::wartenaufnehmen (typTransportEinheit* zTE, typBaustein* vonWem) {
  if (vonWem->isUebergabeBereit (zTE, this)) {
    setZustand (AKTIVIERT);
    planesofort (ETaaufnehmen, ObjNummer, vonWem->Nummer ());
  }
  else  vonWem->UebergabeVorbereiten (zTE);
}

//---------------------------------------------------------
// Klasse typLinearStaufoerderer
//---------------------------------------------------------

typLinearStaufoerderer::typLinearStaufoerderer (unsigned n, unsigned b)
 : typStaufoerderer (n, b)
 { }

/*virtual*/ typLinearStaufoerderer::~typLinearStaufoerderer () {
}

/*virtual*/ void     typLinearStaufoerderer::typTTPlesen     (InputFile& file) {
  typStaufoerderer::typTTPlesen (file);
  ObjRichtung = ObjPalPlaetze [1]->Position () - ObjPalPlaetze [0]->Position ();
  ObjRichtung = ObjRichtung.unit ();
}

/*virtual*/ ostream& typLinearStaufoerderer::typTTPschreiben (ostream& ostrm) const {
  ostrm.precision (2);
  ostrm << "TTP> ";
  Id_Schreiben (ostrm);
  ostrm << " ";
  ostrm.width (6);
  ostrm << ObjAufnahmezeit.inMinutes () << " ";
  ostrm.width (6);
  ostrm << ObjLaenge << " ";
  ostrm.width (6);
  ostrm << ObjFoerderGeschw << " ";
  ostrm.width (3);
  ostrm << ObjKapazitaet << endl;
  return ostrm;
}

/*virtual*/ void typLinearStaufoerderer::positioniereAufAnfang (struct StauDaten& st) {
  st.LastPos = ObjLaenge - st.Len;
  st.TE->positionieren (ObjPalPlaetze [0]->Position () + ObjRichtung * (st.Len * 0.5));
  st.TE->drehen (ObjWinkel - st.TE->Winkel()); // raus fuer Perkins 07.2000
}

/*virtual*/ void typLinearStaufoerderer::positioniereAufEnde (struct StauDaten& st) {
  st.LastPos = 0.0;
  st.TE->positionieren (ObjPalPlaetze [1]->Position () - ObjRichtung * (st.Len * 0.5));
}
/*virtual*/ void typLinearStaufoerderer::positioniereTE (struct StauDaten& st) {
  st.LastPos = st.Pos;
  st.TE->positionieren (ObjPalPlaetze [1]->Position () - ObjRichtung * (st.Pos + (st.Len * 0.5)));
}

/*virtual*/ void typLinearStaufoerderer::bewegeTE (struct StauDaten& st, double weg) {
  st.LastPos += weg;
  st.TE->bewegen (ObjRichtung * weg);
}

//---------------------------------------------------------
// Klasse typRechtsKurvenStaufoerderer
//---------------------------------------------------------

typRechtsKurvenStaufoerderer::typRechtsKurvenStaufoerderer (unsigned n, unsigned b)
 : typStaufoerderer (n, b),
   ObjAnfangsWinkel (0),
   ObjEndWinkel     (0),
   ObjEndRichtung   ()
 { }

/*virtual*/ typRechtsKurvenStaufoerderer::~typRechtsKurvenStaufoerderer () {
}

/*virtual*/ void  typRechtsKurvenStaufoerderer::typTTPlesen (InputFile& file) {
  typStaufoerderer::typTTPlesen (file);
  Vec3D p1 (ObjPalPlaetze [0]->Position () - ObjGeometry.Pos ());
  Vec3D p2 (ObjPalPlaetze [1]->Position () - ObjGeometry.Pos ());
  ObjRichtung    = p1.cross (EZ).unit ();
  ObjEndRichtung = p2.cross (EZ).unit ();

  ObjAnfangsWinkel = EX.angle (Vec3D (p1.X (), p1.Y (), 0.0)); // nur in XY-Plane
  if (p1.Y () < 0.0) ObjAnfangsWinkel = Angle (360) - ObjAnfangsWinkel;
  ObjAnfangsWinkel -= Angle (90);
  ObjEndWinkel     = EX.angle (Vec3D (p2.X (), p2.Y (), 0.0));
  if (p2.Y () < 0.0) ObjEndWinkel     = Angle (360) - ObjEndWinkel;
  ObjEndWinkel -= Angle (90);

  Angle delta;
  if (ObjEndWinkel < ObjAnfangsWinkel) delta = (ObjAnfangsWinkel - ObjEndWinkel);
  else delta = (Angle (360) - ObjEndWinkel + ObjAnfangsWinkel);
  ObjLaenge = fabs (p1.len () * delta.rad ());
}

/*virtual*/ ostream& typRechtsKurvenStaufoerderer::typTTPschreiben (ostream& ostrm) const {
  ostrm.precision (2);
  ostrm << "TTP> ";
  Id_Schreiben (ostrm);
  ostrm << " ";
  ostrm.width (6);
  ostrm << ObjAufnahmezeit.inMinutes () << " ";
  ostrm.width (6);
  ostrm << ObjLaenge << " ";
  ostrm.width (6);
  ostrm << ObjFoerderGeschw << " ";
  ostrm.width (3);
  ostrm << ObjKapazitaet << endl;
  return ostrm;
}

/*virtual*/ void typRechtsKurvenStaufoerderer::positioniereAufAnfang (struct StauDaten& st) {
  st.LastPos = ObjLaenge - st.Len;
  Vec3D startPos (ObjPalPlaetze [0]->Position () - ObjGeometry.Pos ());
  Angle delta ((st.Len * 0.5) / startPos.len (), 1); // (sekantenLaenge / radius) Bogenmass!!!
  startPos *= ZrotHMatrx3D (-delta);
  st.TE->positionieren (ObjGeometry.Pos () + startPos);
  st.TE->ausrichten (ObjAnfangsWinkel - delta);
}

/*virtual*/ void typRechtsKurvenStaufoerderer::positioniereAufEnde   (struct StauDaten& st) {
  st.LastPos = 0.0;
  st.TE->positionieren (ObjPalPlaetze [1]->Position () - (ObjEndRichtung * (st.Len * 0.5)));
  st.TE->ausrichten (ObjEndWinkel);
}

/*virtual*/ void typRechtsKurvenStaufoerderer::positioniereTE (struct StauDaten& st) {
  if (st.Pos < (ObjLaenge - (st.Len * 0.5))) {
    Vec3D aktPos (st.TE->Position () - ObjGeometry.Pos ());
    Angle delta ((st.LastPos - st.Pos) / aktPos.len (), 1); // Bogenmass!!!
    aktPos *= ZrotHMatrx3D (-delta);
    st.TE->positionieren (ObjGeometry.Pos () + aktPos);
    st.TE->drehen (-delta);
  }
  else
    st.TE->positionieren (ObjPalPlaetze [1]->Position () - ObjRichtung * (st.Pos + (st.Len * 0.5)));
  st.LastPos = st.Pos;
}

/*virtual*/ void typRechtsKurvenStaufoerderer::bewegeTE (struct StauDaten& st, double weg) {
  if (st.LastPos - weg < (ObjLaenge - (st.Len * 0.5))) {
    Vec3D aktPos (st.TE->Position () - ObjGeometry.Pos ());
    Angle delta (weg / aktPos.len (), 1); // Bogenmass!!!
    aktPos *= ZrotHMatrx3D (-delta);
    st.TE->positionieren (ObjGeometry.Pos () + aktPos);
    st.TE->drehen (-delta);
  }
  else
    st.TE->bewegen (ObjRichtung * weg);
  st.LastPos -= weg;
}


//---------------------------------------------------------
// Klasse typLinksKurvenStaufoerderer
//---------------------------------------------------------

typLinksKurvenStaufoerderer::typLinksKurvenStaufoerderer (unsigned n, unsigned b)
 : typStaufoerderer (n, b),
   ObjAnfangsWinkel (0),
   ObjEndWinkel     (0),
   ObjEndRichtung   ()
 { }

/*virtual*/ typLinksKurvenStaufoerderer::~typLinksKurvenStaufoerderer () {
}

/*virtual*/ void  typLinksKurvenStaufoerderer::typTTPlesen (InputFile& file) {
  typStaufoerderer::typTTPlesen (file);
  Vec3D p1 (ObjPalPlaetze [0]->Position () - ObjGeometry.Pos ());
  Vec3D p2 (ObjPalPlaetze [1]->Position () - ObjGeometry.Pos ());
  ObjRichtung    = EZ.cross (p1).unit ();
  ObjEndRichtung = EZ.cross (p2).unit ();

  ObjAnfangsWinkel = EX.angle (Vec3D (p1.X (), p1.Y (), 0.0));
  if (p1.Y () < 0.0) ObjAnfangsWinkel = Angle (360) - ObjAnfangsWinkel;
  ObjAnfangsWinkel += Angle (90);
  ObjEndWinkel     = EX.angle (Vec3D (p2.X (), p2.Y (), 0.0));
  if (p2.Y () < 0.0) ObjEndWinkel     = Angle (360) - ObjEndWinkel;
  ObjEndWinkel += Angle (90);

  Angle delta;
  if (ObjAnfangsWinkel < ObjEndWinkel) delta = (ObjAnfangsWinkel - ObjEndWinkel);
  else delta = (Angle (360) - ObjAnfangsWinkel + ObjEndWinkel);
  ObjLaenge = fabs (p1.len () * delta.rad ());
}

/*virtual*/ ostream& typLinksKurvenStaufoerderer::typTTPschreiben (ostream& ostrm) const {
  ostrm.precision (2);
  ostrm << "TTP> ";
  Id_Schreiben (ostrm);
  ostrm << " ";
  ostrm.width (6);
  ostrm << ObjAufnahmezeit.inMinutes () << " ";
  ostrm.width (6);
  ostrm << ObjLaenge << " ";
  ostrm.width (6);
  ostrm << ObjFoerderGeschw << " ";
  ostrm.width (3);
  ostrm << ObjKapazitaet << endl;
  return ostrm;
}

/*virtual*/ void typLinksKurvenStaufoerderer::positioniereAufAnfang (struct StauDaten& st) {
  st.LastPos = ObjLaenge - st.Len;
  Vec3D startPos (ObjPalPlaetze [0]->Position () - ObjGeometry.Pos ());
  Angle delta ((st.Len * 0.5) / startPos.len (), 1); // (sekantenLaenge / radius) Bogenmass!!!
  startPos *= ZrotHMatrx3D (delta);
  st.TE->positionieren (ObjGeometry.Pos () + startPos);
  st.TE->ausrichten (ObjAnfangsWinkel + delta);
}

/*virtual*/ void typLinksKurvenStaufoerderer::positioniereAufEnde   (struct StauDaten& st) {
  st.LastPos = 0.0;
  st.TE->positionieren (ObjPalPlaetze [1]->Position () - (ObjEndRichtung * (st.Len * 0.5)));
  st.TE->ausrichten (ObjEndWinkel);
}

/*virtual*/ void typLinksKurvenStaufoerderer::positioniereTE (struct StauDaten& st) {
  if (st.Pos < (ObjLaenge - (st.Len * 0.5))) {
    Vec3D aktPos (st.TE->Position () - ObjGeometry.Pos ());
    Angle delta ((st.LastPos - st.Pos) / aktPos.len (), 1); // Bogenmass!!!
    aktPos *= ZrotHMatrx3D (delta);
    st.TE->positionieren (ObjGeometry.Pos () + aktPos);
    st.TE->drehen (delta);
  }
  else
    st.TE->positionieren (ObjPalPlaetze [1]->Position () - ObjRichtung * (st.Pos + (st.Len * 0.5)));
  st.LastPos = st.Pos;
}

/*virtual*/ void typLinksKurvenStaufoerderer::bewegeTE (struct StauDaten& st, double weg) {
  if (st.LastPos - weg < (ObjLaenge - (st.Len * 0.5))) {
    Vec3D aktPos (st.TE->Position () - ObjGeometry.Pos ());
    Angle delta (weg / aktPos.len (), 1); // Bogenmass!!!
    aktPos *= ZrotHMatrx3D (delta);
    st.TE->positionieren (ObjGeometry.Pos () + aktPos);
    st.TE->drehen (delta);
  }
  else
    st.TE->bewegen (ObjRichtung * weg);
  st.LastPos -= weg;
}

// typStaufoerderer
