/***************************************************************************/
/*  CNC-drilling maschine with 3 stepper motors on parallel interface      */
/***************************************************************************/
/*  DJGPP for DOS:   set GO32=driver /gcc/drivers/stdvga.grn ansi          */
/*                   gcc cnc.c -o cnc.out -Wall -lpc -lgrx -lm             */
/*  Str,1998-2001    coff2exe -s go32.exe %1.out                           */
/***************************************************************************/
/*  connections: bits 5..0: Z-DIR, Z-CLK, Y-DIR, Y-CLK, X-DIR, X-CLK       */ 
/*  commands: a character for each step, undefined chars are ignored       */
/*  'XYZ': increase (X=table left, Y=table to front, Z=tool down)          */
/*  'xyz': decrease, '1234': 45 degree move XY/xY/xy/Xy, '0': nop          */
/***************************************************************************/

#include <stdio.h>
#include <stdarg.h>
#include <gppconio.h>
#include <ctype.h>
#include <dos.h>
#include <grx.h>

#define  COLORS    16
#define  WIDTH     640
#define  HIGHT     480
#define  BLACK     0
#define  RED       1
#define  GREEN     2
#define  BLUE      3
#define  YELLOW    4
#define  CYAN      5
#define  MAGENTA   6
#define  ORANGE    7
#define  WHITE     15
#define  XRES      96       /* resolution: steps/mm */
#define  YRES      96
#define  ZRES      56
#define  XMAX      (176*XRES)   /* max distance: mm */
#define  YMAX      (116*YRES)
#define  ZMAX      (14*ZRES)
#define  XSCL      (XRES/3)  /* steps/display pixel */
#define  YSCL      (YRES/3)
#define  ZSCL      (ZRES/3)
#define  MRG       8                 /* Left margin */
#define  TOP       4                  /* Top margin */
#define  GROOF     2.0           /* z-movement hpgl */
#define  PUNCH     3.0       /* z-movement excellon */
#define  SPEED     5                   /* msec/step */
#define  OFF       0x80

int port = 0;
int trace = BLUE;
int items = 0;
float mtime = 0.0;
float stepw = 0.01;
int xpos, ypos, zpos;
int xofs, yofs, zofs;
float curx, cury, curz;
int scode[4] = {0x09, 0x0A, 0x06, 0x05};
int down = 0;
GrTextOption txtopt = { NULL, 1, 1, 0 };

int printfXY(int x, int y, char *fmt, ...)
{ va_list ap;
  int cnt;
  char buf[256];
  va_start(ap, fmt);
  cnt = vsprintf(buf, fmt, ap);
  va_end(ap);
  txtopt.txo_fgcolor.v = WHITE;
  GrDrawString(buf, cnt, x, y, &txtopt);
  return cnt;
}

void message(char *fmt, ...)
{ va_list ap;
  int cnt;
  char buf[256];
  GrFilledBox(0, 463, WIDTH-1, HIGHT-1, BLACK);
  va_start(ap, fmt);
  cnt = vsprintf(buf, fmt, ap);
  va_end(ap);
  txtopt.txo_fgcolor.v = YELLOW;
  GrDrawString(buf, cnt, MRG, 463, &txtopt);
}

void GrafMode(void)
{
  GrSetMode(GR_width_height_color_graphics, WIDTH, HIGHT, COLORS);
  GrSetColor(BLACK, 0, 0, 0);
  GrSetColor(RED, 255, 0, 0);
  GrSetColor(GREEN, 0, 220, 0);
  GrSetColor(BLUE, 0, 0, 255);
  GrSetColor(YELLOW, 200, 200, 0);
  GrSetColor(CYAN, 0, 200, 200);
  GrSetColor(MAGENTA, 200, 0, 200);
  GrSetColor(ORANGE, 200, 100, 0);
  GrSetColor(WHITE, 200, 200, 200);
  GrFilledBox(0, 0, WIDTH-1, HIGHT-1, BLACK);
  printfXY(MRG, TOP, "calibrating! please wait...");
  printfXY(MRG, 408, "X        mm        inch");
  printfXY(MRG, 426, "Y        mm        inch");
  printfXY(MRG, 444, "Z        mm        inch");
  printfXY(250, 408, "U Up        mm", GROOF);
  printfXY(250, 426, "D Down      mm", GROOF);
  printfXY(250, 444, "! Drill     mm", PUNCH);
  printfXY(318, 408, "%3.1f", GROOF);
  printfXY(318, 426, "%3.1f", GROOF);
  printfXY(318, 444, "%3.1f", PUNCH);
  printfXY(390, 408, "E Excellon-File");
  printfXY(390, 426, "H HPGL-File");
  printfXY(390, 444, "P Park");
  printfXY(530, 408, "I Init");
  printfXY(530, 426, "O Origin");
  printfXY(530, 444, "R Reset");
}

