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

#include "MathDefs.h"

#define _WEICHE_CPP
#include "modell.h"
#include "weiche.h"

#include "sfx.h"
#include "transp.h"
#include "baustein.h"
#include "route.h"
#include "ereignis.h"
#include "drehpuff.h"
#include "realzeit.h"

//---------------------------------------------------------
// Klasse typWeiche
//---------------------------------------------------------

typWeiche::typWeiche (unsigned n, unsigned b)
 : typBaustein            (n, b),
   ObjDrehGeschwindigkeit (90),
   ObjDrehSinn            (0),
   ObjZielWinkel          (0.0)
 { }

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

/*virtual*/ void typWeiche::typTGSlesen (InputFile& file) {
  typBaustein::typTGSlesen (file);
  TGBPermPattern* myTGBPattern = MySFXModel->TGBLibrary ()->findPerm (ObjBasistyp, ObjVariante);
  const DynArray<GeoObject>& go (myTGBPattern->AllGeometries ());
  go.First ();
  if (go.Next ()) {
    ObjDrehGeometry = new GeoObject ();
    GeoObject* geo = new GeoObject ();
    geo->copyContents (*(go ()));
    geo->setParent (*ObjDrehGeometry);
    geo->setSelectionToMum ();
    ObjDrehGeometry->setParent (ObjGeometry);
    ObjDrehGeometry->setSelectionToMum ();
    Vec3D v (ObjPalPlaetze[0]->Position ());
    //ObjDrehGeometry->setPos (ObjGeometry.PtToLocal (v), 0); // rausgenommen, da bei Position() des Platzes DrehGeometry+Position zurückgegeben wird
    ObjKorrekturWinkel = ObjWinkel;
  }
  if (ObjPalPlaetze.Count () != 1)
    sim_error ("WEICHE", "typWeiche::typTGSlesen",
               "Weichenvariante mit != 1 Palettenplatz", ObjVariante, FATAL);
}

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

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

  tok.nextToken (file);
  ObjDrehGeschwindigkeit = atof (tok ());
  if (ObjDrehGeschwindigkeit <= 0.0)
    sim_error ("WEICHE", "typWeiche::typTTPlesen",
               "Weiche mit unzulaessiger DrehGeschwindigkeit", ObjNummer, FATAL);

  ObjPhysKapazitaet = ObjKapazitaet = 1;

  file.jump (100); //EOL
}

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

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

/*virtual*/ void typWeiche::e_aufnehmen() {
  typPlatz *p (ObjTransportEinheit->AktPlatz ());
  if (p->Buchung () != ObjTransportEinheit)
    sim_error ("WEICHE", "typWeiche::e_aufnehmen",
               "TransportEinheit ist nicht auf Platz gebucht", ObjNummer, FATAL);
  p->setBuchung (NULL);
  ObjBuchung--;
  ObjTransportEinheit->positionieren (p->Position ());
//  ObjTransportEinheit->faerbeGestalt (default);

  ObjZielBaustein = ObjTransportEinheit->nextBaustein ();
  if (!ObjTransportEinheit->ZielPlatz () && (ObjEingaenge.Count () > 1))//08.05.00 rei
    ObjTransportEinheit->setZielPlatz (ObjZielBaustein->waehlePlatz (ObjTransportEinheit));

  if (ObjStoerDauer > SimTime (0.0))
    planesofort (ETastoerung, ObjNummer);
  else {
    setZustand (AKTIVIERT);
    ObjZielWinkel = UebergabeWinkel (ObjZielBaustein->UebernahmePos (*this));
    if (ObjZielWinkel != ObjWinkel)
      planesofort (ETadrehen, ObjNummer, ObjZielBaustein->Nummer ());
    else {
      ObjZielBaustein = NULL;
      planesofort (ETameldewarten, ObjNummer, 0);
    }
  }
  planesofort (ETeabgeben, ObjTransportEinheit->Meldung ()->VonBaustein ()->Nummer (), ObjNummer);
}

/*virtual*/ void typWeiche::dreh_takt () {
  if (ObjZustand != DREHEN) return;
  if (ObjTaktSchritte > 0) {
    ObjTaktSchritte--;
    drehEinStueck (Angle (ObjDrehSinn * ClassDrehSchritt));
    planedelta (ObjTaktZeit, ETdrehTakt, ObjNummer, ObjZielBaustein->Nummer (), ObjTaktSchritte);
  }
  else drehEinStueck (ObjZielWinkel - ObjWinkel);
  if (MySFXModel->Debug ())
    dprintf ("Drehtakt %1d: w=%5.2f\n", ObjNummer, ObjWinkel.deg ());
}

