#include <stdio.h>
#include <unistd.h>

#include "ev3.h"
#include "ev3_port.h"
#include "ev3_tacho.h"

#include "color.h"
#include "misc_const.h"
#include "tacho_motor.h"

int max_speed;

uint8_t motor[3] = {DESC_LIMIT, DESC_LIMIT, DESC_LIMIT};

int init_motor() {
  bool done = false;
  int attempts = 10;

  for (int i = 0; i < attempts; i++) {
    if (ev3_tacho_init() < 0) {
      sleep(1);
    }
    else {
      done = true;
      break;
    }
  }

  if (done) {
    find_motor();
    return 0;
  }
  else {
    fprintf(stderr,
            "tacho_motor.c: ev3_tacho_init failed (%d attempts)\n",
            attempts);
    return 1;
  }
}

char* find_motor() {
  printf("\nmotor-search-begin\n");

  static char s[STR_LEN];
  for (int i = 0; i < DESC_LIMIT; i++) {
    if (ev3_tacho[i].type_inx != TACHO_TYPE__NONE_) {
      printf("\ttype: %s\n", ev3_tacho_type(ev3_tacho[i].type_inx));
      printf("\tport: %s\n", ev3_tacho_port_name(i, s));
    }
  }

  if (ev3_search_tacho_plugged_in(L_MOTOR_PORT, L_MOTOR_EXT_PORT, motor + LEFT, 0)) {
    get_tacho_max_speed(motor[LEFT], &max_speed);
    set_tacho_command_inx(motor[LEFT], TACHO_RESET);
    printf("\tmax speed L: %d\n", max_speed);
  }
  else {
    fprintf(stderr, "Left motor (%s) not found.\n",
            ev3_port_name(L_MOTOR_PORT, L_MOTOR_EXT_PORT, 0, s));
  }

  if (ev3_search_tacho_plugged_in(R_MOTOR_PORT, R_MOTOR_EXT_PORT, motor + RIGHT, 0)) {
    get_tacho_max_speed(motor[RIGHT], &max_speed);
    set_tacho_command_inx(motor[RIGHT], TACHO_RESET);
    printf("\tmax speed R: %d\n", max_speed);
  }
  else {
    fprintf(stderr, "Right motor (%s) not found.\n",
            ev3_port_name(R_MOTOR_PORT, R_MOTOR_EXT_PORT, 0, s));
  }
  printf("motor-search-end\n\n");
  return s;
}

void set_left_speed (int speed) {
  set_tacho_speed_sp(motor[LEFT], speed);
}
void set_right_speed(int speed) {
  set_tacho_speed_sp(motor[RIGHT], speed);
}

static void tacho_set_speed_perc(motor_side m, int perc) {
  if (0 <= perc && perc <= 100) {
    int speed = max_speed*(perc/100.0);
    printf("set speed: %d\n", speed);
    set_tacho_speed_sp(motor[m], speed);
  }
  else {
    fprintf(stderr,
     "Speed percent (%d) argument outside [0, 100]\n", perc);
  }
}

void run_forward_forever(int speed) {
  run_forever(speed, speed);
}

void run_forever(int left_speed, int right_speed) {
  set_left_speed(left_speed);
  set_right_speed(right_speed);
  multi_set_tacho_command_inx(motor, TACHO_RUN_FOREVER);
}

void run_timed(int left_speed, int right_speed, int ms) {
  set_left_speed(left_speed);
  set_right_speed(right_speed);
  multi_set_tacho_time_sp(motor, ms);
  multi_set_tacho_command_inx(motor, TACHO_RUN_TIMED);
}

void tacho_set_max_speed() {
  get_tacho_max_speed(motor[LEFT], &max_speed);
  printf("Left motor max speed: %d\n", max_speed);
  get_tacho_max_speed(motor[RIGHT], &max_speed);
  printf("Right motor max speed: %d\n", max_speed);
}

void tacho_stop_both() {
  multi_set_tacho_command_inx(motor, TACHO_STOP);
}

static void tacho_stop(motor_side m) {
  set_tacho_stop_action_inx(motor[m], TACHO_COAST);
}

static void tacho_run_timed(motor_side m) {
  set_tacho_command_inx(motor[m], TACHO_RUN_TIMED);
}

static void tacho_set_time_with_ramp(motor_side m, int secs) {
  uint8_t the_motor = motor[m];
  int time_ms    = secs*1000;
  int up_down_ms = time_ms/2;
  set_tacho_ramp_up_sp  (the_motor, up_down_ms);
  set_tacho_time_sp     (the_motor, time_ms);
  set_tacho_ramp_down_sp(the_motor, up_down_ms);
}

void test_left_motor()  {
  printf("testing left motor\n");
  color_right_amber();
  color_left_green();
  test_motor_l(LEFT);
}

void test_right_motor() {
  printf("testing right motor\n");
  color_left_amber();
  color_right_green();
  test_motor_l(RIGHT);
}

static void test_motor_l(motor_side m) {
  if (ev3_search_tacho(LEGO_EV3_L_MOTOR, motor, m)) {
    printf("L motor %d found: %d\n", m, motor[m]);
    tacho_stop(m);
    tacho_set_speed_perc(m, TEST_SPEED_PERC);
    tacho_set_time_with_ramp(m, TEST_TIME);
    tacho_run_timed(m);
  }
  else { fprintf(stderr, "L motor %d not found\n", m); }
}

void test_motor_m() {
  if (ev3_search_tacho(LEGO_EV3_M_MOTOR, motor, 0)) {
    printf("M motor found\n");
    tacho_stop(M);
    tacho_set_speed_perc(M, TEST_SPEED_PERC);
    tacho_set_time_with_ramp(M, TEST_TIME);
    tacho_run_timed(M);
  }
  else { fprintf(stderr, "M motor not found\n"); }
}