// -------------------------------
// GLOBALS
// -------------------------------
#include <Stepper.h>
#define FULLREVOLUTION 4096
#define MOTOR1 3
#define MOTOR2 4
#define MOTOR3 5
#define MOTOR4 6
Stepper myStepper(FULLREVOLUTION, MOTOR1, MOTOR2, MOTOR3,MOTOR4 );
// ----- constants
#define PI 3.1415926535897932384626433832795
#define HALF_PI 1.5707963267948966192313216916398
#define TWO_PI 6.283185307179586476925286766559
#define DEG_TO_RAD 0.017453292519943295769236907684886
#define RAD_TO_DEG 57.295779513082320876798154814105

// ----- motor definitions
#define STEPS_PER_DEG 12800/360     //motor=200 steps/rev; 16 x microstepping; 4 x belt drive
#define NUDGE STEPS_PER_DEG/4       //rotate the motor 0.25 degrees (change number to suit)
#define CW 1                        //motor directions
//#define CCW 08
#define DIR1 8                      //Arduino ports
#define DIR2 10
#define STEP1 9
#define STEP2 11
int y4 = 0;
int x4 = 0;
float y3 = 0;
float x3 = 0;
float ys = 0;
float xs = 0,a,b,c;


long
PULSE_WIDTH = 2,                    //easydriver step pulse-width (uS)
DELAY_MIN = 2500,                   //minimum inter-step delay (uS) between motor steps (controls speed)
DELAY1,                             //inter-step delay for motor1 (uS)
DELAY2,                             //inter-step delay for motor2 (uS)
STEPS1,                             //motor1 steps from 12 o'clock to reach an XY co-ordinate
STEPS2;                             //motor2 steps from 12 o'clock to reach an XY co-ordinate

// ----- plotter definitions
#define BAUD 9600
#define XOFF 0x13                   //pause transmission (19 decimal)
#define XON 0x11                    //resume transmission (17 decimal)
#define PEN 3

float
OFFSET = 210,                       //motor offset along x_axis
YAXIS = 465,                        //motor heights above (0,0)
LENGTH = 300,                       //length of each arm-segment
SCALE_FACTOR = 1,                   //drawing scale (1 = 100%)
ARC_MAX = 2;                        //maximum arc-length (controls smoothness)

int
/*
   XY plotters only deal in integer steps.
*/
THIS_X = 0,                         //this X co-ordinate (rounded)
THIS_Y = 0,                         //this Y co-ordinate (rounded)
LAST_X = 0,                         //last X co-ordinate (rounded)
LAST_Y = 0;                         //last Y co-ordinate (rounded)

// ----- gcode definitions
#define STRING_SIZE 128             //string size

char
BUFFER[STRING_SIZE + 1],
       INPUT_CHAR;

String
INPUT_STRING,
SUB_STRING;

int
INDEX = 0,                        //buffer index
START,                            //used for sub_string extraction
FINISH;

float
X,                                //gcode float values held here
Y,
I,
J;

// -----------------------
// SETUP
// -----------------------
void setup()
{
  // ----- initialise motor1
  pinMode(DIR1, OUTPUT);
  pinMode(STEP1, OUTPUT);
  digitalWrite(DIR1, CW);
  delayMicroseconds(PULSE_WIDTH);
  digitalWrite(STEP1, HIGH);

  // ----- initialise motor2
  pinMode(DIR2, OUTPUT);
  pinMode(STEP2, OUTPUT);
  digitalWrite(DIR2, CW);
  delayMicroseconds(PULSE_WIDTH);
  digitalWrite(STEP2, HIGH);

  // ----- initialise STEPS1, STEPS2 for co-ordinate (0,0)
  calculate_steps(0, 0);

  // ----- pen-lift
  pinMode(PEN, OUTPUT);                                       //D3
  TCCR2A = _BV(COM2B1) | _BV(COM2B0) | _BV(WGM20);            //PWM
  TCCR2B = _BV(WGM22) | _BV(CS22) | _BV(CS21) | _BV(CS20);    //div 1024
  OCR2A = 156;                                                //20mS period
  OCR2B = 148;                                                //2mS period (pen-up)

  

  // ----- plotter setup
  memset(BUFFER, '\0', sizeof(BUFFER));     //fill with string terminators
  INPUT_STRING.reserve(STRING_SIZE);
  INPUT_STRING = "";

  // ----- establish serial link
  Serial.begin(BAUD);

  // ----- flush the buffers
  Serial.flush();                           //clear TX buffer
  while (Serial.available()) Serial.read(); //clear RX buffer

  // ----- display commands
  menu();
}

//--------------------------------------------------------------------------
// MAIN LOOP
//--------------------------------------------------------------------------
void loop() {

  // ----- get the next instruction
  while (Serial.available()) {
    INPUT_CHAR = (char)Serial.read();         //read character
    Serial.write(INPUT_CHAR);                 //echo character to the screen
    BUFFER[INDEX++] = INPUT_CHAR;             //add char to buffer
    if (INPUT_CHAR == '\n') {                 //check for line feed
      Serial.flush();                         //clear TX buffer
      Serial.write(XOFF);                     //pause transmission
      INPUT_STRING = BUFFER;                  //convert to string
      process();                              //interpret string and perform task
      memset(BUFFER, '\0', sizeof(BUFFER));   //fill buffer with string terminators
      INDEX = 0;                              //point to buffer start
      INPUT_STRING = "";                      //empty string
      Serial.flush();                         //clear TX buffer
      Serial.write(XON);                      //resume transmission
    }
  }
}