void Coords(void)
{
  printfXY(MRG+12, 408, "%7.2f", curx);
  printfXY(MRG+100, 408, "%6.3f", curx/25.4);
  printfXY(MRG+12, 426, "%7.2f", cury);
  printfXY(MRG+100, 426, "%6.3f", cury/25.4);
  printfXY(MRG+12, 444, "%7.2f", curz);
  printfXY(MRG+100, 444, "%6.3f", curz/25.4);
}

void DataOut(unsigned char data)
{
  if (!port) return;
  outportb(port + 2, 0x0F);
  outportb(port, data);
  outportb(port + 2, 0x0E);
}

int Step(unsigned char ch)
{
  switch(ch)
    {
      case 'X': if (xpos < XMAX) xpos++; 
                else return 0; break;
      case 'x': if (xpos > 0) xpos--; 
                else return 0; break;
      case 'Y': if (ypos < YMAX) ypos++; 
                else return 0; break;
      case 'y': if (ypos > 0) ypos--; 
                else return 0; break;
      case 'Z': if (zpos < ZMAX) zpos++; 
                else return 0; break;
      case 'z': if (zpos > 0) zpos--; 
                else return 0; break;
      case '1': if ((xpos < XMAX) && (ypos < YMAX)) {xpos++; ypos++;} 
                else return 0; break;
      case '2': if ((xpos > 0) && (ypos < YMAX)) {xpos--; ypos++;} 
                else return 0; break;
      case '3': if ((xpos > 0) && (ypos > 0)) {xpos--; ypos--;} 
                else return 0; break;
      case '4': if ((xpos < XMAX) && (ypos > 0)) {xpos++; ypos--;} 
                else return 0; break;
      case '0': break;
      default: return 1; break;
    }
  GrPlot(MRG+xpos/XSCL, TOP+(YMAX-ypos)/YSCL, trace);
  GrPlot(MRG+xpos/XSCL, TOP+YMAX/YSCL+3+zpos/ZSCL, trace);
  GrPlot(MRG+XMAX/XSCL+3+zpos/ZSCL, TOP+(YMAX-ypos)/YSCL, trace);
  mtime = mtime + SPEED + 0.5;
  if (port == 0) return 0;
  DataOut(0x50 + scode[xpos%4]);
  DataOut(0x20 + scode[ypos%4]);
  DataOut(0x30 + scode[zpos%4]);
  delay(SPEED);
  return 0;
}

int Move(int x, int y, int z)
{
  int dx, dy, m, n, o, i;
  char a, b;
  x += xofs; if (x < 0 || x > XMAX) return 1;
  y += yofs; if (y < 0 || y > YMAX) return 1;
  z += zofs; if (z < 0 || z > ZMAX) return 1;

  dx = x - xpos;
  dy = y - ypos;
  a = '\000';
  
  if (dx > 0 && dy > 0)
  {
    m = dx; n = dy; a = '1'; b = 'Y';
    if (m > n) {o = m; m = n; n = o; b = 'X';}
  }

  if (dx < 0 && dy > 0)
  {
    m = dx * -1; n = dy; a = '2'; b = 'Y';
    if (m > n) {o = m; m = n; n = o; b = 'x';}
  }

  if (dx < 0 && dy < 0)
  {
    m = dx * -1; n = dy * -1; a = '3'; b = 'y';
    if (m > n) {o = m; m = n; n = o; b = 'x';}
  }

  if (dx > 0 && dy < 0)
  {
    m = dx; n = dy * -1; a = '4'; b = 'y';
    if (m > n) {o = m; m = n; n = o; b = 'X';}
  }

  if (a)
  {
    o = n/2;
    for (i = 0; i < n; i++)
    {
      o += m;
      if (n < o) {o -= n; Step(a);} else Step(b);
    }
  }

  while (x > xpos) Step('X');
  while (x < xpos) Step('x');
  while (y > ypos) Step('Y');
  while (y < ypos) Step('y');
  while (z > zpos) Step('Z');
  while (z < zpos) Step('z');
  return 0;
}

