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

#ifdef __BORLANDC__
#if (__BORLANDC__ < 0x0530)
#pragma option -w-sig
#else
#pragma option push -w-sig
#endif
#endif

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

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

#define _MSPS_CPP
#include "msps.h"

#include "sfx.h"
#include "ereignis.h"
#include "serialio.h"

static char Befehl[3];
static char AnBaustein [3];
static char BefehlParameter [4];
static char LesePuffer [64];
static int  PufferZeiger;
int anzbst;
// -----------------------------------------------------------
// --- Lokal benutzte Funktion:                            ---
// --- Uebersetzt die MSPS-Befehle in Simulator-Ereignisse ---
// -----------------------------------------------------------

int MspsZuETyp (int MspsBefehl) {
  switch (MspsBefehl) {
    case  1 : return ETaaufnehmen;
    case  2 : return ETeaufnehmen;
    case 30 : return ETaarbeiten;
    case 31 : return ETearbeiten;
    case 52 : return ETaabgeben;
    case 53 : return ETeabgeben;
    case 54 : return ETdpabgplatz;
    case  3 : return ETafoerdern;
    case  4 : return ETefoerdern;
    case  5 : return ETadrehen;
    case  6 : return ETedrehen;
    case 14 : return ETafahren;
    case 16 : return ETefahren;
    case 15 : return ETausschl;
    case 17 : return ETeausschl;
    case 36 : return ETaeinschl;
    case 37 : return ETeaufnehmen;
    case 63 : return ETbereit;
    case 60 : return ETainit;
    case 61 : return ETeinit;
    case 80 : return ETchtyp;
    default : return 0;
  }
}



// ----------------------------------------------------------
// --- Lokal benutzte Funktion:                           ---
// --- Uebersetzt die Simulatorereignisse in MSPS-Befehle ---
// ----------------------------------------------------------

int ETypZuMsps (int EreignisTyp) {
  switch (EreignisTyp) {
    case ETaaufnehmen : return 1;
    case ETeaufnehmen : return 2;
    case ETaarbeiten  : return 30;
    case ETearbeiten  : return 31;
    case ETaabgeben   : return 52;
    case ETeabgeben   : return 53;
    case ETdpabgplatz : return 54;
    case ETafoerdern  : return 3;
    case ETefoerdern  : return 4;
    case ETadrehen    : return 5;
    case ETedrehen    : return 6;
    case ETafahren    : return 14;
    case ETefahren    : return 16;
    case ETausschl    : return 15;
    case ETeausschl   : return 17;
    case ETaeinschl   : return 36;
    case ETbereit     : return 63;
    case ETainit      : return 60;
    case ETeinit      : return 61;
    case ETchtyp      : return 80;
    default           : return 0;
  }
}



// --------------------------------------------
// --- Lokal benutzte Prozedur zum Loeschen ---
// --- des Empfangspuffers                  ---
// --------------------------------------------

void LoescheLesePuffer (void) {
  int z;
  for (z = 0; z < 63; z ++)
    LesePuffer [z] = '\0';
  PufferZeiger = 0;
}



// ****************************************************************************

// ---------------------------
// --- Benutzerfunktionen  ---
// ---------------------------


// ------------------------------------------------------
// --- Initialisiert die und oeffnet Schnittstelle    ---
// ------------------------------------------------------

void MspsInit (void){
  Befehl[2]           = '\0';
  AnBaustein [2]      = '\0';
  BefehlParameter [3] = '\0';
  LoescheLesePuffer ();
   int zanzbst = 0;//Hilfsvariable für Handsteuerung
  // MSPS durchnumerieren und auf das Echo der
  // Durchnumerierung warten.
  SioSchreiben ("\n%01",4);
  while (SioZeichen != '%') SioLesen ();
  while (!SioLesen());
  zanzbst = (int)(SioZeichen)-48;
  while (!SioLesen());
  anzbst = ((int)(SioZeichen)+10*zanzbst-49);
  while (!SioLesen());
}


// ------------------------------------------------------
// --- Sendet eine Nachricht                          ---
// ------------------------------------------------------

void MspsSenden (int BausteinNr, int ETyp, int Parameter) {
  char NText [14];
  sprintf (AnBaustein, "%02i", BausteinNr);

  // --- Befehl vorbereiten ---
  sprintf (Befehl, "%02i", ETypZuMsps (ETyp));

  // --- Parameter vorbereiten ---
  sprintf (BefehlParameter, "%03i", Parameter);
  if (Parameter < 0)
     BefehlParameter [0] = '-';
  else
     BefehlParameter [0] = '+';

  // --- Befehl senden ---
  sprintf (NText, "%s,00,%s %s", AnBaustein, Befehl, BefehlParameter);
  SioSchreiben (NText, 12);
}