//---------------------------------------------------------------------------
// MENU
//---------------------------------------------------------------------------
/*
   The Arduino F() flash macro is used to conserve RAM.
*/
void menu() {
  Serial.println(F(""));
  Serial.println(F("  ------------------------------------------------------"));
  Serial.println(F("                         MENU"));
  Serial.println(F("  ------------------------------------------------------"));
  Serial.println(F("    MENU ............... menu"));
  Serial.println(F("    G00 X## Y## ........ goto XY (pen-up)"));
  Serial.println(F("    G01 X## Y## ........ goto XY (pen-down)"));
  Serial.println(F("    T1 ................. move pen to 0,0"));
  Serial.println(F("    T2 S##.## .......... set drawing Scale (1=100%)"));
  Serial.println(F("    T3 ................. pen up"));
  Serial.println(F("    T4 ................. pen down"));
  Serial.println(F("    T5 ................. test pattern: ABC"));
  Serial.println(F("    T6 ................. test pattern: target"));
  Serial.println(F("    T7 ................. test pattern: radials"));
  Serial.println(F("  ------------------------------------------------------"));
}

//--------------------------------------------------------------------------
// PROCESS
//--------------------------------------------------------------------------
void process() {

   // ----- convert string to upper case
   INPUT_STRING.toUpperCase();

   // ----------------------------------
   // G00   linear move with pen_up
   // ----------------------------------
   if (INPUT_STRING.startsWith("G00")) {

   // ----- extract X
   START = INPUT_STRING.indexOf('X');
   if (!(START < 0)) {
   FINISH = START + 8;
   SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 1);
   X = SUB_STRING.toFloat();
   }

   // ----- extract Y
   START = INPUT_STRING.indexOf('Y');
   if (!(START < 0)) {
   FINISH = START + 8;
   SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 1);
   Y = SUB_STRING.toFloat();
   }
}
    pen_up();
    move_to(X, Y);
  }

  // ----------------------------------
  // G01   linear move with pen_down
  // ----------------------------------
  if (INPUT_STRING.startsWith("G01")) {

    // ----- extract X
    START = INPUT_STRING.indexOf('X');
    if (!(START < 0)) {
      FINISH = START + 8;
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 1);
      X = SUB_STRING.toFloat();
    }

    // ----- extract Y
    START = INPUT_STRING.indexOf('Y');
    if (!(START < 0)) {
      FINISH = START + 8;
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 1);
      Y = SUB_STRING.toFloat();
    }

    pen_down();
    move_to(X, Y);
  }

  // ----------------------------------
  // G02   clockwise arc with pen_down
  // ----------------------------------
  if (INPUT_STRING.startsWith("G02")) {

    // ----- extract X
    START = INPUT_STRING.indexOf('X');
    if (!(START < 0)) {
      FINISH = INPUT_STRING.indexOf('.', INPUT_STRING.indexOf('X'));
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 7);
      X = SUB_STRING.toFloat();
    }

    // ----- extract Y
    START = INPUT_STRING.indexOf('Y');
    if (!(START < 0)) {
      FINISH = INPUT_STRING.indexOf('.', INPUT_STRING.indexOf('Y'));
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 7);
      Y = SUB_STRING.toFloat();
    }

    // ----- extract I
    START = INPUT_STRING.indexOf('I');
    if (!(START < 0)) {
      FINISH = INPUT_STRING.indexOf('.', INPUT_STRING.indexOf('I'));
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 7);
      I = SUB_STRING.toFloat();
    }

    // ----- extract J
    START = INPUT_STRING.indexOf('J');
    if (!(START < 0)) {
      FINISH = INPUT_STRING.indexOf('.', INPUT_STRING.indexOf('J'));
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 7);
      J = SUB_STRING.toFloat();
    }

    pen_down();
    draw_arc_cw(X, Y, I, J);
  }

  // ------------------------------------------
  // G03   counter-clockwise arc with pen_down
  // ------------------------------------------
  if (INPUT_STRING.startsWith("G03")) {

    // ----- extract X
    START = INPUT_STRING.indexOf('X');
    if (!(START < 0)) {
      FINISH = INPUT_STRING.indexOf('.', INPUT_STRING.indexOf('X'));
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 7);
      X = SUB_STRING.toFloat();
    }

    // ----- extract Y
    START = INPUT_STRING.indexOf('Y');
    if (!(START < 0)) {
      FINISH = INPUT_STRING.indexOf('.', INPUT_STRING.indexOf('Y'));
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 7);
      Y = SUB_STRING.toFloat();
    }

    // ----- extract I
    START = INPUT_STRING.indexOf('I');
    if (!(START < 0)) {
      FINISH = INPUT_STRING.indexOf('.', INPUT_STRING.indexOf('I'));
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 7);
      I = SUB_STRING.toFloat();
    }

    // ----- extract J
    START = INPUT_STRING.indexOf('J');
    if (!(START < 0)) {
      FINISH = INPUT_STRING.indexOf('.', INPUT_STRING.indexOf('J'));
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH + 7);
      J = SUB_STRING.toFloat();
    }

    pen_down();
    draw_arc_ccw(X, Y, I, J);
  }

  // ----------------------------------
  // MENU
  // ----------------------------------
  if (INPUT_STRING.startsWith("MENU")) {
    menu();
  }

  // ----------------------------------
  // T1   position the pen over 0,0
  // ----------------------------------
  if (INPUT_STRING.startsWith("T1")) {

    // ----- variables
    int step;           //loop counter
    int steps = NUDGE;  //steps motor is to rotate

    // ----- instructions
    Serial.println(F(""));
    Serial.println(F("  ----------------------------------------------"));
    Serial.println(F("    Position the pen over the 0,0 co-ordinate:"));
    Serial.println(F("  ----------------------------------------------"));
    Serial.println(F("    X-axis:             Y-axis:"));
    Serial.println(F("   'A'  'S'            'K'  'L'"));
    Serial.println(F("   <-    ->            <-    ->"));
    Serial.println(F("    Exit = 'E'"));

    // ----- flush the buffer
    while (Serial.available() > 0) Serial.read();

    // ----- control motors with 'A', 'S', 'K', and 'L' keys

    char keystroke = ' ';
    while (keystroke != 'E') {  //press 'E' key to exit

      // ----- check for keypress
      if (Serial.available() > 0) {
        keystroke = (char) Serial.read();
      }

      // ----- select task
      switch (keystroke) {
        case 'a':
        case 'A': {
            // ----- rotate motor1 CW
            for (step = 0; step < steps; step++) {
              step1_cw();
            }
            keystroke = ' ';    //otherwise motor will continue to rotate
            break;
          }
        case 's':
        case 'S': {
            // ------ rotate motor1 CCW
            for (step = 0; step < steps; step++) {
              step1_ccw();
            }
            keystroke = ' ';
            break;
          }
        case 'k':
        case 'K': {
            // ----- rotate motor2 CW
            for (step = 0; step < steps; step++) {
              step2_cw();
            }
            keystroke = ' ';
            break;
          }
        case 'l':
        case 'L': {
            // ----- rotate motor2 CCW
            for (step = 0; step < steps; step++) {
              step2_ccw();
            }
            keystroke = ' ';
            break;
          }
        case 'e':
        case 'E': {
            // ----- exit
            Serial.println(F(" "));
            Serial.println(F("  Calibration complete ..."));
            keystroke = 'E';
            break;
          }
        // ----- default for keystroke
        default: {
            break;
          }
      }
    }

    // ----- initialise counters for co-ordinate (0,0)
    calculate_steps(0, 0);        //initialise STEPS1, STEPS2
    THIS_X = 0;                   //current X co-ordinate
    THIS_Y = 0;                   //current Y co-ordinate
    LAST_X = 0;                   //previous X co-ordinate
    LAST_Y = 0;                   //previous Y-co-ordinate
  }

  // ----------------------------------
  // T2   set scale factor
  // ----------------------------------
  if (INPUT_STRING.startsWith("T2")) {
    Serial.println("T2");

    START = INPUT_STRING.indexOf('S');
    if (!(START < 0)) {
      FINISH = START + 6;
      SUB_STRING = INPUT_STRING.substring(START + 1, FINISH);
      SCALE_FACTOR = SUB_STRING.toFloat();
      Serial.print(F("Drawing now ")); Serial.print(SCALE_FACTOR * 100); Serial.println(F("%"));
    }
    else {
      Serial.println(F("Invalid scale factor ... try again. (1 = 100%)"));
    }
  }

  // ----------------------------------
  // T3   pen up
  // ----------------------------------
  if (INPUT_STRING.startsWith("T3")) {
    pen_up();
  }

  // ----------------------------------
  // T4   pen down
  // ----------------------------------
  if (INPUT_STRING.startsWith("T4")) {
    pen_down();
  }

  // ----------------------------------
  // T5   ABC test pattern
  // ----------------------------------
  if (INPUT_STRING.startsWith("T5")) {
    abc();
  }

  // ----------------------------------
  // T6   target test pattern
  // ----------------------------------
  if (INPUT_STRING.startsWith("T6")) {
    target();
  }

  // ----------------------------------
  // T7   radial line test pattern
  // ----------------------------------
  if (INPUT_STRING.startsWith("T7")) {
    radials();
  }
}

