/* $Id: gtoisel.cpp 2.5 2002/05/23 14:49:57 leon Exp $

  M-ukazi ne delujejo, ce so kot argumenti
 */

#include <iostream.h>

#include <ctype.h> // tolower()

#include <fstream.h>

#include <math.h>

#include <stdlib.h>

#include "parse.h"

#include "machinin.h"


#define LEFT_HANDED 0x04


long TotalLines = 0;

Machining Machining;

#ifdef CMDLINE


typedef unsigned short ushort;

typedef struct settingsRec  {
  long spuX;     //TInputLong

  long spuY;     //TInputLong

  long spuZ;     //TInputLong

  long speedX;   //TInputLong

  long speedY;   //TInputLong

  long speedZ;   //TInputLong

  long device;   //TInputLong

  ushort ack;    //TRadioButtons

  long stepSize; //TInputLong

  long ratioZ;   //TInputLong

  ushort option; //TCheckBoxes LeftHanded is default for cmdline

  char logFileName[57];   //TInputLine

  } SettingsRec;


typedef struct homingSpeedRec  {
  long x;   //TInputLong

  long y;   //TInputLong

  long z;   //TInputLong

  } HomingSpeedRec;


SettingsRec localSettings = { 80, 80, 80, 500, 500, 500, 0, 0, 0, 0, 
                              LEFT_HANDED};
SettingsRec *Settings = &localSettings;


int main (int argc, char *argv[])
{
  extern FILE *yyin;
  
  TotalLines = 0;

  if(argc > 1 && (yyin = fopen(argv[1], "r")) == NULL) 
    {
      perror(argv[1]);
      exit(1);
    }
  
  Machining.ReadINI("gisel.ini");
  if(!yyparse ())
      clog <<  "G-CODE parse OK" << endl;
  else
     clog << "G-CODE parse NOT OK" << endl;
  cout.flush();
  return 0;
}

int yyerror(char *str) // C function

{
  extern long int lineno;
  extern char linebuf[200];

  clog << "Error: '" << str << "' at line " << lineno << '\n';
  clog << linebuf << endl;
  return 0;
}

#else /* CMDLINE */


#  include "gisel.h"


extern SettingsRec *Settings;

#endif /* CMDLINE */


// Some usefull macros

#define SQR(x) ((x)*(x))

#define SGN(x) ((x) >= 0 ? 1 : -1)

#define QUADRANT(x, y) ((((x) * (y)) < 0 ? 1 : 0) + ((y) < 0 ? 2 : 0)\

                         + ((y) == 0 ? (((x) > 0) ? 3 : 1) : 0))
#define CUTOFF(x) ( (x) > 1.0 ? 1.0 : ((x) < -1.0 ? -1.0 : (x)))

#define LENGTH(x,y) sqrt((x)*(x)+(y)*(y))

#define ANGLE(x,y) ( (y) < 0.0 ? 2.0*M_PI - acos(CUTOFF((x)/LENGTH(x,y))) \

                            : acos(CUTOFF((x)/LENGTH(x,y))) )
#define DEG(rad) (180.0*(rad)/M_PI)

#define RAD(deg) (M_PI*(deg)/180.0)

#define IVAL node->arg.ival

#define DVAL node->arg.dval

#define COORD(coord, spu) long((coord) * Settings->spu)

#define SPEED(spu) long(Fedrate * Settings->spu / 60.0)

#define ALMOSTZERO(x) (((x) < 1e-2) && ((x) > -1e-2))

#define IMMEDIATE(c) '@' << Settings->device << acknowledge(c)



/**
 ** Returns upper or lower case letter
 **/
inline char acknowledge(char c)
{
  return Settings->ack ?  toupper(c) : tolower(c);
}


/**
 ** Position handling
 **/
class Position 
{
public:
   double x, y, z;      // position

   Position(double x, double y, double z);
   Position(const Position& p); 
   void clear() { x = y = z = 0.0; }
   int operator == (const Position& p) 
	{ return p.x == x &&  p.y == y && p.z == z; }
};

/**
 ** Creates default position
 **/
Position::Position(const Position& p)
{
   x = p.x;
   y = p.y;
   z = p.z;
}

/** Copy constructor **/
Position::Position(double x, double y, double z)
{
  Position::x = x;
  Position::y = y;
  Position::z = z;
}

typedef struct {
  double r;       // radius

  double a;       // starting angle

  double b;       // ending angle

  } CircleArgs;