int aMove(float x, float y, float z)
{ message("");
  curx = x;
  cury = y;
  curz = z;
  if (Move(curx*XRES-0.6, cury*YRES-0.6, curz*ZRES-0.6)) return 1;
  Coords();
  return 0;
}

int rMove(float x, float y, float z)
{ message("");
  curx += x;
  cury += y;
  curz += z;
  if (Move(curx*XRES-0.6,cury*YRES-0.6,curz*ZRES-0.6)) return 1;
  Coords();
  return 0;
}

void Drill(void)
{
  int trc;
  trc = trace;
  trace = RED;
  rMove(0.0, 0.0, PUNCH);
  trace = GREEN;
  rMove(0.0, 0.0, (-1.0)*PUNCH);
  GrFilledBox(MRG+xpos/XSCL-1, TOP+(YMAX-ypos)/YSCL-1, 
              MRG+xpos/XSCL+1, TOP+(YMAX-ypos)/YSCL+1, RED);
  trace = trc;
}

void Penup(void)
{
  int trc;
  if (!down) return;
  trc = trace;
  trace = GREEN;
  rMove(0.0, 0.0, (-1.0)*GROOF);
  trace = trc;
  down = !down;
}

void Pendown(void)
{
  int trc;
  if (down) return;
  trc = trace;
  trace = RED;
  rMove(0.0, 0.0, GROOF);
  trace = trc;
  down = !down;
}

void Init()
{
  GrFilledBox(0, 0, 
              MRG+XMAX/XSCL+3+ZMAX/ZSCL, TOP+YMAX/YSCL+3+ZMAX/ZSCL, BLACK);
  GrFilledBox(MRG, TOP, 
              MRG+XMAX/XSCL, TOP+YMAX/YSCL, WHITE);
  GrFilledBox(MRG, TOP+YMAX/YSCL+3, 
              MRG+XMAX/XSCL, TOP+YMAX/YSCL+3+ZMAX/ZSCL, WHITE);
  GrFilledBox(MRG+XMAX/XSCL+3, TOP, 
              MRG+XMAX/XSCL+3+ZMAX/ZSCL, TOP+YMAX/YSCL, WHITE);
  Penup();
  xofs = xpos;
  yofs = ypos;
  zofs = zpos;
  curx = cury = curz = 0.0;
  GrLine(MRG+xpos/XSCL-3, TOP+(YMAX-ypos)/YSCL, 
         MRG+xpos/XSCL+3, TOP+(YMAX-ypos)/YSCL, ORANGE);
  GrLine(MRG+xpos/XSCL, TOP+(YMAX-ypos)/YSCL-3, 
         MRG+xpos/XSCL, TOP+(YMAX-ypos)/YSCL+3, ORANGE);
  Step('0');
  Coords();
}

void CkPort(int p)
{
  port = p;
  outportb(port+2, 0x04);
  if ((inportb(port+1) & 0x08) == 0) {port = 0; return;}
  outportb(port+2, 0x00);
  if ((inportb(port+1) & 0x08) != 0) port = 0;
}

