#include <stdio.h>
#include <unistd.h>
#include "hw_impl_unix.h"
#include "quad_app.h"
#include <fcntl.h>

struct VirtQuadIO *virt_quad_io;
int virt_quad_io_file_descriptor;

enum CLISubCommand {
  START,
  GET,
  SET,
  USAGE,
  HELP,
};

int start_virt_quad(char *argv[]);
int handle_io_output(const char *name);
int handle_io_input(const char *name, const char *value_str);
void set_shm(float value, float *dest, pthread_mutex_t *lock);
void print_shm_float(float *value, pthread_mutex_t *lock);
void print_shm_int(int *value, pthread_mutex_t *lock);
void usage(char *executable_name);
enum CLISubCommand parse_sub_command(char *argv[]);
void open_new_shm_io();
void open_existing_shm_io();
void help_sub_command(char *argv[]);

/**
 * Implement each of the hardware interfaces.
 */
int setup_hardware(hardware_t *hardware) {
  hardware->i2c_0 = create_unix_i2c();
  hardware->i2c_1 = create_unix_i2c();
  hardware->rc_receiver = create_unix_rc_receiver();
  hardware->motors = create_unix_motors();
  hardware->uart_0 = create_unix_uart();
  hardware->comm = create_unix_comm(&hardware->uart_0);
  hardware->global_timer = create_unix_global_timer();
  hardware->axi_timer = create_unix_axi_timer();
  hardware->mio7_led = create_unix_mio7_led();
  hardware->sys = create_unix_system();
  hardware->imu = create_unix_imu(&hardware->i2c_0);
  hardware->lidar = create_unix_lidar(&hardware->i2c_1);
  hardware->of = create_unix_optical_flow(&hardware->i2c_0);
  return 0;
}

int main(int argc, char *argv[]) {
  enum CLISubCommand sub_command = parse_sub_command(argv + 1);

  switch (sub_command) {
  case START:
    open_new_shm_io();
    start_virt_quad(argv + 2); // will block
    break;
  case SET:
    open_existing_shm_io();
    handle_io_input(argv[2], argv[3]);
    break;
  case GET:
    open_existing_shm_io();
    handle_io_output(argv[2]);
    break;
  case HELP:
    help_sub_command(argv);
    break;
  case USAGE:
    usage(argv[0]);
    break;
  }

  return 0;
}

enum CLISubCommand parse_sub_command(char *argv[]) {
  if (argv[0] == NULL) {
    return USAGE;
  }
  else if (strcmp(argv[0], "start") == 0) {
    return START;
  }
  else if (strcmp(argv[0], "help") == 0) {
    return HELP;
  }
  else if (strcmp(argv[0], "get") == 0 && argv[1] != NULL) {
    return GET;
  }
  else if (strcmp(argv[0], "set") == 0 && argv[1] != NULL && argv[2] != NULL) {
    return SET;
  }
  else {
    return USAGE;
  }
}

int start_virt_quad(char *argv[]) {
  int fd;

  // Allow making the quad quiet
  if (argv[0] != NULL && strcmp(argv[0], "-q") == 0) {
    fd = open("/dev/null", O_WRONLY);
    close(STDOUT_FILENO);
    dup2(STDOUT_FILENO, fd);
  }

  puts("Starting the virtual quad.");
  puts("Open another tab to query and control the virt quad, using the get and set commands.");
  quad_main(setup_hardware);
  return 0;
}

void open_new_shm_io() {
  // Prepare the shared memory for io. Clear memory and initialize.
  int *fd = &virt_quad_io_file_descriptor;
  *fd = shm_open(VIRT_QUAD_SHARED_MEMORY, O_CREAT | O_RDWR | O_TRUNC, 0666);
  ftruncate(*fd, sizeof(struct VirtQuadIO));
  virt_quad_io = mmap(NULL, sizeof(struct VirtQuadIO), PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
  pthread_mutexattr_t attr;
  pthread_mutexattr_init(&attr);
  pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_SHARED);
  pthread_mutex_init(&virt_quad_io->led_lock, &attr);
  pthread_mutex_init(&virt_quad_io->motors_lock, &attr);
  pthread_mutex_init(&virt_quad_io->rc_lock, &attr);
  pthread_mutex_init(&virt_quad_io->i2c_lock, &attr);
}