// -------------------------------
// MOVE_TO
// -------------------------------
/*
   We need to make this plotter think that it is an XY plotter
  Serial.print("((");
   Serial.println(a);
   Serial.println(b);
   Serial.println(c);
   delay(100);
   Serial.print("))");
   Serial.println(xs);
   Serial.println(ys);
*/
void move_to(float x, float y) {
  Serial.print(x);
  Serial.print("!!");
  Serial.print(y);
  
  my(x,y);
  // ----- apply scale factor
   THIS_X = (int)((x * SCALE_FACTOR));
  THIS_Y = (int)((y * SCALE_FACTOR));

  // ----- draw a line between these co-ordinates
  draw_line(LAST_X, LAST_Y, THIS_X, THIS_Y);

  // ----- remember last rounded co-ordinate
  LAST_X = THIS_X;
  LAST_Y = THIS_Y;

}
in  if(x!=x4 & y!=y4){
   Serial.print("==");
   s1 = (x-x4)/(y-y4); 
   Serial.print(s1);
   Serial.print("rrr");
   j=0;
    if(x<x4){
      d=x4-(x);
      g=(x*-1);}
    else{
      d=(x-x4);
      g=x;}
   if(d<5)
   {f=0.1;}
   else
   { if(d>5 & d<10){
    f=0.5;
   }
   else 
   f=1;}
   for(i=0;j<g;i++) {
   {
   j=j+0.1;
   Serial.print(j);
   y3 =(s1*j)-(s1*x)+y;
   Serial.print(y3);
   b=atan(y/j);
   c=(acos((sq(j)+sq(z)-sq(A)-sq(B))/(2*A*B)))*(180/k);
   a=(atan(z/j)*180/k)+(atan(B*sin(c)/(A+B*cos(c)))*(180/k));
   xs = a *(2048/360);
   ys = b*(2048/360);
   }
   step1_cw();
   myStepper.step(a);
   }
   x4 = x;
   
}
}
// ------------------------------------------------------------------------
// DRaw line
// ------G00 x-12 y-12-------------------------------------------------------