int Calibrate(void)
{
  DataOut(OFF); /* enable and clear outputs */
  xpos = XMAX; while (!(inportb(port+1) & 0x40) && xpos) Step('x');
  if (!xpos) return 1;
  xpos = 0; while ((inportb(port+1) & 0x40) && (xpos < XRES)) Step('X');
  if (xpos == XRES) return 1;
  ypos = YMAX; while ((inportb(port+1) & 0x80) && ypos) Step('y');
  if (!ypos) return 1;
  ypos = 0; while (!(inportb(port+1) & 0x80) && (ypos < YRES)) Step('Y');
  if (ypos == YRES) return 1;
  zpos = ZMAX; while (!(inportb(port+1) & 0x20) && zpos) Step('z');
  if (!zpos) return 1;
  zpos = 0; while ((inportb(port+1) & 0x20) && (zpos < ZRES)) Step('Z');
  if (zpos == ZRES) return 1;
  return 0;
}

void Start(int check)
{
  if (check)
  {
    CkPort(0x378);
    if (!port) CkPort(0x378);
    if (!port) CkPort(0x3BC);
    if (port) if (Calibrate()) port = 0;
  }
  xpos = ypos = zpos = 0;
  Init();
  message("port %x", port);
}

void prompt(char *s)
{
  int i = 0;
  while (i < 74)
  {
    s[i] = '\000';
    message("file: %s", s);
    s[i] = getch();
    if (s[i] == '\015') {s[i] = '\000'; return;}
    if (s[i] == '\010') {if (i) i--; s[i] = '\000';} 
    else if (s[i] >= ' ') i++;
  }
  s[74] = '\000';
}

int Term(int poll)
{
  if (poll)
  {
    if (!kbhit()) return 0;
    getch();
    message("pause: esc to terminate, other key to continue");
  }
  if (getch() != '\033') {message(""); return 0;}
  message("terminated");
  return 1;
}

void Excellon(void)
{
  FILE *in;
  char fname[128];
  char str[256];
  int i, n, m;
  int mirror = 0;
  float x, y, xm, s, unit = 0.000254, xof = 100000, yof = 100000;
  message("Init done at %f/%f ca 0.5mm above board? no: esc", xof, yof);
  if (Term(0)) return;
  prompt(fname);
  in = fopen(fname, "rb");
  if (in == NULL) {message("file %s not found", fname); return;}
  Penup();
  m = items = 0;
  mtime = 0.0;
  trace = CYAN;
  while (1) {
    if (Term(1)) break;
    fgets(str, 256, in);
    if (feof(in)) break;
    for (i = 0; str[i] > ' '; i++);
    str[i] = '\000';
    if (str[0] == 'T') {
      message("mount tool %s, any key when ready", str);
      if (Term(0)) break;
    }
    if (str[0] == 'X') {
      x = y = 0.0; i = 1;
      while (str[i] >= '0' && str[i] <= '9') {x = x*10+str[i]-'0'; i++;}
      i++;
      while (str[i] >= '0' && str[i] <= '9') {y = y*10+str[i]-'0'; i++;}
      if (mirror) x = (xm - x);
      x = x*unit;
      y = y*unit;
      if (aMove(x-(xof*unit), y-(yof*unit), 0.0) == 0) {Drill(); items++;}
      m++;
    }
    if (str[0] == 'R') {
      x = y = 0.0; n = 0; s = 1.0; i = 1;
      if (mirror) s = -1.0;
      while (str[i] >= '0' && str[i] <= '9') {n = n*10+str[i]-'0'; i++;}
      if (str[i++] == 'X') {
        if (str[i] == '-') {s = s * -1.0; i++;}
        while (str[i] >= '0' && str[i] <= '9') {x = x*10+str[i]-'0'; i++;}
        x = s*x*unit;
      } else {
        if (str[i] == '-') {s = s * -1.0; i++;}
        while (str[i] >= '0' && str[i] <= '9') {y = y*10+str[i]-'0'; i++;}
        y = s*y*unit;
      }
      for (i = 0; i < n; i++) {
        if (rMove(x, y, 0.0) == 0) {Drill(); items++;}
        m++;
      }
    }
    if (str[0] == 'M' && str[1] == 'I' && str[2] == 'R') 
    {
      xm = 0.0; i = 3;
      while (str[i] >= '0' && str[i] <= '9') {xm = xm*10+str[i]-'0'; i++;}
      mirror = 1;
    }
  }
  fclose(in);
  trace = BLUE;
  message("%s done, %i holes of %i processed, approx. %0.0f minutes", 
           fname, items, m, mtime/60000.0);
}

