#include "length.h"
#include "grid.h"
#include "machine.h"

#include <assert.h>
#include <stdio.h>

machine::machine(grid const& bit, int xyspeed, int zspeed, int maxspeed):
    at(0), x(0), y(0), mat(0), mx(0), my(0), mspeed(0),
    bitx(xsize(bit) / 2), bity(ysize(bit) / 2),
    xyspeed(xyspeed), zspeed(zspeed), maxspeed(maxspeed),
    res_per_meter(parse_length("1m")) {
  printf("IN\n");
}

void machine::move(int x, int y, depth at) { go(x, y, at, maxspeed); }

void machine::cut(int x, int y, depth at) {
  go(x, y, at, at != this->at ? zspeed : xyspeed);
}

void machine::stop() {
  go(x, y, 0, maxspeed);
  finish();
  printf("H\n");
}

void machine::go(int x, int y, depth at, int speed) {
  assert(x >= bitx && y >= bity);
  if (speed != mspeed ||
     (x == mx) != (mx == this->x) ||
     (y == my) != (my == this->y) ||
     (at == mat) != (mat == this->at)) {
    finish();
  }

  this->x = x;
  this->y = y;
  this->at = at;
  mspeed = speed;
}

void machine::finish() {
  if (mx != x || my != y || mat != at) {
    printf("!VZ %d;Z %d,%d,%d\n", mspeed,
           (x - bitx) * 40000 / res_per_meter,
           (y - bity) * 40000 / res_per_meter,
           -at * 40000 / res_per_meter);
    mx = x;
    my = y;
    mat = at;
  }
}