void draw_line(int x1, int y1, int x2, int y2) {

  // ----- locals
  int
  x = x1,                             //current X-axis position
  y = y1,                             //current Y-axis position
  dy,                                 //line slope
  dx,
  slope,
  longest,                            //axis lengths
  shortest,
  maximum,
  error,                              //bresenham thresholds
  threshold;

  // ----- find longest and shortest axis
  dy = y2 - y1;                         //vertical distance
  dx = x2 - x1;                         //horizontal distance
  longest = max(abs(dy), abs(dx));      //longest axis
  shortest = min(abs(dy), abs(dx));  
  //shortest axis
  // ----- scale Bresenham values by 2*longest
  error = -longest;                     //add offset to so we can test at zero
  threshold = 0;  
  Serial.print("f");                      //test now done at zero
  maximum = (longest << 1);             //multiply by two
  slope = (shortest << 1);  
  Serial.print(maximum); THIS_X = (int)((x * SCALE_FACTOR));
  THIS_Y = (int)((y * SCALE_FACTOR));

  // ----- draw a line between these co-ordinates
  draw_line(LAST_X, LAST_Y, THIS_X, THIS_Y);

  // ----- remember last rounded co-ordinate
  LAST_X = THIS_X;
  LAST_Y = THIS_Y;
  Serial.print("##");
  x3=0;
  /*
     The XY axes are automatically swapped by using "longest" in
     the "for loop". XYswap is used to decode the motors.
  */
  bool XYswap = true;                    //used for motor decoding
  if (abs(dx) >= abs(dy)) XYswap = false;

  for (int i = 0; i < longest; i++) {

    // ----- move left/right along X axis
    if (XYswap) {   //swap
      if (dy < 0) {
        y--;
      } else {
        y++;
      }
    } else {    //no swap
      if (dx < 0) {
        x--;
      } else {
        x++;
      }
    }

    // ----- move up/down Y axis
    error += slope;
    if (error > threshold) {
      error -= maximum;
       
      // ----- move up/down along Y axis
      if (XYswap) {  //swap
        if (dx < 0) {
          x--;
        } else {
          x++;
        }
      } else {  //no swap
        if (dy < 0) {
          y--;
        } else {
          y++;
        }
      }
    }

    // ----- plot the next rounded XY co-ordinate
    plot(x, y);
  }
}

//----------------------------------------------------------------------------------------
// PLOT
void plot(int x, int y ) {
  
  // ----- locals
  float
  x_axis = (float)x,                  //calculate_steps() requires a float
  y_axis = (float)y;

  long
  current_steps1 = STEPS1,            //current motor steps
  current_steps2 = STEPS2,
  steps1,                             //extra motor steps to reach this co-ordinate
  steps2;

  bool
  direction1,                         //motor directions
  direction2;

  long
  current_time,                       //system time
  previous_time1,                     //previous system time for motor1
  previous_time2;                     //previous system time for motor2

  // ----- calculate extra motor steps
  calculate_steps(x_axis, y_axis);              //calculate fresh values for STEPS1, STEPS2
  steps1 = abs(STEPS1 - current_steps1);        //extra steps required
  steps2 = abs(STEPS2 - current_steps2);
  Serial.print("?????");        //extra steps required
  Serial.print(steps1);
  Serial.print("---");
  // ----- calculate the motor directions
  direction1 = (STEPS1 > current_steps1) ? CW : CCW;
  direction2 = (STEPS2 > current_steps2) ? CW : CCW;

  // ----- calculate motor delays for the extra steps
  calculate_delays(steps1, steps2);

  // ----- preload the timers and counters
  previous_time1 = micros();                      //reset the timer
  previous_time2 = micros();                      //reset the timer

  // ----- now step the motors
  /*
     steps1 and steps2 are now used as down-counters
  */
  while ((steps1 != 0) || (steps2 != 0)) {        //stop when both counters equal zero
    // ----- step motor1
    if (steps1 > 0) {                             //prevent additional step ... it occasionally happens!
      current_time = micros();
      if (current_time - previous_time1 > DELAY1) {
        previous_time1 = current_time;              //reset timer
        steps1--;                                   //decrement counter1
        step_motor1(direction1); THIS_X = (int)((x * SCALE_FACTOR));
  THIS_Y = (int)((y * SCALE_FACTOR));

  // ----- draw a line between these co-ordinates
  draw_line(LAST_X, LAST_Y, THIS_X, THIS_Y);

  // ----- remember last rounded co-ordinate
  LAST_X = THIS_X;
  LAST_Y = THIS_Y;
      }
    }
    // ----- step motor2
    if (steps2 > 0) {                             //prevent additional step ... it occasionally happens!
      current_time = micros();
      if (current_time - previous_time2 > DELAY2) {
        previous_time2 = current_time;              //reset timer
        steps2--;                                   //decrement counter2
        step_motor2(direction2);
      }
    }
  }
}

//----------------------------------------------------------------------------------------
// CALCULATE STEPS

