#include "speller/speller.h"
Speller::Speller(int ar, char **av) {

  argc = ar;
  argv = av;
  spelling = 0;

  window = new MapView();

  connect(window, SIGNAL(keyPress(QKeyEvent *)), this,
          SLOT(keyEvent(QKeyEvent *)));
  connect(this, SIGNAL(load_texture(int, Vertex *, QImage *)), window,
          SLOT(load_texture(int, Vertex *, QImage *)));
  connect(window, SIGNAL(map_loaded()), this, SLOT(map_loaded()));
  connect(window, SIGNAL(mouse_press(QMouseEvent *)), this,
          SLOT(mouse_press(QMouseEvent *)));
}

Speller::~Speller() {

  insight_stop_notif();
  insight_destroy();
  ros::shutdown();
  delete window;
  this->wait();
}

void Speller::run() {

  // init_ble();
  init_ros();
  load_map();
  ros::Rate loop(10);
  ros::Rate loop2(10);
  while (ros::ok()) {
    loop2.sleep();
    while (spelling && ros::ok()) {
      printf("Shape %d\n", map_shape);
      get_ble_data();
      // TODO-Send data for classification
      // check classifier result();
      classify(buf_len, buf);
      printf("classifier %d\n", classifier_result);
      reconfigure_map(classifier_result, map_shape);
      load_map();
      loop.sleep();
    }
  }
}

void Speller::classify(int len, struct insight_data *buf) {
  sleep(2);
  if ((me.x() * me.y()) < 0) {
    if (me.x() < 0)
      classifier_result = TOP_LEFT;
    else
      classifier_result = BOTTOM_RIGHT;
  } else {
    if (me.x() < 0)
      classifier_result = BOTTOM_LEFT;
    else
      classifier_result = TOP_RIGHT;
  }
}

void Speller::reconfigure_map(int result, int shape) {
  int w = area_displayed->width();
  int h = area_displayed->height();
  int x = area_displayed->x();
  int y = area_displayed->y();

  switch (shape) {
  case SQUARE:
    switch (result) {
    case TOP_LEFT:
      area_displayed->setWidth(w / 1.8);
      area_displayed->setY(y + h / 1.8);
      break;
    case TOP_RIGHT:
      area_displayed->setX(x + w / 1.8);
      area_displayed->setY(y + h / 1.8);
      break;
    case BOTTOM_LEFT:
      area_displayed->setHeight(h / 1.8);
      area_displayed->setWidth(w / 1.8);
      break;
    case BOTTOM_RIGHT:
      area_displayed->setHeight(h / 1.8);
      area_displayed->setX(x + w / 1.8);
      break;
    }
    break;
  case HORIZONTAL:
    switch (result) {
    case TOP_LEFT:
    case BOTTOM_LEFT:
      area_displayed->setWidth(w / 1.8);
      break;
    case TOP_RIGHT:
    case BOTTOM_RIGHT:
      area_displayed->setX(x + w / 1.8);
      break;
    }
    break;
  case VERTICAL:
    switch (result) {
    case TOP_LEFT:
    case TOP_RIGHT:
      area_displayed->setY(h + h / 1.8);
      break;
    case BOTTOM_LEFT:
    case BOTTOM_RIGHT:
      area_displayed->setHeight(h / 1.8);
      break;
    }
    break;
  }
}

__inline__ void Speller::load_map() {
  float max = std::max(area_displayed->width(), area_displayed->height());

  float w = area_displayed->width() / max;
  float h = area_displayed->height() / max;
  float aspect_ratio = w / h;

  if (aspect_ratio > 1.5)
    map_shape = HORIZONTAL;
  else if (aspect_ratio < 0.5)
    map_shape = VERTICAL;
  else
    map_shape = SQUARE;

  Vertex tmp_info[] = {
      Vertex(QVector3D(-w, -h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
             QVector2D(0.0f, 0.0f)),
      Vertex(QVector3D(-w, h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
             QVector2D(0.0f, 1.0f)),
      Vertex(QVector3D(w, h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
             QVector2D(1.0, 1.0f)),
      Vertex(QVector3D(w, -h, -1.0f), QVector4D(1.0f, 0.0f, 0.0f, 0.75f),
             QVector2D(1.0f, 0.0f))};

  QImage tmp = curr_map->copy(*area_displayed);

  emit(load_texture(sizeof(tmp_info), tmp_info, &tmp));

  map_loading_done = 0;

  while (!map_loading_done)
    sleep(0);
}

__inline__ void Speller::get_ble_data() {

  window->start_blinking();
  // int ret = insight_write_to_buffer(buf_len, buf);

  // while ((!ret) && (insight_buffer_status() == 1))
  //    sleep(0);
  sleep(2);
  window->stop_blinking();
}

__inline__ void Speller::init_ros() {

  ros::init(argc, argv, "speller");

  nh = new ros::NodeHandle();
  pub_goal =
      nh->advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
  map_client = nh->serviceClient<nav_msgs::GetMap>("/static_map");

  //  sub_pose = nh->subscribe("qt_pi/pose", 1, &Speller::poseCB, this);

  nav_msgs::GetMap tmp;
  while (!map_client.call(tmp))
    sleep(0);
  map = tmp.response.map;

  map_tex_buffer = (uint8_t *)malloc(map.info.height * map.info.width * 3);

  for (int i = 0; i < map.info.height * map.info.width; i++) {
    if (map.data[i] > 50) {
      map_tex_buffer[3 * i + 0] = 0;
      map_tex_buffer[3 * i + 1] = 0;
      map_tex_buffer[3 * i + 2] = 0;
    } else {
      map_tex_buffer[3 * i + 0] = 255;
      map_tex_buffer[3 * i + 1] = 255;
      map_tex_buffer[3 * i + 2] = 255;
    }
  }
  curr_map = new QImage(map_tex_buffer, map.info.width, map.info.height,
                        3 * map.info.width, QImage::Format_RGB888);
  area_displayed = new QRect(QPoint(0, 0), curr_map->size());
}

void Speller::poseCB(nav_msgs::Odometry::ConstPtr &msg) {
  pose_current = msg->pose.pose;
}

__inline__ void Speller::init_ble() {

  const char *a = "EF:BD:39:4E:08:49";
  const char *b = "81072f41-9f3d-11e3-a9dc-0002a5d5c51b";

  if (!insight_init(a, b))
    buf = (struct insight_data *)malloc(sizeof(struct insight_data) * buf_len);
}

void Speller::map_loaded() { map_loading_done = 1; }

void Speller::keyEvent(QKeyEvent *e) {

  if (e->key() == Qt::Key_Space) {

    if (!spelling)
      insight_start_notif();
    else
      insight_stop_notif();
    spelling = !spelling;
  }
}

void Speller::mouse_press(QMouseEvent *e) {
  me = e->pos();
  me.setX(me.x() - 500);
  me.setY(500 - me.y());
  printf("%d %d\n", me.x(), me.y());
}