void Excellon2(void)
{
  FILE *in;
  char fname[128];
  char str[256];
  float xy[2*1024];
  int i, j, n, m;
  float x, y, s, unit = 0.000254, xof = 100000, yof = 100000;
  message("Init done at %f/%f ca 0.5mm above board? no: esc", xof, yof);
  if (Term(0)) return;
  prompt(fname);
  in = fopen(fname, "rb");
  if (in == NULL) {message("file %s not found", fname); return;}
  Penup();
  m = items = 0;
  mtime = 0.0;
  trace = CYAN;
  while (1) { 
    fgets(str, 256, in);
    if (feof(in) || m >= 1024) break;
    for (i = 0; str[i] > ' '; i++);
    str[i] = '\000';
    if (str[0] == 'X')
    {
      x = y = 0.0; i = 1;
      while (str[i] >= '0' && str[i] <= '9') {x = x*10+str[i]-'0'; i++;}
      i++;
      while (str[i] >= '0' && str[i] <= '9') {y = y*10+str[i]-'0'; i++;}
      xy[2*m] = x*unit - xof*unit;
      xy[2*m+1] = y*unit - yof*unit;
      m++;
    }
    if (str[0] == 'R') {
      x = y = 0.0; n = 0; s = 1.0; i = 1;
      while (str[i] >= '0' && str[i] <= '9') {n = n*10+str[i]-'0'; i++;}
      if (str[i++] == 'X') {
        if (str[i] == '-') {s = s * -1.0; i++;}
        while (str[i] >= '0' && str[i] <= '9') {x = x*10+str[i]-'0'; i++;}
        x = s*x*unit;
      } else {
        if (str[i] == '-') {s = s * -1.0; i++;}
        while (str[i] >= '0' && str[i] <= '9') {y = y*10+str[i]-'0'; i++;}
        y = s*y*unit;
      }
      j = m - 1;
      for (i = 0; i < n; i++) {
        xy[2*m] = xy[2*j] + i*x*unit;
        xy[2*m+1] = xy[2*j+1] + i*y*unit;
        m++;
      }
    }
  }
  fclose(in);
  for (i = 0; i < m; i++) {
    if (Term(1)) break;
    n = 0; s = 999999.9;
    for (j = 0; j < m; j++) {
      x = curx - xy[2*j];
      y = cury - xy[2*j+1];
      if (x*x + y*y < s) {
        s = x*x + y*y;
        n = j;
      }
    }
    if (aMove(xy[2*n], xy[2*n+1], 0.0) == 0) {Drill(); items++;}
    xy[2*n] = xy[2*n+1] = 999.9;
  }
  trace = BLUE;
  message("%s done, %i holes of %i processed, approx. %0.0f minutes", 
           fname, items, m, mtime/60000.0);
}