void calculate_steps(float x, float y) {

  // ----- locals
  float
  distance,             //pen distance to motors
  angle1,               //motor1 angle
  angle2;               //motor2 angle

  // ----- calculate distances
  distance = sqrt((OFFSET - x) * (OFFSET - x) + (YAXIS - y) * (YAXIS - y));

  // ----- calculate motor1 angle when pen at (x,y)
  if (x > OFFSET) {
    angle1 = PI + acos(distance / (2 * LENGTH)) - atan((x - OFFSET) / (YAXIS - y)); //radians
  } else {
    angle1 = PI + acos(distance / (2 * LENGTH)) + atan((OFFSET - x) / (YAXIS - y)); //radians
  }

  // ----- calculate motor2 angle when pen at (x,y)
  if (x > OFFSET) {
    angle2 = PI - acos(distance / (2 * LENGTH)) - atan((x - OFFSET) / (YAXIS - y)); //radians
  } else {
    angle2 = PI - acos(distance / (2 * LENGTH)) + atan((OFFSET - x) / (YAXIS - y)); //radians
  }
 Serial.print(angle1);
 Serial.print("---");
 Serial.print(angle2);
 Serial.print("++++");
  // ----- calculate steps required to reach (x,y) from 12 o'clock
  STEPS1 = int(angle1 * RAD_TO_DEG * STEPS_PER_DEG);
  STEPS2 = int(angle2 * RAD_TO_DEG * STEPS_PER_DEG);
  STEPS1 = a ;
   Serial.print(STEPS1);
 Serial.print("____");
 Serial.print(STEPS2);
}

//---------------------------------------------------------------------------
// CALCULATE DELAYS
//---------------------------------------------------------------------------
/*
   Assigns values to DELAY1, DELAY2 ready for next pen move
*/
void calculate_delays(long steps1, long steps2) {

  // ----- locals
  float
  rotate_time;

  long
  min_steps,
  max_steps,
  delay_max;

  // ----- find max and min number of steps
  max_steps = max(steps1, steps2);

  min_steps = min(steps1, steps2);

  // ----- calculate the total time for to complete the move
  rotate_time = (float)(max_steps * DELAY_MIN);

  // ----- calculate delay for motor with min_steps
  if (min_steps < 1) min_steps = 1;   //prevent possible divide by zero
  delay_max = (long)(rotate_time / ((float)min_steps));

  // ----- assign delays to each motor
  DELAY1 = (steps1 > steps2) ? DELAY_MIN : delay_max;
  DELAY2 = (steps1 > steps2) ? delay_max : DELAY_MIN;
}

//----------------------------------------------------------------------------
// DRAW ARC CLOCKWISE (G02)
//----------------------------------------------------------------------------
void draw_arc_cw(float x, float y, float i, float j) {

  // ----- inkscape sometimes produces some crazy values for i,j
  if ((i < -100) || (i > 100) || (j < -100) || (j > 100)) {
    move_to(x, y);
  } else {

    // ----- variables
    float
    thisX = LAST_X / SCALE_FACTOR, //current unscaled X co-ordinate
    thisY = LAST_Y / SCALE_FACTOR, //current unscaled Y co-ordinate
    nextX = x,                    //next X co-ordinate
    nextY = y,                    //next Y co-ordinate
    newX,                         //interpolated X co-ordinate
    newY,                         //interpolated Y co-ordinate
    I = i,                        //horizontal distance thisX from circle center
    J = j,                        //vertical distance thisY from circle center
    circleX = thisX + I,          //circle X co-ordinate
    circleY = thisY + J,          //circle Y co-ordinate
    delta_x,                      //horizontal distance between thisX and nextX
    delta_y,                      //vertical distance between thisY and nextY
    chord,                        //line_length between lastXY and nextXY
    radius,                       //circle radius
    alpha,                        //interior angle of arc
    beta,                         //fraction of alpha
    arc,                          //subtended by alpha
    current_angle,                //measured CCW from 3 o'clock
    next_angle;                   //measured CCW from 3 o'clock

    // ----- calculate arc
    delta_x = thisX - nextX;
    delta_y = thisY - nextY;
    chord = sqrt(delta_x * delta_x + delta_y * delta_y);
    radius = sqrt(I * I + J * J);
    alpha = 2 * asin(chord / (2 * radius)); //see construction lines
    arc = alpha * radius;         //radians

    // ----- sub-divide alpha
    int segments = 1;
    if (arc > ARC_MAX) {
      segments = (int)(arc / ARC_MAX);
      beta = alpha / segments;
    } else {
      beta = alpha;
    }

    // ----- calculate current angle
    /*
      atan2() angles between 0 and PI are CCW +ve from 3 o'clock.
      atan2() angles between 2*PI and PI are CW -ve relative to 3 o'clock
    */
    current_angle = atan2(-J, -I);
    if (current_angle <= 0) current_angle += 2 * PI; //angles now 360..0 degrees CW

    // ----- plot intermediate CW co-ordinates
    next_angle = current_angle;                             //initialise angle
    for (int segment = 1; segment < segments; segment++) {
      next_angle -= beta;                                   //move CW around circle
      if (next_angle < 0) next_angle += 2 * PI;             //check if angle crosses zero
      newX = circleX + radius * cos(next_angle);            //standard circle formula
      newY = circleY + radius * sin(next_angle);
      move_to(newX, newY);
    }

    // ----- draw final line
    move_to(nextX, nextY);
  }
}

