// Automatic homing against the limit switch on EBB pin B1, and origin-offset

// bookkeeping (port of nextdrawcore/homing.py).

//

// The switch is normally closed: B1 reads 0 when present and not pressed,

// 1 when pressed (or absent).

import { absMoveWrapper, exhaustQueue, QG_MOTION, readStatusByte, readStepOffsets, readStepPosition,

  writeStepOffsets } from './control.js';

import { distance, pyInt, pyRound } from './calc.js';

/** XY inches -> absolute (A, B) motor step position. */

export function xyToStepPos(nd, x, y) {

  if (nd.params.resolution === 2) {

    x /= 2;

    y /= 2;

  }

  const k = 2 * nd.params.nativeResFactor;

  return [pyRound(k * (x + y)), pyRound(k * (x - y))];

}

/** Absolute (A, B) motor step position -> XY inches. */

export function stepsToXyPos(nd, a, b) {

  let x = (a + b) / (4 * nd.params.nativeResFactor);

  let y = (a - b) / (4 * nd.params.nativeResFactor);

  if (nd.params.resolution === 2) {

    x *= 2;

    y *= 2;

  }

  return [x, y];

}

const SPEED_FAST = 2; // in/s, coarse

const SPEED_FINE = 0.25; // in/s

const SIDE_DIST = 6; // mm

export class Homing {

  constructor(nd) {

    this.nd = nd;

    this.failed = false;

    this.paused = false;

    this.stepScale = null;

    this.res = 0;

  }

  message(text) {

    this.nd.log(text);

  }

  /**

   * Home the machine (if supported and not already homed) and set the

   * step counter origin. Returns true on success.

   */

  async findHome() {

    const nd = this.nd;

    this.failed = false;

    this.paused = false;

    if (nd.options.preview) return true;

    const autoHoming = nd.options.homing && nd.params.autoHome;

    if (!nd.machine.connected || nd.machine.err != null) {

      await this.markFailed();

      return false;

    }

    if (await nd.machine.varRead(12)) { // Already homed

      await this.readPosition();

      return true;

    }

    if (autoHoming) {

      if (nd.warnings.has('voltage')) {

        nd.warn('homing_voltage');

        await this.markFailed();

        return false;

      }

      if (nd.params.skipVoltageCheck && !(await nd.machine.queryVoltage(200))) {

        nd.warn('homing_voltage');

        await this.markFailed();

        return false;

      }

      this.stepScale = nd.stepScale;

      this.res = nd.params.resolution === 1 ? 1 : 2;

      await this.rhmHoming();

      if (this.failed) {

        await this.markFailed();

        return false;

      }

      await nd.machine.clearSteps();

      await this.lhmHoming();

    }

    if (this.failed) {

      await this.markFailed();

      return false;

    }

    await this.setHome();

    return true;

  }

  async markFailed() {

    const nd = this.nd;

    this.failed = true;

    if (nd.plotStatus.stopped === 0) {

      nd.plotStatus.stopped = nd.warnings.has('voltage') ? 105 : 106;

    }

    if (!nd.machine.connected || nd.machine.err != null) return;

    await nd.machine.varWrite(0, 12);

  }

  /** Wait for motion to finish; E-stop on button press. */

  async block(timeoutMs = null) {

    const nd = this.nd;

    if (this.failed) return;

    let timeLeft = timeoutMs == null ? 1000 : timeoutMs;

    for (;;) {

      const qg = await readStatusByte(nd);

      if (nd.plotStatus.button) {

        this.message('Homing interrupted by button press.');

        this.paused = true;

        this.failed = true;

        await nd.machine.query('ES');

        return;

      }

      if (qg == null) {

        this.failed = true;

        this.message('Error reading status byte during homing.');

        return;

      }

      if ((qg & QG_MOTION) === 0) return;

      if (timeLeft <= 0) {

        this.failed = true;

        this.message('Timeout at block function in homing routine.');

        return;

      }

      if (timeLeft < 10) {

        await nd.clock.sleep(timeLeft);

        timeLeft = 0;

      } else {

        await nd.clock.sleep(2);

        if (timeoutMs != null) timeLeft -= 2;

      }

    }

  }

  /** 1 if switch pressed (or absent), 0 if present and open, 2 on error. */

  async queryLimitSwitch() {

    const nd = this.nd;

    if (!nd.machine.connected || nd.machine.err != null) return 2;

    await this.block();

    const response = await nd.machine.dioBRead(1);

    if (response == null) {

      this.message('Error reading serial data in Homing process.');

      this.failed = true;

      return 2;

    }

    return response ? 1 : 0;

  }

  async moveXyInch(x, y, timeMs) {

    if (this.failed || this.paused) return;

    const steps1 = pyRound(this.stepScale * (x + y));

    const steps2 = pyRound(this.stepScale * (x - y));

    await this.nd.machine.xyMove(steps2, steps1, timeMs);

  }

  /**

   * Move left with only the right-hand motor until the limit switch trips.

   * Returns the apparent distance moved (inches), or -1 on failure.

   */