// ------------------------------------------------------
// --- Testet, ob eine Nachricht angekommen ist       ---
// ------------------------------------------------------

int MspsLesen (int *VonBausteinNr, int *ETyp, int *Parameter){
   int Befehl , i, ok;
   int AnBausteinNr;
   int NachrichtDa;
   char Zahl [3];
   NachrichtDa = 0;

   strcpy (Zahl, "000");
   while (!NachrichtDa  && SioLesen()) {
       LesePuffer [PufferZeiger] = SioZeichen;
       PufferZeiger ++;
       // Ist eine Nachricht komplett da?
       if (SioZeichen == 10) {
          // Testen, ob die Nachricht aus g|ltigen Zeichen besteht
          ok = 1;
          for (i=0; i<PufferZeiger-1; i++) {
            if ((LesePuffer[i]<' ') || (LesePuffer[i]>'9')) ok = 0;
            if (LesePuffer[i] == '%') ok=0;
          }
          if (ok) {
       
            Zahl [0] = ' ';
            Zahl [1] = LesePuffer [0];
            Zahl [2] = LesePuffer [1];
	        AnBausteinNr  = atoi (Zahl);
            // Ist die Nachricht auch f|r den Leitstand bestimmt?
            if (AnBausteinNr == 0) {
              NachrichtDa = 1;

              Zahl [0] = ' ';
              Zahl [1] = LesePuffer [3];
              Zahl [2] = LesePuffer [4];
	          *VonBausteinNr = atoi (Zahl);

              Zahl [0] = ' ';
              Zahl [1] = LesePuffer [6];
              Zahl [2] = LesePuffer [7];
	          Befehl  = atoi (Zahl);

              Zahl [0] = LesePuffer[9];
              Zahl [1] = LesePuffer [10];
              Zahl [2] = LesePuffer [11];
  	          *Parameter = atoi (Zahl);
              *ETyp = MspsZuETyp (Befehl);
            }
          }
          LoescheLesePuffer ();
       }
   }
   return NachrichtDa;
}


// ------------------------------------------------------
// --- Schliesst die zuvor geoeffnete Schnittstelle   ---
// --- wieder                                         ---
// ------------------------------------------------------

void MspsExit (void) {
  // ESC an Modelltechnik senden
  char EscZeichen = 27;
  SioSchreiben (&EscZeichen,1);
  while (SioZeichen != 27 ) SioLesen ();
  SioExit ();
}

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


// ---------------------------------------------------------------------
// --- Mit dem NC-Interpreter werden waehrend der Initialisierung    ---
// --- der Modelltechnik alle vom Modell benoetigten NC-Programme,   ---
// --- die nach DIN 66024/66025 erstellt sein muessen, ausgelesen,   ---
// --- interpretiert und binaer an die MSPS der Maschinen geschickt. ---
// ---------------------------------------------------------------------

//---------------------------------------------------------
// Klasse NCInterpreter
//---------------------------------------------------------

NCInterpreter::NCInterpreter ()
 { }


NCInterpreter::~NCInterpreter ()
 { }

void NCInterpreter::Linie_uebersetzen (float  uKoord, char *buf) {
  short a, l, h;

  a = (int) (uKoord * 80); // Umrechnung f|r CNC-Maschine: 1mm = 80 Einheiten
  h = a / 256;             // Highbyte der Koordinate
  l = a - (h * 256);	   // Lowbyte der Koordinate
  sprintf (buf, "%02X%02X", l, h);
} // Linie_uebersetzen


void NCInterpreter::Erz_Zeile (int g, char *Koor_x, char *Koor_y, char *Koor_z) {
  char weg [4];
  char zeile [255];

  if (g == 00) 
    sprintf (weg, "%02X", g);
  else 
    sprintf (weg, "%02X", 1);
    sprintf (zeile, "%02i,00,72 %s%s%s%s",bst_nr, weg, Koor_x, Koor_y, Koor_z);
    SioSchreiben(zeile, strlen(zeile));
} // Erz_Zeile

