#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sensor_msgs/image_encodings.h>
#include <iostream>
#include <cmath>
#include <tuple>
#include <std_srvs/Empty.h>


using namespace std;
using namespace cv;


int low_r=85, low_g=0, low_b=0;
int high_r=255, high_g=50, high_b=50;

static const std::string IME_PROZORA = "Robo0 kamera";

float u0=160.5; //zadano u zadatku ne moramo gledati u matricu
float f = 265.23;

image_transport::Publisher objav;

void slikaPovPoziv(const sensor_msgs::ImageConstPtr& msg) {
	cv::Mat frame_threshold, ballPixels;

    cv_bridge::CvImagePtr cv_ptr;

    try {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }


	cv::inRange(cv_ptr->image,Scalar(low_b, low_g, low_r), Scalar(high_b,high_g,high_r), frame_threshold);

	cv::findNonZero(frame_threshold, ballPixels);


	auto mid = ballPixels.begin() + ballPixels.size()/2; //meni tu baca errore


    // ažuriraj sliku
    cv::imshow(IME_PROZORA, cv_ptr->image);
	imshow("Object Detection",frame_threshold);

    cv::waitKey(3);

    // objavi izmjenjenu sliku
    objav.publish(cv_ptr->toImageMsg());
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "image_converter");
    ros::NodeHandle node;

    cv::namedWindow(IME_PROZORA);

    image_transport::ImageTransport im_trans(node);

    image_transport::Subscriber pret = im_trans.subscribe("/robo0/camera/image", 1,
      &slikaPovPoziv); //subscriber za detekciju objekta na temelju crvene boje


    objav = im_trans.advertise("/processed/image", 1);

    ros::spin();
    return 0;
}