#include <stdio.h>

#include "drive_stop.h"

#include "button.h"
#include "tacho_motor.h"
#include "ir.h"
#include "misc_const.h"

typedef enum ds_state {
  DRIVE,
  STOP
} Ds_state;
Ds_state dss = STOP;

int ds_speed = 150;

static void ds_drive() {
  dss = DRIVE;
  run_forward_forever(ds_speed);
}

static void ds_stop() {
  dss = STOP;
  tacho_stop_both();
}

void drive_stop() {
  /* intro */
  printf("drive-stop started\n");

  int ds_prox;

  ds_drive();
  while (!button_pressed()) {
    ds_prox = ir_get();
    if (dss == DRIVE && ds_prox < 50) {
      printf("DS: object found - stop (%d)\n", ds_prox);
      ds_stop();
    }
    else if (dss == STOP && ds_prox >= 50) {
      printf("DS: path clear - start (%d)\n", ds_prox);
      ds_drive();
    }
    nsleep(200);
  }

  /* outro */
  ds_stop();
  printf("drive-stop stopped\n");
}