static Position Pos(0.0, 0.0, 0.0);          /* current position */
static Position ToPos(0.0, 0.0, 0.0);        /* move To position */
static Position Center(0.0, 0.0, 0.0);       /* circle center    */
static double Fedrate = 400.0;
static CircleArgs CircleArg = { 0.0, 0.0, 0.0 };
static short Axes = 7;
static int CmdNumber = 0;                    /* Number for the M, S, T */
enum {ABSOLUTE, RELATIVE} MovingType = ABSOLUTE;
enum {COUNTERCLOCKWISE = -1, CLOCKWISE = 0 } Direction;
enum {RAB, IJK} CircleType;

/**
 ** Outputs default coordinate for use by absolute/relative move
 **/
ostream & operator << (ostream& s, Position &move)
{
  s << COORD(move.x, spuX) << "," << SPEED(spuX) << ","
    << COORD(move.y, spuY) << "," << SPEED(spuY) << ","
    << COORD(Settings->option | LEFT_HANDED ? -move.z : move.z, spuZ)
    << "," << SPEED(spuZ) << ",0,30";
  return s;
}


/**
 ** Machining generation for user outputs
 **/
static void
executeMachining(char type, int number)
{
   char *cmd = Machining.GetCommand(type, number);

   if (cmd == NULL)
     {
       extern long int lineno;
       clog << "Warning at line " << lineno << ": Machining type "
            << type << number << " not defined." << endl;
     }
   else
     {
        cout << "@" << Settings->device << "i\n";
        cout << cmd << "\n9\n";
        cout << "@" << Settings->device << "s\n";
        TotalLines += 3;
     }

}

/**
 ** Fills arguments into structures
 **/
static Node *fillArgs(Node *node)
{
  while (node)
    {
      switch (node->type)
        {
#define SINGLE(var, val) var = MovingType == ABSOLUTE ? val : var += val;

          case 
	    Node_X:
            ToPos.x = MovingType == ABSOLUTE ? DVAL : Pos.x + DVAL;
            break;
          case 
	    Node_Y:
            ToPos.y = MovingType == ABSOLUTE ? DVAL : Pos.y + DVAL;
            break;
          case 
	    Node_Z:
            ToPos.z = MovingType == ABSOLUTE ? DVAL : Pos.z + DVAL;
            break;
          case 
	    Node_I:
            Center.x = MovingType == ABSOLUTE ? DVAL : Pos.x + DVAL;
            CircleType = IJK;
            break;
          case 
	    Node_J:
            Center.y = MovingType == ABSOLUTE ? DVAL : Pos.y + DVAL;
            CircleType = IJK;
            break;
          case 
	    Node_K:
            Center.z = MovingType == ABSOLUTE ? DVAL : Pos.z + DVAL;
            CircleType = IJK;
            break;
          case 
	    Node_R:  /* radius MAHO ?*/
            CircleArg.r = DVAL;
            CircleType = RAB;
            break;
          case 
	    Node_A:  /* start angle ??? */
            CircleArg.a = DVAL;
            CircleType = RAB;
            break;
          case 
	    Node_B:  /* end angle ??? */
            CircleArg.b = DVAL;
            CircleType = RAB;
            break;
          case
            Node_F:
            Fedrate = DVAL;
            break;
          case 
	    Node_MCODE:
            executeMachining('M', DVAL);
            break;
          case
            Node_S:
            executeMachining('S', DVAL);
            break;
          case
            Node_T:
            executeMachining('T', DVAL);
            break;
          case 
	    Node_EndCmd:
            return node->next;
          default:
            extern long int lineno;
            clog << "Warning at line "<< lineno 
		 << ": Unexpected argument type '"
                 << node->type << "'" <<  endl;
            break;
        }
      node = node->next;
    }
  return NULL;
}




/** Pause for the green button */
static void m00(Node *node) 
{
  cout << "@" << Settings->device << "i\n";
  cout << "44\n9\n";
  cout << "@" << Settings->device << "s\n";
  fillArgs(node);  // to ni najbolj prav

  TotalLines += 3;
}


static void
executeMCODE(Node *node)
{
   clog << "M" << node->arg.ival << endl;
   switch (node->arg.ival)
     {
     case 00:  m00(node->next); break;
     case 30:  /* do nothing */; break;
    default:
       extern long int lineno;
	     clog << "Warning at line " << lineno << ": M-CODE type M"
            << node->arg.ival << " not defined." << endl;
	     break;
     }

}





/**  working move **/
static void g01(Node *node) 
{
  while (node)
    {
      node = fillArgs(node);
	  if (!(Pos == ToPos)) // do not move if no change in position 

	  {
	      cout << IMMEDIATE('m') << ToPos << '\n';	
          Pos = ToPos;
	  }
      TotalLines++;
    }
}