void open_existing_shm_io() {
  // Open exising shared memory for io. DO NOT CLEAR.
  int *fd = &virt_quad_io_file_descriptor;
  *fd = shm_open(VIRT_QUAD_SHARED_MEMORY, O_CREAT | O_RDWR, 0666);
  virt_quad_io = mmap(NULL, sizeof(struct VirtQuadIO), PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
}


/**
 * The user wants to read an output by name. Get the output
 * and print to stdout.
 */
int handle_io_output(const char *name) {
  int ret = 0;
  struct VirtQuadIO *io = virt_quad_io;
  if (strcmp(name, "led") == 0) {
    print_shm_int(&io->led, &io->led_lock);
  }
  else if (strcmp(name, "motor1") == 0) {
    print_shm_float(&io->motors[0], &io->motors_lock);
  }
  else if (strcmp(name, "motor2") == 0) {
    print_shm_float(&io->motors[1], &io->motors_lock);
  }
  else if (strcmp(name, "motor3") == 0) {
    print_shm_float(&io->motors[2], &io->motors_lock);
  }
  else if (strcmp(name, "motor4") == 0) {
    print_shm_float(&io->motors[3], &io->motors_lock);
  }
  else {
    puts("Given output not recognized.");
    ret = -1;
  }
  return ret;
}

/**
 * The user wants to set an input by name, and has supplied
 * a value. Set the appropriate input to the given value.
 */
int handle_io_input(const char *name, const char *value_str) {
  int ret = 0;
  errno = 0;
  float value = strtof(value_str, NULL);
  if (errno) {
    ret = -1;
    puts("Given number format was bad.");
  }
  else {
    struct VirtQuadIO *io = virt_quad_io;
    if (strcmp(name, "rc_throttle") == 0) {
      set_shm(value, &io->rc_receiver[THROTTLE], &io->rc_lock);
    }
    else if (strcmp(name, "rc_roll") == 0) {
      set_shm(value, &io->rc_receiver[ROLL], &io->rc_lock);
    }
    else if (strcmp(name, "rc_pitch") == 0) {
      set_shm(value, &io->rc_receiver[PITCH], &io->rc_lock);
    }
    else if (strcmp(name, "rc_yaw") == 0) {
      set_shm(value, &io->rc_receiver[YAW], &io->rc_lock);
    }
    else if (strcmp(name, "rc_gear") == 0) {
      set_shm(value, &io->rc_receiver[GEAR], &io->rc_lock);
    }
    else if (strcmp(name, "rc_flap") == 0) {
      set_shm(value, &io->rc_receiver[FLAP], &io->rc_lock);
    }
    else if (strcmp(name, "i2c_imu_x") == 0) {
      set_shm(value, &io->gam.accel_x, &io->i2c_lock);
    }
    else if (strcmp(name, "i2c_imu_y") == 0) {
      set_shm(value, &io->gam.accel_y, &io->i2c_lock);
    }
    else if (strcmp(name, "i2c_imu_z") == 0) {
      set_shm(value, &io->gam.accel_z, &io->i2c_lock);
    }
    else if (strcmp(name, "i2c_imu_p") == 0) {
      set_shm(value, &io->gam.gyro_xVel_p, &io->i2c_lock);
    }
    else if (strcmp(name, "i2c_imu_q") == 0) {
      set_shm(value, &io->gam.gyro_yVel_q, &io->i2c_lock);
    }
    else if (strcmp(name, "i2c_imu_r") == 0) {
      set_shm(value, &io->gam.gyro_zVel_r, &io->i2c_lock);
    }
    else if (strcmp(name, "i2c_imu_mag_x") == 0) {
      set_shm(value, &io->gam.mag_x, &io->i2c_lock);
    }
    else if (strcmp(name, "i2c_imu_mag_y") == 0) {
      set_shm(value, &io->gam.mag_y, &io->i2c_lock);
    }
    else if (strcmp(name, "i2c_imu_mag_z") == 0) {
      set_shm(value, &io->gam.mag_z, &io->i2c_lock);
    }
    else {
      puts("Given input not recognized.");
      ret = -1;
    }
  }
  return ret;
}

void set_shm(float value, float *dest, pthread_mutex_t *lock) {
  pthread_mutex_lock(lock);
  *dest = value;
  pthread_mutex_unlock(lock);
}

void print_shm_float(float *value, pthread_mutex_t *lock) {
  pthread_mutex_lock(lock);
  printf("%f\n", *value);
  pthread_mutex_unlock(lock);
}

void print_shm_int(int *value, pthread_mutex_t *lock) {
  pthread_mutex_lock(lock);
  printf("%d\n", *value);
  pthread_mutex_unlock(lock);
}

void usage(char *executable_name) {
  printf("Usage: %s command [ args ]\n", executable_name);
  puts("Overview:");
  puts("  The virtual quad emulates the behavior of the real quad in the Unix");
  puts("  environment. Start the virtual quad in one tab, and then control the I/O");
  puts("  of the virtual quad through commands.");
  puts("");
  puts("  For help on a particular command, pass the command name to the help command");
  puts("  Example:");
  printf("    %s help get\n", executable_name);
  puts("");
  puts("Commands:");
  puts("  help");
  puts("  get");
  puts("  set");
  puts("  start");
  puts("");
}

void help_sub_command(char *argv[]) {
  if (argv[2] == NULL) {
    usage(argv[0]);
  }
  else if (strcmp(argv[2], "help") == 0) {
    puts("Really?");
  }
  else if (strcmp(argv[2], "get") == 0) {
    printf("Usage: %s get output_name\n", argv[0]);
    puts("");
    puts("  Get an output on the virtual quad, specified by its output_name, and print");
    puts("  to stdout");
    puts("");
    puts("  Possible output names");
    puts("    led");
    puts("    motor1");
    puts("    motor2");
    puts("    motor3");
    puts("    motor4");
    puts("");
  }
  else if (strcmp(argv[2], "set") == 0) {
    printf("Usage: %s set input_name val\n", argv[0]);
    puts("");
    puts("  Set the value of an input on the quad, specified by its input_name");
    puts("");
    puts("  Possible input names");
    puts("    rc_throttle");
    puts("    rc_pitch");
    puts("    rc_roll");
    puts("    rc_yaw");
    puts("    rc_gear");
    puts("    rc_flap");
    puts("    i2c_imu_x");
    puts("    i2c_imu_y");
    puts("    i2c_imu_z");
    puts("    i2c_imu_p");
    puts("    i2c_imu_q");
    puts("    i2c_imu_r");
    puts("    i2c_imu_mag_x");
    puts("    i2c_imu_mag_y");
    puts("    i2c_imu_mag_z");
    puts("");
  }
  else if (strcmp(argv[2], "start") == 0) {
    printf("Usage: %s start [ -q ]\n", argv[0]);
    puts("");
    puts("  Start the virtual quad. Exit using ctrl-c. In order to get and set I/O,");
    puts("  you'll need to open another tab.");
    puts("");
    puts("  Flags");
    puts("    q - Keep the quad quiet; don't print logs to the stdout");
    puts("");
  }
  else {
    usage(argv[0]);
  }
}