//----------------------------------------------------------------------------
// DRAW ARC COUNTER-CLOCKWISE (G03)
//----------------------------------------------------------------------------
/*
   We know the start and finish co-ordinates which allows us to calculate the
   chord length. We can also calculate the radius using the biarc I,J values.
   If we bisect the chord the center angle becomes 2*asin(chord/(2*radius)).
   The arc length may now be calculated using the formula arc_length = radius*angle.
*/
void draw_arc_ccw(float x, float y, float i, float j) {

  // ----- inkscape sometimes produces some crazy values for i,j
  if ((i < -100) || (i > 100) || (j < -100) || (j > 100)) {
    move_to(x, y);
  } else {

    // ----- variables
    float
    thisX = LAST_X / SCALE_FACTOR,  //current unscaled X co-ordinate
    thisY = LAST_Y / SCALE_FACTOR,  //current unscaled Y co-ordinate
    nextX = x,                      //next X co-ordinate
    nextY = y,                      //next Y co-ordinate
    newX,                           //interpolated X co-ordinate
    newY,                           //interpolated Y co-ordinate
    I = i,                          //horizontal distance thisX from circle center
    J = j,                          //vertical distance thisY from circle center
    circleX = thisX + I,            //circle X co-ordinate
    circleY = thisY + J,            //circle Y co-ordinate
    delta_x,                        //horizontal distance between thisX and nextX
    delta_y,                        //vertical distance between thisY and nextY
    chord,                          //line_length between lastXY and nextXY
    radius,                         //circle radius
    alpha,                          //interior angle of arc
    beta,                           //fraction of alpha
    arc,                            //subtended by alpha
    current_angle,                  //measured CCW from 3 o'clock
    next_angle;                     //measured CCW from 3 o'clock

    // ----- calculate arc
    delta_x = thisX - nextX;
    delta_y = thisY - nextY;
    chord = sqrt(delta_x * delta_x + delta_y * delta_y);
    radius = sqrt(I * I + J * J);
    alpha = 2 * asin(chord / (2 * radius));     //see construction lines
    arc = alpha * radius;                       //radians

    // ----- sub-divide alpha
    int segments = 1;
    if (arc > ARC_MAX) {
      segments = (int)(arc / ARC_MAX);
      beta = alpha / segments;
    } else {
      beta = alpha;
    }

    // ----- calculate current angle
    /*
        tan2() angles between 0 and PI are CCW +ve from 3 o'clock.
        atan2() angles between 2*PI and PI are CW -ve relative to 3 o'clock
    */
    current_angle = atan2(-J, -I);
    if (current_angle <= 0) current_angle += 2 * PI;        //angles now 360..0 degrees CW

    // ----- plot intermediate CCW co-ordinates
    next_angle = current_angle;                             //initialise angle
    for (int segment = 1; segment < segments; segment++) {
      next_angle += beta;                                   //move CCW around circle
      if (next_angle > 2 * PI) next_angle -= 2 * PI;        //check if angle crosses zero
      newX = circleX + radius * cos(next_angle);            //standard circle formula
      newY = circleY + radius * sin(next_angle);
      move_to(newX, newY);
    }

    // ----- draw final line
    move_to(nextX, nextY);
  }
}

//--------------------------------------------------------------------
// STEP MOTOR1
//--------- -----------------------------------------------------------
void step_motor1(bool dir1) {
  if (dir1 == CW) {
    step1_cw();
  } else {
    step1_ccw();
  }
}

//--------------------------------------------------------------------
// STEP MOTOR2
//--------- -----------------------------------------------------------
void step_motor2(bool dir2) {
  if (dir2 == CW) {
    step2_cw();
  } else {
    step2_ccw();
  }
}

//--------------------------------------------------------------------
// STEP1_CW        (step actuator motor1 clockwise)
//--------- -----------------------------------------------------------
void step1_cw() {
  Serial.print(CW);
  //----- set direction
  digitalWrite(DIR1, CW);
  delayMicroseconds(PULSE_WIDTH);

  //----- step motor
  digitalWrite(STEP1, HIGH);
  delayMicroseconds(PULSE_WIDTH);
  digitalWrite(STEP1, LOW);
  delayMicroseconds(PULSE_WIDTH);
}

//--------------------------------------------------------------------
// STEP1_CCW        (step actuator motor1 counter-clockwise)
//--------- -----------------------------------------------------------
void step1_ccw() {

  //----- set direction
  digitalWrite(DIR1, CCW);
  delayMicroseconds(PULSE_WIDTH);

  //----- step motor
  digitalWrite(STEP1, HIGH);
  delayMicroseconds(PULSE_WIDTH);
  digitalWrite(STEP1, LOW);
  delayMicroseconds(PULSE_WIDTH);
}

//--------------------------------------------------------------------
// STEP2_CW        (step actuator motor2 clockwise)
//--------- -----------------------------------------------------------
void step2_cw() {

  //----- set direction
  digitalWrite(DIR2, CW);
  delayMicroseconds(PULSE_WIDTH);

  //----- step motor
  digitalWrite(STEP2, HIGH);
  delayMicroseconds(PULSE_WIDTH);
  digitalWrite(STEP2, LOW);
  delayMicroseconds(PULSE_WIDTH);
}

//--------------------------------------------------------------------
// STEP2_CCW        (step actuator motor2 counter-clockwise)
//--------- -----------------------------------------------------------
void step2_ccw() {

  //----- set direction
  digitalWrite(DIR2, CCW);
  delayMicroseconds(PULSE_WIDTH);

  //----- step motor
  digitalWrite(STEP2, HIGH);
  delayMicroseconds(PULSE_WIDTH);
  digitalWrite(STEP2, LOW);
  delayMicroseconds(PULSE_WIDTH);
}

//---------------------------------------------------------------------------
// PEN_UP
// Raise the pen
// Changing the value in OCR2B changes the pulse-width to the SG-90 servo
//---------------------------------------------------------------------------
void pen_up() {
  OCR2B = 148;                //1mS pulse
  delay(250);                 //give pen-lift time to respond
}

//---------------------------------------------------------------------------
// PEN_DOWN
// Lower the pen
// Changing the value in OCR2B changes the pulse-width to the SG-90 servo
//---------------------------------------------------------------------------
void pen_down() {
  OCR2B = 140;                //2mS pulse
  delay(250);                 //give pen-lift time to respond
}


