#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <error.h>
#include <errno.h>
#include <unistd.h>
#include <math.h>

#define PI 3.141592653589793

#define NO_MOVEMENT 1   // suppress movement while developing, just in case of accidents!

#define MAX_RESPONSE 1024

int debug = 1;
int lineno = 0;
char line[MAX_RESPONSE];

void send(FILE *dev, char *s) {
  if (debug) fprintf(stderr, "%s\n", s);
  fprintf(dev, "%s\r", s); fflush(dev);
}

void receive(FILE *dev, char *text) {
  int i = 1;
  for (;;) {
    // readline();
    char *s = line;
    for (;;) {
      int c = fgetc(dev);
      if (c == EOF) break;
      if (c == '\r') continue;
      if (c == '\n') break;
      *s++ = c; i++;
      if (i == MAX_RESPONSE) break;
    }
    *s = '\0';
    lineno++;
    fprintf(stderr, "%d: %s\n", lineno, line);
    if (strcmp(line, text) == 0) break;
    if (strcmp(line, "ok") == 0) break;
  }
}

void nc(char *s, FILE *out, FILE *in) {
#ifndef NO_MOVEMENT
  send(out, s);
  receive(in, "ok");
#endif
}

// application program follows.  Not so careful about global state here.

FILE *mx3pro_in, *mx3pro_out;

// All units in mm.

float in(float mm) {
  return mm/25.4;
}
  
float mm(float m) {
  return m;
}

float x = 0.0, y = 0.0, z = 0.0; // in mm * 100 ??? REALLY??? CHECK!!!

#define TOOL_WIDTH mm(1.25)   // 1.25mm cutting head

int spindle_speed = 6000,
    z_plunge_speed = 200, z_retract_speed = 500,
    xymove_speed = 500, xycut_speed = 200,
    xy_speed = 0, current_default_speed = 0;

void gcode(char *s) {
  nc(s, mx3pro_out, mx3pro_in);
}

void setheight(float absz, int zspeed) {
  static char shortcmd[128];
  z = absz; current_default_speed = zspeed;
  sprintf(shortcmd, "G1 Z%0.1f F%0d\n", z, zspeed);
  gcode(shortcmd);
}

void retract(void) {
  setheight(10.0, z_retract_speed);
}

void penup(void) {
  setheight(5.0, z_plunge_speed);
  xy_speed = xymove_speed;
}

void pendown(void) {
  setheight(-0.2, z_plunge_speed);
  xy_speed = xycut_speed;
}

void gotoxy(float x, float y) {
  static char shortcmd[128];
  if (xy_speed != current_default_speed) {
    current_default_speed = xy_speed;
    sprintf(shortcmd, "G1 X%0.3f Y%0.3f F%0d\n", x, y, xy_speed);
  } else {
    sprintf(shortcmd, "G1 X%0.3f Y%0.3f\n", x, y);
  }
  gcode(shortcmd);
}

void move(float dx, float dy) {
  x += dx; y += dy;
  gotoxy(x, y);
}

void north(float dy) {
  move(0.0, dy);
}

void south(float dy) {
  move(0.0, -dy);
}

void east(float dx) {
  move(dx, 0.0);
}

void west(float dx) {
  move(-dx, 0.0);
}

void start_spindle(int speed) {
  static char shortcmd[128];
  sprintf(shortcmd, "M3 S%0d\n", speed); // assuming your spindle output hasn't blown up :-(
  gcode(shortcmd);
}

void stop_spindle(void) {
  gcode("M5");
}

void finished(void) {
  gcode("M30");
}

void drill(float x, float y, float z, float depth) {
}

void cuboid(float x, float y, float z, float dx, float dy, float dz) {
}


// not exactly a 'solid of revolution' since this is negative space, but yes, basically that.

void revolution1(float cx, float cy, float cz, float radius, float stepover, float depth(float radius)) {
  float d, r, tantheta, theta, angle;
  // depth is determined by calling the external function with the distance from the center as a parameter
  retract();
  gotoxy(cx, cy);
  setheight(cz, z_retract_speed);
  d = depth(0.0);
  setheight(cz-d, z_plunge_speed);
  r = 0.0+stepover;
  for (;;) {
    if ((r + TOOL_WIDTH/2.0) > radius) break;
    north(stepover);
    // determine angle that equates to a movement around the circle of 'stepover' units
    // Tan theta = opposite/adjacent.  opposite is desired movement (approximately); adjacent is radius
    tantheta = stepover/radius;
    theta = atanf(tantheta);
    angle = 0;
    // draw circle!
    while (angle < (2 * PI)) {
      // calculate x,y of point on circumference at this angle.
      // move there.
      angle += theta;
    }
    r += stepover;
  }
  retract();
}

void revolution2(float cx, float cy, float cz, float radius, float stepover, float depth(float radius, float param), float param) {
  // depth is determined by calling the external function with the distance from the center as a parameter
  // with a second parameter 'param' being passed to the function.
}

float fixed_depth(float radius, float param) {
  return param;
}

void cylinder(float cx, float cy, float cz, float radius, float stepover, float depth) {
  revolution2(cx, cy, cz, radius, stepover, &fixed_depth, cz-depth);
}
  
float carriage_bolt_head(float radius, float param) {
  return param; // + depth of arc as a function of radius
}


