
#ifndef QRLOCATE_H_
#define QRLOCATE_H_

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

#include <zxing/qrcode/QRCodeReader.h>
#include <zxing/common/GlobalHistogramBinarizer.h>

#include <crosbot/config.hpp>
#include <vector>

using namespace crosbot;

namespace casualty {

using namespace zxing;
using namespace zxing::qrcode;

class BufferBitmapSource : public LuminanceSource {
private:
  int width, height;
  char* buffer;

public:
  BufferBitmapSource(int inWidth, int inHeight, const unsigned char * inBuffer);
  ~BufferBitmapSource() {}

  int getWidth() const { return width; }
  int getHeight() const { return height; }
  ArrayRef<char> getRow(int y, ArrayRef<char> row) const;
  ArrayRef<char> getMatrix() const;
};


class QRLocator : public nodelet::Nodelet {
public:
	QRLocator();
	~QRLocator() {};

	void configure(ConfigElementPtr config);

	void callbackTimer(const ros::TimerEvent& tEvent);
    void callbackColourImage(const sensor_msgs::ImagePtr& latestImage);
    void callbackDistImage(const sensor_msgs::ImagePtr& latestImage);

	virtual void onInit();

private:
    ros::Timer timerCallback;
    ros::Subscriber imageColourSub;
    ros::Subscriber imageDistSub;
    ros::Publisher snapPub;
    ros::ServiceClient snapListClient;
    tf::TransformListener tfListener;

    DecodeHints hints;
    QRCodeReader reader;

    bool gotColourImage;
    bool gotDistImage;
    sensor_msgs::ImagePtr nextColourImage;          // Next image to process
    sensor_msgs::ImagePtr nextDistImage;            // Next image to process
    std::string lastCode;                           // Last QR Code that we found
    int nextId;                                     // Id of next landmark

    // Config
    std::string frameHead;
    std::string frameIcp;
    std::string frameRobot;
    std::string frameSlam;
    double frequency;
    double maxDistValue;
    double matchDistance;

	void processLatestImage();
	Pose transformPose(std::string fromFrame, std::string toFrame, Pose pose);
};

} // namespace casualty


#endif /* QRLOCATE_H_ */