/*virtual*/ void typWeiche::a_drehen () {
  double drehWinkel = (ObjZielWinkel - ObjWinkel).deg ();
  if (drehWinkel > 180)  drehWinkel -= 360;
  else if (drehWinkel < -180)  drehWinkel += 360;
  ObjDrehSinn = Sign (drehWinkel);
  double drehZeit = fabs (drehWinkel) / ObjDrehGeschwindigkeit;
  setZustand (DREHEN);
  if (MySFXModel->RealTime ())
    RzsSenden (ObjNummer, ETadrehen, ((int) Round (drehWinkel)) / 90);
  planedelta (drehZeit, ETedrehen, ObjNummer, ObjZielBaustein->Nummer (), ((int) Round (drehWinkel)));

  if (MySFXModel->Cycle ()) {
    ObjTaktSchritte = (unsigned) ((fabs (drehWinkel) - (ClassDrehSchritt / ClassTaktFaktor)) / ClassDrehSchritt);
    if (MySFXModel->Debug ())
      dprintf ("Drehen %3d: drehWinkel=%8.3f Schritte=%1d\n",
                             ObjNummer, drehWinkel, ObjTaktSchritte);
    if (ObjTaktSchritte > 0) {
      ObjTaktZeit = (ClassDrehSchritt / ObjDrehGeschwindigkeit) * SimTime::SecondsPerMinute ();
      if (MySFXModel->Debug ())
        dprintf ("            drehZeit=%8.5f TaktZeit=%8.5f\n",
                              drehZeit, ObjTaktZeit.inMinutes ());
      dreh_takt ();
    }
  }
  if (((!MySFXModel->Cycle ()) || (ObjTaktSchritte == 0)) && (drehWinkel != 0.0))
    drehAufWinkel (ObjZielWinkel);
}

/*virtual*/ void typWeiche::e_drehen () {
  if (MySFXModel->Cycle ())  drehAufWinkel (ObjZielWinkel);
  ObjZielBaustein = NULL;

  if (ObjStoerDauer > SimTime (0.0))
    planesofort (ETastoerung, ObjNummer);
  else {
    setZustand (AKTIVIERT);
    if (ObjPalPlaetze [0]->TransportEinheit ())
      planesofort (ETameldewarten, ObjNummer, 0);
    else if (!ObjMeldungen.isEmpty ())
      planesofort (ETaktivieren, ObjNummer, ObjNummer);
    else {
      setZustand (RUHE);
      aktiviereEingaenge ();
    }
  }
}

/*virtual*/ void typWeiche::e_abgeben () {
  if (ObjEingaenge.Count () == 1) {
    ObjZielBaustein = ObjEingaenge.First ();
    ObjZielWinkel = UebernahmeWinkel (ObjZielBaustein->UebergabePos (*this));
    if (ObjZielWinkel != ObjWinkel) {
      setZustand (AKTIVIERT);
      planeein (Modellzeit (), ETadrehen, ObjNummer, ObjZielBaustein->Nummer ());
      return;
    }
  }
  setZustand (RUHE);
  planesofort (ETaktivieren, ObjNummer, ObjNummer);
  aktiviereEingaenge ();
}

/*virtual*/ void typWeiche::eStoeren () {
  ObjStoerEnde = NULL;
  if (ObjStoerGeschichte) ObjStoerGeschichte->erfassen (0.0);
  switch (ObjAlterZustand) {
    case AUFNEHMEN :  setZustand (AKTIVIERT);
                      ObjZielWinkel = UebergabeWinkel (ObjZielBaustein->UebernahmePos (*this));
                      if (ObjZielWinkel != ObjWinkel)
                        planesofort (ETadrehen, ObjNummer, ObjZielBaustein->Nummer ());
                      else {
                        ObjZielBaustein = NULL;
                        planesofort (ETameldewarten, ObjNummer, 0);
                      }
                      break;

    case DREHEN    :  setZustand (AKTIVIERT);
                      if (ObjPalPlaetze [0]->TransportEinheit ())
                        planesofort (ETameldewarten, ObjNummer, 0);
                      else if (!ObjMeldungen.isEmpty ())
                          planesofort (ETaktivieren, ObjNummer, ObjNummer);
                      else  setZustand (RUHE);
                      break;

    default        :  setZustand (ObjAlterZustand); // RUHE or POLLING!!!
                      planesofort (ETaktivieren, ObjNummer, ObjNummer);
                      aktiviereEingaenge ();
                      aktiviereAusgaenge ();
                      break;
  }
}