void NCInterpreter::Bogen_uebersetzen (int Weg, float xKoor2, float yKoor2, float /* zKoor */, float i_x, float i_y) {
  float  radius, M_x, M_y, winkel_a, winkel_e, winkel,teil_winkel, x_teil;
  float  y_teil, Anzahl, alpha_1, alpha_2, Grad, Teilung;
  int teil;
  Grad = 180 / S_PI;
  radius = Hypot (i_x, i_y);
  
  M_x = x_1 + i_x; M_y = y_1 + i_y;
//	if (i_x/radius>-1 || i_x/radius<1) 
  alpha_1 = Grad * (asin(i_x/radius));
  alpha_2 = Grad * (asin((xKoor2 - (x_1 + i_x))/radius));

  if (i_y<=0) 
    winkel_a = 90 + alpha_1;
  else 
    winkel_a = 270 - alpha_1;

  if (M_y<=yKoor2) 
    winkel_e = 90 - alpha_2;
  else 
    winkel_e = 270 + alpha_2;
	
  winkel = winkel_e - winkel_a;
  if (winkel>0 && Weg==2) 
    winkel = (360 - winkel) * (-1);
  else 
    if (winkel<0 && Weg==3) 
	  winkel = 360 + winkel;

  Teilung = radius / 5 * 12;
  if (Teilung<24) 
    Teilung = 24;
  Anzahl = winkel / 360 * Teilung;
  teil = (int)Anzahl;
  teil_winkel = winkel / teil;
  if (Weg==2) { teil_winkel = winkel_a - teil_winkel;}
  else {teil_winkel = winkel_a + teil_winkel;}
  for (int i=1; i<fabs(teil); i++) {
    x_teil = M_x + radius * cos(teil_winkel/Grad);
    y_teil = M_y + radius * sin(teil_winkel/Grad);
    if (Weg==2) 
      teil_winkel = teil_winkel - winkel/teil;
    else
      teil_winkel = teil_winkel + winkel/teil;
    Linie_uebersetzen(x_teil,hex_x);
    Linie_uebersetzen(y_teil,hex_y);
    Linie_uebersetzen(z,hex_z);
    Erz_Zeile(Weg, hex_x, hex_y, hex_z);
  } // for i<fabs(teil)
  Linie_uebersetzen(xKoor2, hex_x);
  Linie_uebersetzen(yKoor2, hex_y);
  Erz_Zeile(Weg, hex_x, hex_y, hex_z);
}


void NCInterpreter::auslesen () {
  InputFile* file = new InputFile (Datei);
  STDToken tok;

  tok.nextToken (*file);      //Zeilennummer lesen
  do {
    tok.nextToken (*file);    //Wegbedingung lesen
	(tok ()) [0] = ' ';        //Buchstaben von Zahl trennen
	w = atoi (tok ());
	if (w == 0 || w == 1 || w == 2 || w == 3) {
	  if (inkrement == 1) {
	  	xmerk = x;
	  	ymerk = y;
	  	zmerk = z;
	  	x = y = z = 0.0;
	  } //if(inkrement ==1)
	  tok.nextToken (*file);  //Koordinaten lesen
	  i = j = 0;
	  do {
	  	k = (tok ()) [0];
	  	(tok ()) [0] = ' ';
	  	switch (k) {
	      case 'X' : x = atof (tok ()); break;
          case 'Y' : y = atof (tok ()); break;
	      case 'Z' : z = atof (tok ()); break;
          case 'I' : i = atof (tok ()); break;
		  case 'J' : j = atof (tok ()); break;
	  	} // switch
        tok.nextToken (*file);  // naechsten Koordinaten lesen
	  } while ((tok ()) [0] != 'N');
	  if (inkrement == 1) {
	  	x += xmerk;
	  	y += ymerk;
	  	z += zmerk;
	  } //if (inkrement ==1)
	  if (w == 0 || w == 1) {
	  	Linie_uebersetzen (x, hex_x);
	  	Linie_uebersetzen (y, hex_y);
	  	Linie_uebersetzen (z, hex_z);
	  	Erz_Zeile (w, hex_x, hex_y, hex_z);
	  } else if (w == 2 || w == 3){
	  	Bogen_uebersetzen (w, x, y, z, i, j);
	  } //if (w==2 || w==3)
    } // (w==0 || w==1 || w==2 ||w==3)
	else if (w == 30) {
      fertig = 1;
	  sprintf (zeilen, "%02i,00,72 FF", bst_nr);
	  SioSchreiben(zeilen, strlen (zeilen));
	  //printf ("%s\n",zeilen);
	} else if (w == 90) {
	  inkrement = 0;
	  tok.nextToken (*file);
    } else if (w == 91) {
	  inkrement = 1;
	  tok.nextToken (*file);
    } // else if (w==91)
    x_1 = x; y_1 = y; z_1 = z;

  } while (!fertig);
  file->close ();
  delete file;
} // auslesen



void NCInterpreter::Uebersetzen (int Baustein, int ncprognr, char *verzname) {
  x = y = z = i = j = 0.0;
  xmerk = ymerk = zmerk = 0.0;
  w = inkrement = fertig = 0;
  bst_nr = Baustein;
  sprintf (zeilen, "%02i,00,32 %03i",bst_nr,ncprognr);
  SioSchreiben(zeilen,strlen(zeilen));
  sprintf (Datei, "%s/%03i.nc",verzname,ncprognr);
  auslesen();
}

#ifdef __BORLANDC__
#if (__BORLANDC__ < 0x0530)
#pragma option -wsig
#else
#pragma option pop
#endif
#endif