// ----------------------------------------
// ABC
// ----------------------------------------
void abc() {

  // ----- letter C
  pen_up();
  move_to( 46.077581, 8.038555 );
  pen_down();
  move_to( 44.484913, 8.164859 );
  move_to( 43.092675, 8.514975 );
  move_to( 41.810333, 9.119737 );
  move_to( 40.749474, 9.934507 );
  move_to( 39.898294, 10.964664 );
  move_to( 39.222991, 12.287431 );
  move_to( 38.836654, 13.724444 );
  move_to( 38.688233, 15.544577 );
  move_to( 38.827317, 17.240735 );
  move_to( 39.203543, 18.665602 );
  move_to( 39.848521, 19.983646 );
  move_to( 40.700859, 21.067141 );
  move_to( 41.728238, 21.900614 );
  move_to( 43.034337, 22.554734 );
  move_to( 44.459619, 22.940826 );
  move_to( 46.087304, 23.079767 );
  move_to( 46.999087, 23.050125 );
  move_to( 47.730461, 22.972816 );
  move_to( 48.465078, 22.853756 );
  move_to( 49.091658, 22.710298 );
  move_to( 49.735578, 22.504820 );
  move_to( 50.268119, 22.292217 );
  move_to( 50.799881, 22.061981 );
  move_to( 51.201509, 21.874136 );
  move_to( 51.201509, 18.364194 );
  move_to( 50.773706, 18.364194 );
  move_to( 50.497832, 18.593065 );
  move_to( 50.083384, 18.918397 );
  move_to( 49.664637, 19.224080 );
  move_to( 49.149994, 19.550380 );
  move_to( 48.602240, 19.833093 );
  move_to( 47.992979, 20.075412 );
  move_to( 47.361382, 20.233433 );
  move_to( 46.660952, 20.289314 );
  move_to( 45.884644, 20.224242 );
  move_to( 45.163636, 20.036522 );
  move_to( 44.498101, 19.720535 );
  move_to( 43.851054, 19.219805 );
  move_to( 43.355966, 18.613137 );
  move_to( 42.917663, 17.751657 );
  move_to( 42.668402, 16.819076 );
  move_to( 42.567640, 15.534855 );
  move_to( 42.678469, 14.195736 );
  move_to( 42.946831, 13.259712 );
  move_to( 43.413758, 12.401478 );
  move_to( 43.919115, 11.820734 );
  move_to( 45.241419, 11.052632 );
  move_to( 45.978754, 10.883200 );
  move_to( 46.680397, 10.829005 );
  move_to( 47.353314, 10.879912 );
  move_to( 48.022146, 11.033184 );
  move_to( 48.674696, 11.277806 );
  move_to( 49.256945, 11.587387 );
  move_to( 49.723404, 11.883902 );
  move_to( 50.141723, 12.190202 );
  move_to( 50.546004, 12.515932 );
  move_to( 50.812596, 12.744402 );
  move_to( 51.201509, 12.744402 );
  move_to( 51.201509, 9.283076 );
  move_to( 50.656313, 9.041645 );
  move_to( 50.161168, 8.826105 );
  move_to( 49.659311, 8.628995 );
  move_to( 49.120826, 8.456637 );
  move_to( 48.406883, 8.270693 );
  move_to( 47.788799, 8.145506 );
  move_to( 47.163646, 8.072462 );
  move_to( 46.077581, 8.038555 );
  move_to( 46.077581, 8.038555 );
  pen_up();

  // ----- letter B
  move_to( 36.753391, 12.754125 );
  pen_down();
  move_to( 36.638363, 11.732067 );
  move_to( 36.325587, 10.877621 );
  move_to( 35.824865, 10.121077 );
  move_to( 35.168569, 9.506701 );
  move_to( 34.268011, 8.957841 );
  move_to( 33.282343, 8.592755 );
  move_to( 32.250726, 8.402320 );
  move_to( 30.686347, 8.320518 );
  move_to( 24.366511, 8.320518 );
  move_to( 24.366511, 22.797804 );
  move_to( 29.986303, 22.797804 );
  move_to( 31.733546, 22.757964 );
  move_to( 32.543406, 22.681131 );
  move_to( 33.339527, 22.498523 );
  move_to( 34.157395, 22.165821 );
  move_to( 34.894695, 21.654645 );
  move_to( 35.382471, 21.037973 );
  move_to( 35.681264, 20.308309 );
  move_to( 35.790830, 19.433707 );
  move_to( 35.644134, 18.429163 );
  move_to( 35.236630, 17.576648 );
  move_to( 34.591894, 16.889091 );
  move_to( 33.671256, 16.332124 );
  move_to( 33.671256, 16.254343 );
  move_to( 34.990077, 15.791394 );
  move_to( 35.917229, 15.087603 );
  move_to( 36.517941, 14.109174 );
  move_to( 36.753391, 12.754125 );
  move_to( 36.753391, 12.754125 );
  pen_up();

  move_to( 32.883707, 12.812463 );
  pen_down();
  move_to( 32.799937, 13.475254 );
  move_to( 32.611467, 13.872250 );
  move_to( 32.293975, 14.174883 );
  move_to( 31.707244, 14.436175 );
  move_to( 31.264089, 14.524544 );
  move_to( 30.501613, 14.572293 );
  move_to( 29.733495, 14.579493 );
  move_to( 28.897349, 14.582030 );
  move_to( 28.080630, 14.582030 );
  move_to( 28.080630, 10.974863 );
  move_to( 28.352869, 10.974863 );
  move_to( 29.927944, 10.978253 );
  move_to( 30.608564, 10.984572 );
  move_to( 31.276168, 11.055991 );
  move_to( 31.862810, 11.237364 );
  move_to( 32.385104, 11.547787 );
  move_to( 32.660082, 11.879072 );
  move_to( 32.883707, 12.812463 );
  move_to( 32.883707, 12.812463 );
  pen_up();

  move_to( 31.969761, 18.704495 );
  pen_down();
  move_to( 31.925661, 19.053577 );
  move_to( 31.785027, 19.423984 );
  move_to( 31.553429, 19.721988 );
  move_to( 31.153044, 19.958739 );
  move_to( 30.736285, 20.070609 );
  move_to( 30.132147, 20.124028 );
  move_to( 29.529231, 20.137196 );
  move_to( 28.430652, 20.143473 );
  move_to( 28.080630, 20.143473 );
  move_to( 28.080630, 17.080783 );
  move_to( 28.663999, 17.080783 );
  move_to( 29.548576, 17.089343 );
  move_to( 30.171037, 17.109951 );
  move_to( 30.782282, 17.184377 );
  move_to( 31.153044, 17.304408 );
  move_to( 31.599191, 17.587578 );
  move_to( 31.814194, 17.878055 );
  move_to( 31.927043, 18.246639 );
  move_to( 31.969761, 18.704495 );
  move_to( 31.969761, 18.704495 );
  pen_up();

  // ----- letter A
  move_to( 22.266381, 8.320518 );
  pen_down();
  move_to( 18.406419, 8.320518 );
  move_to( 17.404968, 11.247086 );
  move_to( 12.037969, 11.247086 );
  move_to( 11.036518, 8.320518 );
  move_to( 7.273784, 8.320518 );
  move_to( 12.621338, 22.797804 );
  move_to( 16.918827, 22.797804 );
  move_to( 22.266381, 8.320518 );
  pen_up();

  move_to( 16.500745, 13.901420 );
  pen_down();
  move_to( 14.721468, 19.093409 );
  move_to( 12.942191, 13.901420 );
  move_to( 16.500745, 13.901420 );
  pen_up();

  // ----- home
  move_to( 0.0000, 0.0000 );
}