/*virtual*/ bool typWeiche::ausrichtenAufBaustein (typBaustein* zielBst) {
  if (ObjZustand == RUHE) {
    ObjZielBaustein = zielBst;
    ObjZielWinkel = UebernahmeWinkel (ObjZielBaustein->UebergabePos (*this));
    if (ObjZielWinkel != ObjWinkel) {
      setZustand (AKTIVIERT);
      planesofort (ETadrehen, ObjNummer, ObjZielBaustein->Nummer ());
    }
    return true;
  }
  return false;

}

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

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

/*virtual*/ Vec3D typWeiche::UebernahmePos (const typBaustein&) const {
  return (ObjGeometry.Pos ());
}

/*virtual*/ Vec3D typWeiche::UebergabePos (const typBaustein&) const {
  return (ObjGeometry.Pos ());
}

/*virtual*/ void typWeiche::individuellAktivieren (typBaustein* vonWem) {
  if (((ObjZustand == RUHE)  && (!ObjMeldungen.isEmpty ()))
      || ((ObjZustand == WARTEDP) && (ObjZielBaustein == vonWem))
      || (vonWem == this) || (ObjZustand == POLLING))  {
    typMeldung *zM = findeAusfuehrbareMeldung ();
    if (zM) {
      ObjTransportEinheit = zM->TransportEinheit ();
      ObjZielBaustein = zM->VonBaustein ();
      ObjZielWinkel = UebernahmeWinkel (ObjZielBaustein->UebergabePos (*this));
      if (ObjZielWinkel != ObjWinkel) {
        setZustand (AKTIVIERT);
        planesofort (ETadrehen, ObjNummer, ObjZielBaustein->Nummer ());
      }
      else  wartenaufnehmen (ObjTransportEinheit, ObjZielBaustein);
    }
    else
      if (!(ObjMeldungen.isEmpty ())) {
        setZustand (POLLING);
      }
  }
}

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

void typWeiche::drehEinStueck (const Angle& deltaw) {
  drehen (deltaw);
  if (ObjPalPlaetze [0]->TransportEinheit ()) {
    if (ObjPalPlaetze [0]->Position () != ObjGeometry.Pos ())
      ObjTransportEinheit->positionieren (ObjPalPlaetze [0]->Position ());
    ObjTransportEinheit->drehen (deltaw);
  }
}

void typWeiche::drehAufWinkel (const Angle& absAngl) {
  Angle oldWinkel (ObjWinkel);
  ausrichten (absAngl);
  if (ObjPalPlaetze [0]->TransportEinheit ()) {
    if (ObjPalPlaetze [0]->Position () != ObjGeometry.Pos ())
      ObjTransportEinheit->positionieren (ObjPalPlaetze [0]->Position ());
    ObjTransportEinheit->ausrichten (absAngl +  ObjTransportEinheit->Winkel () - oldWinkel);
  }
}

Angle typWeiche::UebernahmeWinkel (const Vec3D& vonPos) {
//  Vec3D v (vonPos.X (), vonPos.Y (), ObjGeometry.Pos ().Z ());
//  v = ObjGeometry.Pos () - v;
  Vec3D v (ObjGeometry.Pos () - vonPos);
  Angle ang (EX.angle (v.scale (Vec3D (1.0, 1.0, 0.0))));
  return (v.Y () < 0.0) ? (Angle (360) - ang) : ang;
  
}

Angle typWeiche::UebergabeWinkel (const Vec3D& nachPos) {
  //Vec3D v (nachPos.X (), nachPos.Y (), ObjGeometry.Pos ().Z ());
  //v -= ObjGeometry.Pos ();
  Vec3D v (nachPos - ObjGeometry.Pos ());
  Angle ang (EX.angle (v.scale (Vec3D (1.0, 1.0, 0.0))));
  return (v.Y () < 0.0) ? (Angle (360) - ang) : ang;
}
