#include <opencv2/opencv.hpp>

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <cmath>
#include <ctime>
#include <boost/date_time/posix_time/posix_time_types.hpp>


using namespace cv;
using namespace std;

Point PointOnCurve(Point p0, Point p1, Point p2, Point p3, float t)
{
   Point ret;

   float t2 = t * t;
   float t3 = t2 * t;

   ret.x = 0.5f * ((2.0f * p1.x) +
   (-p0.x + p2.x) * t +
   (2.0f * p0.x - 5.0f * p1.x + 4 * p2.x - p3.x) * t2 +
   (-p0.x + 3.0f * p1.x - 3.0f * p2.x + p3.x) * t3);

   ret.y = 0.5f * ((2.0f * p1.y) +
   (-p0.y + p2.y) * t +
   (2.0f * p0.y - 5.0f * p1.y + 4 * p2.y - p3.y) * t2 +
   (-p0.y + 3.0f * p1.y - 3.0f * p2.y + p3.y) * t3);

   return ret;
}

int main(int argc, char* argv[])
{
  double real_radius = 0.06;
  VideoCapture cap(0);
  int x = 0;
  Point last_graph_point;
  boost::posix_time::ptime t1 = boost::posix_time::microsec_clock::local_time();

  vector<Point> points;
  namedWindow("interpolate",CV_WINDOW_AUTOSIZE);
  Mat graph = Mat::zeros(500,500, CV_8U);;

  while (1)
  {
    Mat frame;
    Mat result;
    Mat channels[3];

    bool bSuccess = cap.read(frame);
    flip(frame,frame, 1);

    Mat gray;
    cvtColor(frame, gray, COLOR_BGR2HSV);
    vector<Vec3f> circles;

    inRange(gray, Scalar(90,100,50),Scalar(130,255,255),gray);
    GaussianBlur( gray, gray, Size(11, 11), 500);

    HoughCircles( gray, circles, CV_HOUGH_GRADIENT, 1, 1000, 100, 20, 10);
    double radius;
    double time_passed;
    if(circles.size())
    {
      radius = circles[0][2];
      points.push_back(Point(circles[0][0], circles[0][1]));
      circle(frame, Point(circles[0][0], circles[0][1]), radius, Scalar(0,0,255),5);
      boost::posix_time::ptime t2 = boost::posix_time::microsec_clock::local_time();
      boost::posix_time::time_duration diff = t2 - t1;
      t1 = boost::posix_time::microsec_clock::local_time();
      time_passed = diff.total_milliseconds() * 0.001;
    }

    if (points.size() > 3)
    {
      vector<Point> points_interpolated;
      for (int i = 0;i < points.size() - 3; i++)
      {
        for (int j = 0; j < 20; j++)
        {
          points_interpolated.push_back(PointOnCurve(points[i], points[i + 1], points[i + 2], points[i + 3], (1.0 / 50) * j));
        }
      }

      for (int i = 0;i < points_interpolated.size() - 1; i++)
      {
        line(frame, points_interpolated[i], points_interpolated[i + 1], Scalar(255,0,0),2);
      }

      double distance = 0;
      double xsq = pow(points[points.size() - 2].x - points[points.size() - 1].x, 2);
      double ysq = pow(points[points.size() - 2].y - points[points.size() - 1].y, 2);
      distance = sqrt(xsq + ysq);


      if (circles.size())
      {
        double ratio = real_radius / radius;
        double vel =  distance * ratio / time_passed;
        String s =format("%.2f m/s",vel);
        line(graph, last_graph_point,Point(x, vel * 70), Scalar(255,0,0),1);
        last_graph_point = Point(x,vel * 70);
        putText(frame, s, points[points.size() - 1], FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255,255), 2);
      }
    }

    x+=5;
    if (x > 500)
    {
      graph = Mat::zeros(500,500, CV_8U);;
      x = 0;
      last_graph_point = Point(0,0);
    }

    imshow("graph",graph);
    imshow("interpolate",frame);

    waitKey(1);
  }
  return 0;

}