
#include "qrlocate/qrlocate.hpp"

#include <crosbot/data.hpp>
#include <crosbot/utils.hpp>
#include <crosbot_map/tag.hpp>
#include <crosbot_map/SnapMsg.h>
#include <crosbot_map/ListSnaps.h>

#define DEFAULT_FREQUENCY           0.2
#define DEFAULT_MAXWAIT_TRANSFORM   2.0
#define LOG_START                   "QRLocate ::"

#define CELL(x, y, wd) ((y) * (wd) + (x))

PLUGINLIB_DECLARE_CLASS(qrlocate, qrlocate, casualty::QRLocator, nodelet::Nodelet)

using std::string;
using std::vector;
using namespace crosbot;

namespace casualty {

using namespace zxing;
using namespace zxing::qrcode;

BufferBitmapSource::BufferBitmapSource(int inWidth, int inHeight, const unsigned char * inBuffer) :
	  LuminanceSource(inWidth, inHeight),
	  width(inWidth), height(inHeight)
{
  buffer = (char *)malloc(width * height * sizeof(char));
  for (unsigned int y = 0; y < height; ++y) {
      char *row = &buffer[y*width];
      const unsigned char* inRow = &inBuffer[y*width*3];
      int i;
      for (unsigned int x = 0; x < width; ++x) {
          i  = inRow[0];
          i += inRow[1];
          i += inRow[2];

          row[x] = i/3;
          inRow += 3;
      }
  }
}

ArrayRef<char> BufferBitmapSource::getRow(int y, ArrayRef<char> row) const {
  if (y < 0 || y >= height)
    {
        ERROR("ERROR, attempted to read row %d of a %d height image.\n", y, height);
        return NULL;
    }
    // WARNING: NO ERROR CHECKING! You will want to add some in your code.
  row = ArrayRef<char>(buffer + y*width, width);
  return row;
}

ArrayRef<char> BufferBitmapSource::getMatrix() const { ArrayRef<char>rval(buffer, width*height); return rval; }

QRLocator::QRLocator() : hints(DecodeHints::DEFAULT_HINT) {
    // Tell the decoder to try as hard as possible.
    hints.setTryHarder(true);
}

void QRLocator::onInit() {
    // Load configuration
    ros::NodeHandle nhConfig("/qrlocate");
    ConfigElementPtr config = new ROSConfigElement(nhConfig);
    configure(config);

    // Reset
    gotColourImage = false;
    gotDistImage = false;
    nextId = 0;

    ros::NodeHandle& nh = getNodeHandle();
    timerCallback = nh.createTimer(ros::Duration(frequency), &QRLocator::callbackTimer, this);
    imageColourSub = nh.subscribe("image_colour", 1, &QRLocator::callbackColourImage, this);
    imageDistSub = nh.subscribe("image_dist", 1, &QRLocator::callbackDistImage, this);
    snapPub = nh.advertise<crosbot_map::SnapMsg>("/snaps/publisher", 1);
    snapListClient = nh.serviceClient<crosbot_map::ListSnaps>("/snaps/list", 1);
}

void QRLocator::configure(ConfigElementPtr config) {
    frameHead       = config->getParam("frameHead", "/head");
    frameIcp        = config->getParam("frameIcp", "/icp");
    frameRobot      = config->getParam("frameRobot", "/base_footprint");
    frameSlam       = config->getParam("frameSlam", "/slam_frame");
    frequency       = config->getParamAsDouble("frequency", DEFAULT_FREQUENCY);
    maxDistValue    = config->getParamAsDouble("maxDistValue", 1.2);
    matchDistance   = config->getParamAsDouble("matchDistance", 2.0);
}

void QRLocator::callbackTimer(const ros::TimerEvent& tEvent) {
    //LOG("%s callbackTimer \n", LOG_START);
    processLatestImage();
}

void QRLocator::callbackColourImage(const sensor_msgs::ImagePtr& latestImage) {
    //LOG("%s colour image received.\n", LOG_START);
    nextColourImage = latestImage;
    gotColourImage = true;
}

void QRLocator::callbackDistImage(const sensor_msgs::ImagePtr& latestImage) {
    //LOG("%s distance image received.\n", LOG_START);
    nextDistImage = latestImage;
    gotDistImage = true;
}

void QRLocator::processLatestImage() {
    // Get local reference
    if (!gotColourImage) return;
    if (!gotDistImage) return;
    //LOG("%s process.\n", LOG_START);
    sensor_msgs::ImagePtr latestColourImage = nextColourImage;
    sensor_msgs::ImagePtr latestDistImage = nextDistImage;

    try {
        // Convert the buffer to something that the library understands.
        Ref<LuminanceSource> source (new BufferBitmapSource(latestColourImage->width, latestColourImage->height, &latestColourImage->data[0]));

        // Turn it into a binary image.
        Ref<Binarizer> binarizer (new GlobalHistogramBinarizer(source));
        Ref<BinaryBitmap> image(new BinaryBitmap(binarizer));

        // Perform the decoding.
        Ref<Result> result(reader.decode(image, hints));
        ArrayRef< Ref< ResultPoint > >& points = result->getResultPoints();

        // Output the result.
        //LOG("%s result(%d): %s\n", LOG_START, points->size(), result->getText()->getText().c_str());

        // Estimate location of the code by its size & orientation/ using depth image
        crosbot::ImagePtr distImg = new crosbot::Image(*latestDistImage, crosbot::Image::Mono16);
        unsigned short* imgData = (unsigned short *) distImg->data;
        double totalDist = 0;
        int count = 0;
        for (int y = 0; y != distImg->height; ++y) {
            for (int x = 0; x != distImg->width; ++x) {
                double value = imgData[CELL(x, y, distImg->width)] / 1000;
                if (value < maxDistValue) {
                    totalDist += value;
                    count++;
                }
            }
        }
        double averageDistance = (count > 0) ? totalDist / count : 0.4;
        //LOG("%s \t distance: %.3lf\n", LOG_START, averageDistance);

        // Get robot position
        Pose robotPose;
        Pose robotPoseIcp = transformPose(frameRobot, frameIcp, robotPose);
        Pose robotPoseSlam = transformPose(frameRobot, frameSlam, robotPose);

        // Get Pos of the code
        Pose codePose;
        codePose.position.z = averageDistance;
        Pose codePoseRobot = transformPose(frameHead, frameRobot, codePose);
        Pose codePoseSlam = transformPose(frameHead, frameSlam, codePose);

        // See if we've found this before
        string code = result->getText()->getText();
        string description = "QRCode: " + code;
        bool found = false;
        if (description == lastCode) {
            found = true;
        } else {
            crosbot_map::ListSnapsRequest listRequest;
            crosbot_map::ListSnapsResponse listResponse;
            snapListClient.call(listRequest, listResponse);
            for(int i = 0; !found && i != listResponse.snaps.size(); ++i) {
                if (listResponse.snaps[i].type == Snap::LANDMARK &&
                    listResponse.snaps[i].description == description) {
                    // Transpose point
                    Pose snapPoseSlam = robotPoseSlam.toTF() * Pose(listResponse.snaps[i].pose).toTF();
                    if (snapPoseSlam.position.distanceTo(codePoseSlam.position) < matchDistance) {
                        found = true;
                    }
                }
            }
        }

        if (!found) {
            // Create Snap
            nextId++;
            LOG("%s create snap() id - %d\n", LOG_START, nextId);
            SnapPtr snap = new Snap();
            snap->id = nextId;
            snap->type = Snap::LANDMARK;
            snap->status = Snap::CONFIRMED;
            snap->description = description;
            snap->robot = robotPoseIcp;
            snap->pose.position = codePoseRobot.position;
            snap->images.push_back(new Image(*latestColourImage, crosbot::Image::RGBA16));

            // Publish
            lastCode = description;
            snapPub.publish(snap->toROS());
        }

    } catch (zxing::Exception& e) {
        //LOG("%s exception: %s\n", LOG_START, e.what());
    }
}

Pose QRLocator::transformPose(string fromFrame, string toFrame, Pose pose) {
    //LOG("%s transformPose\n", LOG_START);
    std::string error;
    ros::Time timestamp = ros::Time::now();
    bool transformLoaded = tfListener.waitForTransform(toFrame, fromFrame, timestamp,
            ros::Duration(DEFAULT_MAXWAIT_TRANSFORM), ros::Duration(0.01), &error);
    if (!transformLoaded) {
        LOG("%s \t\t Could not load transform from %s to %s\n", LOG_START, fromFrame.c_str(), toFrame.c_str());
        LOG("%s \t\t Error: %s\n", LOG_START, error.c_str());
        return pose;
    }
    //LOG("%s \t got transform\n", LOG_START);

    // Do transform
    geometry_msgs::PoseStamped fromPose;
    fromPose.header.frame_id = fromFrame;
    fromPose.header.stamp = timestamp;
    fromPose.pose = pose.toROS();
    geometry_msgs::PoseStamped toPose;
    tfListener.transformPose(toFrame, fromPose, toPose);
    return Pose(toPose.pose);
}

} // namespace casualty