void HPGL(void)
{
  FILE *in;
  char fname[128];
  char str[256];
  int i;
  int mirror = 0;
  float x, y, x0, y0, xm;
  prompt(fname);
  in = fopen(fname, "rb");
  if (in == NULL) {
    message("file %s not found", fname);
    return;
  }
  Penup();
  items = 0;
  mtime = 0.0;
  x0 = y0 = 0.0;
  while (1) {
    if (Term(1)) break;
    i = 0;
    do {
      str[i] = getc(in);
      if (str[i] > ' ' && str[i] != ';') i++;
    } while (str[i] != ';' && i < 256 && !feof(in));
    if (str[i] != ';') break;
    str[i] = '\000';
    if (str[0] == 'P' && str[1] == 'A') {
      x = y = 0.0; i = 2;
      while (str[i] >= '0' && str[i] <= '9') {x = x*10+str[i]-'0'; i++;}
      if (str[i] != '\000') i++;
      while (str[i] >= '0' && str[i] <= '9') {y = y*10+str[i]-'0'; i++;}
      x -= x0;
      y -= y0;
      if (mirror) x = (xm - x);
      x = x*0.025;
      y = y*0.025;
      if (down) trace = MAGENTA; else trace = CYAN;
      aMove(x, y, curz);
      if (down) items++;
    }
    if (str[0] == 'P' && str[1] == 'D') Pendown();
    if (str[0] == 'P' && str[1] == 'U') Penup();
    if (str[0] == 'S' && str[1] == 'P') {
      message("select pen %s, any key when ready", str);
      if (Term(0)) break;
    }
    if (str[0] == 'I' && str[1] == 'W') {
      x0 = y0 = 0.0; i = 2;
      while (str[i] >= '0' && str[i] <= '9') {x0 = x0*10+str[i]-'0'; i++;}
      if (str[i] != '\000') i++;
      while (str[i] >= '0' && str[i] <= '9') {y0 = y0*10+str[i]-'0'; i++;}
    }
    if (str[0] == 'M' && str[1] == 'I' && str[2] == 'R') {
      xm = 0.0; i = 3;
      while (str[i] >= '0' && str[i] <= '9') {xm = xm*10+str[i]-'0'; i++;}
      mirror = 1;
    }
    if (str[0] == 'M' && str[1] == 'I') mirror = 1;
  }
  fclose(in);
  trace = BLUE;
  message("%s done, %i segments processed, approx. %0.0f minutes", 
           fname, items, mtime/60000.0);
}

int main(int argc, char *argv[])
{
  unsigned char ch, sc;
  
  if ((txtopt.txo_font = GrLoadFont("default.fnt")) == NULL) {
    printf("default.fnt not found\n");
    return 1;
  }
  txtopt.txo_bgcolor.v = BLACK;

  GrafMode();
  Start(argc == 1);
  while (kbhit()) getch();
  do { 
    ch = getch();
    switch(tolower(ch)) {
      case 'q': ch = '\033'; break;
      case 'i': Init(); message("coordinates set to (0,0,0)"); break;
      case 'e': if (ch == 'E') Excellon2(); else Excellon(); break;
      case 'h': HPGL(); break;
      case 'o': aMove(0.0, 0.0, 0.0); message("origin (0,0,0)"); break;
      case 'r': Start(argc == 1); break;
      case 'p': xofs = yofs = zofs = 0; Move(xpos, ypos, 0); xofs = XMAX/2; 
                Move(0, 0, 0); Init(); message("parked"); break;
      case 'u': Penup(); break;
      case 'd': Pendown(); break;
      case '!': Drill(); break;
      case '+': rMove(0.0, 0.0, stepw); break;
      case '-': rMove(0.0, 0.0, -1.0*stepw); break;
      case '\000': sc = getch(); 
                if (sc == 71) rMove(-1.0*stepw, stepw, 0.0);
                if (sc == 72) rMove(0.0, stepw, 0.0);
                if (sc == 73) rMove(stepw, stepw, 0.0);
                if (sc == 75) rMove(-1.0*stepw, 0.0, 0.0);
                if (sc == 76) {
                  if (stepw < 0.05) stepw = 0.1;
                  else if (stepw < 0.2) stepw = 0.254;
                  else if (stepw < 0.5) stepw = 1.0;
                  else if (stepw < 2.0) stepw = 2.54;
                  else if (stepw < 5.0) stepw = 10.0;
                  else if (stepw < 20.0) stepw = 25.4;
                  else stepw = 0.01;
                  message("stepwidth %6.2fmm %6.4finch", 
                  stepw, stepw/25.4);
                }
                if (sc == 77) rMove(stepw, 0.0, 0.0);
                if (sc == 79) rMove(-1.0*stepw, -1.0*stepw, 0.0);
                if (sc == 80) rMove(0.0, -1.0*stepw, 0.0);
                if (sc == 81) rMove(stepw, -1.0*stepw, 0.0);
                break;
      default: break;
    }
  } while (ch != '\033');
  DataOut(OFF);
  GrSetMode(GR_default_text);
  return 0;
}