  async leftUntilBump(speed, maxDist) {

    const nd = this.nd;

    if (this.failed || this.paused) {

      this.failed = true;

      return -1;

    }

    if ((await this.queryLimitSwitch()) === 1) {

      this.failed = true;

      this.message('Automatic homing failed. (Error: Limit switch not ready.)');

      return -1;

    }

    if (nd.warnings.has('voltage')) {

      nd.warn('homing_voltage');

      return -1;

    }

    await this.block();

    await nd.machine.motorsEnable(this.res, 0); // Freewheel the left-hand motor

    await this.block();

    await this.enableLimitDetection();

    await this.moveXyInch(-maxDist, 0, Math.abs(pyInt(1000 * maxDist / speed)));

    // Until Y = 0 the carriage actually moves diagonally, at half the distance.

    await this.block();

    const limitOccurred = nd.plotStatus.limit;

    await nd.machine.command('CU,51,0'); // Disable limit switch detection

    if (!limitOccurred && !this.paused) {

      this.failed = true;

      this.message('Automatic homing failed with a "limit not found" error. ' +

        `This could indicate the wrong plotter model is selected (currently ${nd.params.modelName}).`);

      return -1;

    }

    // EM cleared the step counter before the move, so this is this move's count.

    const steps = await nd.machine.querySteps();

    if (steps == null) {

      this.failed = true;

      return -1;

    }

    return Math.abs(steps[1] / this.stepScale);

  }

  /** Back off right 6 mm, left 3.5 mm, then fine-approach the switch (<= 7 mm). */

  async rhmHomingFine() {

    const nd = this.nd;

    if (this.failed || this.paused) return -1;

    const leftCoarseDist = 7; // mm

    await nd.machine.motorsEnable(this.res, this.res);

    let step = 6 / 25.4;

    await this.moveXyInch(step, 0, pyInt(1000 * Math.abs(step / SPEED_FAST)));

    if ((await this.queryLimitSwitch()) === 1) {

      this.failed = true;

      this.message('Automatic homing failed. (Error: Limit switch not detected.)');

      return -1;

    }

    step = (leftCoarseDist / 2) / 25.4;

    await this.moveXyInch(-step, 0, pyInt(1000 * Math.abs(step / SPEED_FAST)));

    if ((await this.queryLimitSwitch()) === 1) {

      this.failed = true;

      this.message('Automatic homing failed. (Error: Limit switch position error.)');

    }

    if (this.failed) return -1;

    return this.leftUntilBump(SPEED_FINE, leftCoarseDist / 25.4);

  }

  /** Right-hand-motor homing: find X = 0 with Y = 0 (see homing.py for stages). */

  async rhmHoming() {

    const nd = this.nd;

    const [xMax, yMax] = nd.bounds[1];

    await nd.machine.dioBConfig(1, 1, 1); // B1 as input, initially high

    await nd.machine.command('CU,50,0');

    let firstCoarseDist;

    if ((await this.queryLimitSwitch()) === 1) {

      firstCoarseDist = 0.05; // Switch already pressed; skip the first coarse move.

    } else {

      // Twice the diagonal (one motor moves at half distance) plus any extra X travel.

      const maxDist = 2 * yMax + (xMax - yMax) + 0.5;

      const apparent = await this.leftUntilBump(SPEED_FAST, maxDist);

      if (this.failed) return;

      firstCoarseDist = apparent / 2;

    }

    let dist = await this.rhmHomingFine(); // ~2.5 mm (0.0984 in) if Y = 0

    if (this.failed) return;

    const firstDistClose = dist < 0.15;

    dist = await this.rhmHomingFine();

    if (dist < 0) {

      this.failed = true;

      return;

    }

    if (dist <= 0.0984) return;

    if (firstDistClose && dist < 0.12) return;

    if (dist < 0.15) {

      const dist2 = await this.rhmHomingFine();

      if (dist2 > 0.12) {

        this.failed = true;

        this.message('Homing failed');

      }

      return;

    }

    // Not yet at Y = 0: big move right, then coarse and fine approach again.

    let remaining = yMax - firstCoarseDist - 2.5 / 25.4;

    remaining = Math.max(remaining, 0);

    await nd.machine.motorsEnable(this.res, this.res);

    await this.moveXyInch(remaining, 0, pyInt(1000 * remaining / SPEED_FAST));

    const coarse = await this.leftUntilBump(SPEED_FAST, 2 * (remaining + 0.1));

    if (coarse < 0) {

      this.message('Error: Homing failed at second coarse move; no limit found.');

      this.failed = true;

      return;

    }

    dist = await this.rhmHomingFine();

    if (dist < 0) {

      this.message('Error; limit not found');

      return;

    }

    if (dist <= 0.0984) return;

    if (dist < 0.15) {

      const dist2 = await this.rhmHomingFine();

      if (dist2 > 0.12) {

        this.message('Error: Homing failed (not found; secondary)');

        this.failed = true;

      }

    }

  }