int Ask(char *Prompt) {
  int i;
  fprintf(stderr, "%s", Prompt); fflush(stderr); // which should have been automatic
  i = 1;
  for (;;) {
    // readline();
    char *s = line;
    for (;;) {
      int c = fgetc(stdin);
      if (c == EOF) break;
      if (c == '\r') continue;
      if (c == '\n') break;
      *s++ = c; i++;
      if (i == MAX_RESPONSE) break;
    }
    *s = '\0';
  }
}

void Interpreter(void) {
  int i;
  for (;;) {
    i = 1;
    for (;;) {
      // readline();
      char *s = line;
      for (;;) {
        int c = fgetc(stdin);
        if (c == EOF) break;
        if (c == '\r') continue;
        if (c == '\n') break;
        *s++ = c; i++;
        if (i == MAX_RESPONSE) break;
      }
      *s = '\0';
    }
    if (strcasecmp(line, "quit") == 0) exit(0);
    if (strcasecmp(line, "ready") == 0) return;
    if (strcasecmp(line, "go") == 0) return;
    if (strcasecmp(line, "probe") == 0) {
      fprintf(stderr, "Please place the probe under the tool.\n");
      do { } while (!Ask("Ready? "));
    } else if (strcasecmp(line, "zeroxy") == 0) {
      fprintf(stderr, "The current X,Y position is now defined as origin (0,0)\n");
      fprintf(stderr, "The current Z height is %f mm\n", 3.456);
    } else if (strcasecmp(line, "zeroxyz") == 0) {
      fprintf(stderr, "The current X,Y,Z position is now defined as origin (0,0,0)\n");
    } else if (strcasecmp(line, "center") == 0) {
      fprintf(stderr, "Starting a homing cycle and moving to the center at retraction height,\n");
      fprintf(stderr, "which will be defined as (0,0).\n");
    } else if (strcasecmp(line, "origin") == 0) {
      fprintf(stderr, "Starting a homing cycle and moving to the lower-left corner at retraction height,\n");
      fprintf(stderr, "which will be defined as (0,0).\n");
    } else {
      gcode(line);
    }
  }
}

int main(int argc, char **argv) {
  // set baud to 115200
  system("stty -F /dev/ttyUSB0 speed 115200  -brkint -icrnl -imaxbel -opost -onlcr -isig -icanon -iexten -echo -echoe -echok -echoctl -echoke");
  mx3pro_out = fopen("/dev/ttyUSB0", "wb");
  if (mx3pro_out == NULL) {
    fprintf(stderr, "%s: cannot open /dev/ttyUSB0 as output\n", argv[0]);
    exit(EXIT_FAILURE);
  }
  mx3pro_in  = fopen("/dev/ttyUSB0", "rb");
  if (mx3pro_in == NULL) {
    fprintf(stderr, "%s: cannot open /dev/ttyUSB0 as input\n", argv[0]);
    exit(EXIT_FAILURE);
  }

  // machine sends a list of changed parameters...
  // [GC:G0 G54 G17 G21 G90 G94 M5 M9 T0 F0 S20000]
  // G0      select rapid motion mode, i.e. head movement enabled, not cutting movement.
  // G54     which zero offset to use
  // G17     working in X/Y plane
  // G21     sizes are in millimeters
  // G90     coordinates are absolute, not relative
  // G94     mm per minute feed mode
  // M5      spindle off
  // M9      mist and flood coolant off!
  // T0      Tool #0 (ie use default)
  // F0      Feed speed 0 (ie use default)
  // S20000  Spindle max speed is 20000
  
  receive(mx3pro_in, "[MSG:'$H'|'$X' to unlock]"); // or 'ok' if not locked.
  // $H      Run homing cycle
  // $X      Unlock
  gcode("$X");
  gcode("$30=20000"); // tell mx3 we have a high-speed spindle!
  gcode("G90"); // absolute positioning system

  // this particular program defines its zero as the center of the rectangle to be cut...
  fprintf(stderr, "Please manually position the tool in the center of where the bolt will go,\n");
  fprintf(stderr, "with the tip at board level.\n");
  fprintf(stderr, "Type 'ready' when it is in position. (or 'quit' to cancel)\n");
  Interpreter();
  penup();
  // Drill a hole in the center for repositioning and for guiding a drill later.
  //  (Must be deeper than square part of carriage bolt.)

  // NEED MICROMETER.

  // Cut the circular part of the carriage bolt head, maximum d mm deep with radius r mm.
  //   (diameter is 1+3/16in (1.1875in), total depth including the flat part at the top is 7/64in (0.1094in))

  revolution2(0.0,0.0,0.0, (in(1.1875)/2.0), (TOOL_WIDTH/2.0), carriage_bolt_head, mm(0.5));

  // Cut a square d2 mm deep with side s mm.
  //   (5/16in (0.3125in) square, depth ???)
  cuboid(0.0,0.0,0.0, 0.3125,0.3125, 0.2);
  
  stop_spindle();
  retract();
  finished();

  // Manually drill through the board from the center of the hole with 5/8in drill (or marginally tighter?)

  // we could for example ask the user to manually position the head to a starting position before executing
  // a subroutine to engrave a text string.

  // (what might be really cool would be projecting a preview on the wood to confirm positioning before cutting!)
  
  fclose(mx3pro_in);
  fclose(mx3pro_out);

  exit(EXIT_SUCCESS);
  return EXIT_FAILURE;
}