/** fast move **/
static void g00(Node *node) 
{
  double oldFeedrate = Fedrate;
  Fedrate = 3000.0; // TODO

  g01(node);
  Fedrate = oldFeedrate;
}


/**
 ** Error factor for the circular interpolation
 **/
static double errorFactor(float Rx, float Ry, double radius, double Xs, double Ys)
{
#if 0

  // this is by the ISEL German book, but false

  #define SUMME(xx) ( xx > 0 ? (xx)*((xx) + 1) : (xx)*((xx) - 1))

  if (Direction == COUNTERCLOCKWISE)
    return (Rx * Ry * radius + Rx * Ry * SUMME(radius - 1)
          - Rx * SUMME(Xs + (Rx - Ry)/2.0) 
          + Ry * SUMME(Ys + (Rx + Ry)/2.0))/2.0;
  else
    return(- Rx * Ry * radius 
           - Rx * Ry * SUMME(radius - 1)
           - Rx * SUMME(Xs + (Rx + Ry)/2.0) 
           + Ry * SUMME(Ys + (Ry - Rx)/2.0))/2.0;
#else

  // by the English ISEL book

  double error = Rx*Ry*(Xs*(Xs-Rx) + Ys*(Ys-Ry) - SQR(radius))/2;
  if (Direction == CLOCKWISE)
    return -error;

  return error;
#endif

}

/**
 ** Calculates path absolute length
 **/
static double winding(double angle)
{
  double qlen = floor(angle/M_PI_2);  // quadrant length

  double sabs = fabs(sin(angle));     // sinus length

  if (short(qlen) % 2)                // invert even quadrants

    qlen += 1 - sabs;
  else
    qlen += sabs;

  return qlen;
}

/**
 ** Total number of steps for the circular interpolation
 **/
static double circleSteps(double radius, double A, double B)
{
  double wx, wy;

//		  clog << "Circle A:" <<  A << " B:" <<  B << endl;


  if (Direction == CLOCKWISE)
    {
      if (ALMOSTZERO((A-B)*radius))
	{

	  A += 2*M_PI;
          extern long int lineno;
	  clog << "Warning: Ambigious circle " << A << " == " << B << " at line " 
		<< lineno << ". Doing full circle!" << endl;
	}

      if (A < B)
        A += 2 * M_PI;
      wy = winding(A) - winding(B);
      wx = winding(A + M_PI_2) - winding(B + M_PI_2);
    }
  else
    {
      if (ALMOSTZERO((A-B)*radius))
	{
	  B += 2*M_PI;
          extern long int lineno;
	   clog << "Warning: Ambigious circle " << A << " == " << B << " at line " 

		<< lineno << ". Doing full circle!" << endl;
	}


      if (B < A)
        B += 2 * M_PI;
      wy = winding(B) - winding(A);
      wx = winding(B + M_PI_2) - winding(A + M_PI_2);
    }

  return radius * (wx + wy);
}

/**
 ** Clockwise G02 and Counterclockwise G03 circular interpolation
 ** XY Plane only -> other planes TODO
 **/