  /** Left-hand-motor precision homing: step out diagonally, approach, return. */

  async lhmHoming() {

    const nd = this.nd;

    await nd.machine.motorsEnable(this.res, this.res);

    let step = SIDE_DIST / 25.4;

    const motorSteps1 = pyRound(this.stepScale * (step + step));

    let timeMs = pyInt(1000 * Math.abs(step / SPEED_FAST));

    await nd.machine.xyMove(0, motorSteps1, timeMs);

    step = (SIDE_DIST / 2) / 25.4; // First half of the leftward move, fast

    const timeMs1 = pyInt(1000 * Math.abs(step / SPEED_FAST));

    let motorSteps2 = pyRound(this.stepScale * (-2 * step));

    await nd.machine.xyMove(motorSteps2, 0, timeMs1);

    step = SIDE_DIST / 25.4; // Second half, slow, until the switch

    timeMs = pyInt(1000 * Math.abs(step / SPEED_FINE));

    motorSteps2 = pyRound(this.stepScale * (-2 * step));

    if ((await this.queryLimitSwitch()) === 1) {

      this.failed = true;

      this.message('Automatic homing failed. (Limit switch not ready; Precision stage)');

      return;

    }

    await this.enableLimitDetection();

    await nd.machine.xyMove(motorSteps2, 0, timeMs);

    await this.block();

    const limitOccurred = nd.plotStatus.limit;

    await nd.machine.command('CU,51,0');

    if (!limitOccurred) {

      this.failed = true;

      this.message('Automatic homing failed. (Limit not found; precision stage)');

      return;

    }

    await nd.machine.xyMove(motorSteps1, -motorSteps1, 4 * timeMs1);

    await this.block();

  }

  async enableLimitDetection() {

    await this.nd.machine.command('CU,52,2'); // Target: B1 high

    await this.nd.machine.command('CU,51,2'); // Mask: B1

    this.nd.plotStatus.limit = false;

  }

  /** Set nd.pen.phys from the EBB step counter, minus the origin offset. */

  async readPosition() {

    const nd = this.nd;

    if (nd.options.preview) {

      nd.pen.phys.accum1 = 0;

      nd.pen.phys.accum2 = 0;

    }

    if (!nd.machine.connected || nd.machine.err != null) return;

    await exhaustQueue(nd);

    const pos = await nd.machine.querySteps();

    const offset = await readStepOffsets(nd);

    if (pos == null || offset == null) {

      this.message('Error reading step positions.');

      return;

    }

    const offsetXy = stepsToXyPos(nd, offset[0] / 1000, offset[1] / 1000);

    const posXy = stepsToXyPos(nd, pos[0], pos[1]);

    nd.pen.phys.xpos = posXy[0] - offsetXy[0];

    nd.pen.phys.ypos = posXy[1] - offsetXy[1];

    nd.pen.phys.accum1 = 0;

    nd.pen.phys.accum2 = 0;

    await nd.machine.clearAccumulators();

  }

  /** Zero the step counter here, clear offsets, and mark the machine homed. */

  async setHome() {

    const nd = this.nd;

    await exhaustQueue(nd);

    await nd.machine.clearSteps();

    await writeStepOffsets(nd, 0, 0);

    await nd.machine.varWrite(1, 12);

    await this.readPosition();

  }

  /** Shift the origin offset by (dx, dy) inches. */

  async adjustOriginOffset(dx, dy) {

    const nd = this.nd;

    const offset = await readStepOffsets(nd);

    if (offset == null) {

      this.message('Error reading step positions.');

      return;

    }

    const offsetXy = stepsToXyPos(nd, offset[0] / 1000, offset[1] / 1000);

    const ab = xyToStepPos(nd, (offsetXy[0] + dx) * 1000, (offsetXy[1] + dy) * 1000);

    await writeStepOffsets(nd, ab[0], ab[1]);

    await this.readPosition();

  }

  async xyToStepPosWithOffset(x, y) {

    const offset = await readStepOffsets(this.nd);

    if (offset == null) return null;

    const offsetXy = stepsToXyPos(this.nd, offset[0] / 1000, offset[1] / 1000);

    return xyToStepPos(this.nd, x + offsetXy[0], y + offsetXy[1]);

  }

  /** Absolute HM move to (x, y) inches relative to the origin offset. */

  async precisionMoveTo(x, y, rate = 3000) {

    const nd = this.nd;

    const dest = await this.xyToStepPosWithOffset(x, y);

    if (dest == null) return;

    if (nd.options.preview) {

      nd.plotStatus.stats.addDist(nd, distance(x - nd.pen.phys.xpos, y - nd.pen.phys.ypos));

      nd.pen.phys.xpos = x;

      nd.pen.phys.ypos = y;

      return;

    }

    const read = await readStepPosition(nd);

    if (read == null) return;

    if (dest[0] !== read[0] || dest[1] !== read[1]) {

      await absMoveWrapper(nd, dest[0], dest[1], rate);

    }

    await this.readPosition();

  }

}