#include <QApplication>
#include <QImage>
#include <QLabel>
#include <nav_msgs/GetMap.h>
#include <ros/ros.h>

int main(int argc, char *argv[]) {
  QApplication a(argc, argv);
  ros::init(argc, argv, "Holla");

  QImage *myImage;

  auto nh = new ros::NodeHandle();

  auto map_client = nh->serviceClient<nav_msgs::GetMap>("/static_map");

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

  uint8_t *map_tex = (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[3 * i + 0] = 0;
      map_tex[3 * i + 1] = 0;
      map_tex[3 * i + 2] = 0;
    } else {
      map_tex[3 * i + 0] = 255;
      map_tex[3 * i + 1] = 255;
      map_tex[3 * i + 2] = 255;
    }
  }
  QImage *tmp_ = (new QImage(map_tex, map.info.width, map.info.height,
                             3 * map.info.width, QImage::Format_RGB888));
  QLabel myLabel;
  myLabel.setPixmap(QPixmap::fromImage(tmp_->mirrored()));

  myLabel.show();

  return a.exec();
}