//----------------------------------------------------------------------------
// TARGET test pattern
//----------------------------------------------------------------------------
void target() {

  // ----- circle
  pen_up();
  move_to(130, 100);
  pen_down();
  move_to(128, 88);
  move_to(122, 79);
  move_to(112, 73);
  move_to(100, 70);
  move_to(87, 73);
  move_to(78, 79);
  move_to(71, 88);
  move_to(69, 100);
  move_to(71, 111);
  move_to(78, 123);
  move_to(87, 130);
  move_to(100, 132);
  move_to(112, 130);
  move_to(122, 123);
  move_to(129, 110);
  move_to(130, 100);
  pen_up();

  // ----- back-slash
  pen_up();
  move_to(50, 150);
  pen_down();
  move_to(78, 123);
  move_to(100, 100);
  move_to(123, 79);
  move_to(150, 50);
  pen_up();

  // ----- slash
  pen_up();
  move_to(50, 50);
  pen_down();
  move_to(78, 79);
  move_to(100, 100);
  move_to(122, 123);
  move_to(150, 150);
  pen_up();

  // ----- square
  pen_up();
  move_to(50, 150);
  pen_down();
  move_to(100, 150);
  move_to(150, 150);
  move_to(150, 100);
  move_to(150, 50);
  move_to(100, 50);
  move_to(50, 50);
  move_to(50, 100);
  move_to(50, 150);
  pen_up();

  // ------ home
  move_to(0.0000, 0.0000);
}

//----------------------------------------------------------------------------
// RADIALS test pattern
//----------------------------------------------------------------------------
void radials() {

  // ----- move to the centre of the square
  pen_up();
  move_to(100, 100);

  // ----- draw octant 0 radials
  pen_down();
  move_to(150, 100);
  pen_up();
  move_to(100, 100);

  pen_down();
  move_to(150, 125);
  pen_up();
  move_to(100, 100);

  pen_down();
  move_to(150, 150);
  pen_up();
  move_to(100, 100);

  // ----- draw octant 1 radials
  pen_down();
  move_to(125, 150);
  pen_up();
  move_to(100, 100);

  pen_down();
  move_to(100, 150);
  pen_up();
  move_to(100, 100);

  // ----- draw octant 2 radials
  pen_down();
  move_to(75, 150);
  pen_up();
  move_to(100, 100);

  pen_down();
  move_to(50, 150);
  pen_up();
  move_to(100, 100);

  // ----- draw octant 3 radials
  pen_down();
  move_to(50, 125);
  pen_up();
  move_to(100, 100);

  pen_down();
  move_to(50, 100);
  pen_up();
  move_to(100, 100);

  // ----- draw octant 4 radials
  pen_down();
  move_to(50, 75);
  pen_up();
  move_to(100, 100);

  pen_down();
  move_to(50, 50);
  pen_up();
  move_to(100, 100);

  // ----- draw octant 5 radials
  pen_down();
  move_to(75, 50);
  pen_up();
  move_to(100, 100);

  pen_down();
  move_to(100, 50);
  pen_up();
  move_to(100, 100);

  // ----- draw octant 6 radials
  pen_down();
  move_to(125, 50);
  pen_up();
  move_to(100, 100);

  pen_down();
  move_to(150, 50);
  pen_up();
  move_to(100, 100);

  // ----- draw octant 7 radials
  pen_down();
  move_to(150, 75);
  pen_up();
  move_to(100, 100);
  pen_up();

  // ----- draw box
  move_to(50, 50);
  pen_down();
  move_to(50, 150);
  move_to(150, 150);
  move_to(150, 50);
  move_to(50, 50);
  pen_up();

  // home --------------
  move_to(0.0000, 0.0000);
}