static void circle(Node *node) 
{
  double Xs, Ys; // relative coordinate system starting point

  double radius; // circle radius;

  double A, B;   // Starting and ending angle;


  node = fillArgs(node);

  if (CircleType == RAB)
    {
      radius = CircleArg.r * Settings->spuX;
      A = RAD(CircleArg.a);
      B = RAD(CircleArg.b);
      Xs = CircleArg.r * cos(A) * Settings->spuX;
      Ys = CircleArg.r * sin(A) * Settings->spuY;
      Pos.x = -CircleArg.r * cos(A) + CircleArg.r * cos(B);
      Pos.y = -CircleArg.r * sin(A) + CircleArg.r * sin(B);
    }
  else // IJK

    {
      double Ex, Ey; // end point


      Xs = (Pos.x - Center.x) * Settings->spuX; // Starting point

      Ys = (Pos.y - Center.y) * Settings->spuY;   
      // Ex = ToPos.x * Settings->spuX ; // end point (X, Y)

      // Ey = ToPos.y * Settings->spuY ;  

      radius = sqrt(SQR((Pos.x-Center.x)*Settings->spuX) 
		    + SQR((Pos.y-Center.y)*Settings->spuY));
      if (ALMOSTZERO(radius))
	{
  	   extern long int lineno;
	   clog << "Warning: Circle at line " << lineno
	        << " has too small radius. Ignoring!" << endl;
	   return;
	}
      if (!ALMOSTZERO(LENGTH(Pos.x-Center.x, Pos.y-Center.y) 
	       - LENGTH(ToPos.x-Center.x, ToPos.y-Center.y)))
	{
           extern long int lineno;
	   clog << "Warning: Inconsistent circle at line " 
		<< lineno << ". Ignoring!" << endl;
	   return;
        }
      A = ANGLE(Pos.x - Center.x, Pos.y - Center.y);
      B = ANGLE(ToPos.x - Center.x, ToPos.y - Center.y);
    }


  Xs = floor(Xs+0.5);
  Ys = floor(Ys+0.5);
  radius = floor(radius+0.5);
  // we now have integer values


  long Rx;
  long Ry;
  if (Direction == CLOCKWISE)
   {
//	  clog << "Xs = " << Xs << " Ys = " << Ys << endl;

	  Rx = ALMOSTZERO(Ys) ? -SGN(Xs) :  SGN(Ys);
      Ry = ALMOSTZERO(Xs) ? -SGN(Ys) : -SGN(Xs);
//	  clog << "Rx = " << Rx << " Ry = " << Ry << endl;

   }
  else
    {
      Rx = ALMOSTZERO(Ys) ? -SGN(Xs) : -SGN(Ys);
      Ry = ALMOSTZERO(Xs) ? -SGN(Ys) :  SGN(Xs);
    }


  cout << IMMEDIATE('f') << Direction << '\n';

  cout << IMMEDIATE('y') 
    << long(circleSteps(radius, A, B)) << ","
    << SPEED(spuX) << ","
    << long(errorFactor(Rx, Ry, radius, Xs, Ys)) << ","
    << long(Xs) << "," << long(Ys) << "," 
    << Rx << "," << Ry << '\n'; 

  Pos = ToPos;
  TotalLines += 2;
}

/** Set 3D interpolation ON **/
static void g20(Node *node) 
{
  cout << IMMEDIATE('z') << "1\n";
  fillArgs(node);
  TotalLines++;
}

/** Set Absolute Zero **/
static void g52(Node *node) 
{
  cout << IMMEDIATE('n') << Axes << '\n';
  Pos.clear();
  fillArgs(node);
  TotalLines++;
}

/**  Home motors  **/
static void g74(Node *node) 
{
  cout << IMMEDIATE('r') << 7 << '\n'; // TODO  which motors

  fillArgs(node);
  TotalLines++;
}

/** Set Absolute moving **/
static void g90(void)
{
  MovingType = ABSOLUTE;
}
/**  Set Relative moving **/
static void g91() 
{
  MovingType = RELATIVE;
}

/** Set Homing speed **/
static void g94(Node *node) 
{
  long hs = long(DVAL * Settings->spuX / 60.0);
  cout << IMMEDIATE('d')
        << hs << "," << hs << "," << hs << '\n';
  fillArgs(node);
  TotalLines++;
}


/** This  is Callback function called by yyparse() **/
void ExecCommandCb(Node *node)
{
// #define CHECKONLY

#ifndef CHECKONLY

// #define LISTNODES

#ifdef LISTNODES

  while (node)
    {
      if (node->type >= 100)
        cout << "Node :" << node->type << " Ival:" << node->arg.ival << endl;
      else
        cout << "Node :" << node->type << " Dval:" << node->arg.dval << endl;
      node = node->next;
    }
    cout << "---------------" << endl;
#else

   switch (node->type)
    {
      case Node_GCODE:
		 
        switch (node->arg.ival)
          {
          case 00:  g00(node->next); break;
          case 01:  g01(node->next); break;
          case 02:  
		          Direction = CLOCKWISE; 
		          circle(node->next); 
		          break;
          case 03:  
		          Direction = COUNTERCLOCKWISE; 
		          circle(node->next); 
		          break;
          case 20:  g20(node->next); break;
          case 52:  g52(node->next); break;
          case 74:  g74(node->next); break;
          case 90:  g90(); break;
          case 91:  g91(); break;
          case 94:  g94(node->next); break;
		    
  	  default: 
        extern long int lineno;
		    clog << "Warning at line " << lineno 
		         << ": G-CODE type G" <<
            node->arg.ival  << " not yet implemented." << endl;
		    break;
          }
        break;
      case Node_MCODE:
        executeMCODE(node);
        break;
    }
#endif // LISTNODES

#endif // CHECKONLY

}



syntax highlighted by Code2HTML, v. 0.9.1