#pragma once
#include <iostream>
#include <sstream>
#include <opencv2/opencv.hpp>
using namespace std;

class Ball
{
#define EPS 1.0e-6
 private:
  cv::Vec3f pos;
  cv::Vec3f vel;
  cv::Vec3f accel;
  double r;
 public:
 Ball() : pos(0, 0, 0), vel(0, 0, 0), accel(0, 0, 0), r(1.0) {}
  Ball(cv::Vec3f &pos, cv::Vec3f &v, cv::Vec3f &accel, double r) {
    this->pos = pos;
    this->vel = v;
    this->accel = accel;
    this->r = r;
  }
  ~Ball() {}
  void setPos(cv::Vec3f &pos) { this->pos = pos; }
  cv::Vec3f getPos() { return pos; }
  void setV(cv::Vec3f &v) { this->vel = v; }
  cv::Vec3f getV() { return vel; }
  void setAccel(cv::Vec3f &a) { accel = a; }
  cv::Vec3f getAccel() { return accel; }
  void setR(double r) { this->r = r; }
  double getR() { return r; }
  void step() {
    vel += accel;
    pos += vel;
  }
  bool bounce(pair<cv::Vec3f, cv::Vec3f> &seg) {
    cv::Vec3f nearPt = nearest(seg, pos);
    double len = cv::norm(pos, nearPt);

    cv::Vec3f oldPos = pos - vel;
    pair<cv::Vec3f, cv::Vec3f> trail(oldPos, pos);
    pair<cv::Vec3f, cv::Vec3f> pr = nearest(seg, trail);
    double len2 = cv::norm(pr.first, pr.second);

    if (len2 < EPS) {
      cv::Vec3f pt = oldPos - pr.second;
      cv::Vec3f par = seg.second - seg.first;
      cv::Vec3f normal = par.cross(cv::Vec3f(0, 0, 1));
      normal /= cv::norm(normal);
      double cs = pt.dot(normal);
      if (cs < 0){
	normal *= -1.0; cs *= -1.0;
      }
      vel = 2 * cs * normal - pt;
      pos += vel;
      return true;
    }
    else if (len < r) {
      reflect(seg, nearPt);
      return true;
    }
    return false;
  }
  bool bounce(Ball &ball) {
    cv::Vec3f normal = pos - ball.pos;
    double len = cv::norm(normal);
    if (len >= r + ball.r) return false;
    if (len < 1.0) {
      pos[0] = -1;
      normal = pos - ball.pos;
      len = cv::norm(normal);
    }
    normal /= len;
    cv::Vec3f u = -vel;
    double cs = normal.dot(u);
    if (cs > 0) vel = 2 * cs * normal - u;
    pos += (r + ball.r - len) / 2 * normal;

    normal *= -1.0;
    u = -ball.vel;
    cs = normal.dot(u);
    if (cs > 0) ball.vel = 2 * cs * normal - u;
    ball.pos += (r + ball.r - len) / 2 * normal;

    return true;
  }
  pair<cv::Vec3f, cv::Vec3f> nearest(pair<cv::Vec3f, cv::Vec3f> &seg1, pair<cv::Vec3f, cv::Vec3f> &seg2) {
    pair<cv::Vec3f, cv::Vec3f> ans;
    cv::Vec3f ab = seg1.second - seg1.first, cd = seg2.second - seg2.first, ac = seg2.first - seg1.first;
    cv::Vec3f v = ab.cross(cd);
    if (v[2] > EPS) {
      cv::Vec3f u = ac.cross(cd), w = ac.cross(ab);
      double t = u[2] / v[2], s = w[2] / v[2];
      if (t >= 0 - EPS && t <= 1.0 + EPS && s >= 0 - EPS && s <= 1.0 + EPS) {
	if (abs(t) < EPS) { ans.first = ans.second = seg1.first; }
	else if (abs(t - 1) < EPS) { ans.first = ans.second = seg1.second; }
	else if (abs(s) < EPS) { ans.first = ans.second = seg2.first; }
	else if (abs(s - 1) < EPS) { ans.first = ans.second = seg2.second; }
	else {
	  ab[2] = 0.0;
	  ans.first = ans.second = ab*t + seg1.first;
	}
	return ans;
      }
    }
    ans.first = seg1.first;
    ans.second = nearest(seg2, seg1.first);
    double minlen = cv::norm(ans.second, seg1.first);
    v = nearest(seg2, seg1.second);
    double len = cv::norm(v, seg1.second);
    if (len < minlen) { ans.first = seg1.first; ans.second = v; minlen = len; }
    v = nearest(seg1, seg2.first);
    len = cv::norm(v, seg2.first);
    if (len < minlen) { ans.first = v; ans.second = seg2.first; minlen = len; }
    v = nearest(seg1, seg2.second);
    len = cv::norm(v, seg2.second);
    if (len < minlen) { ans.first = v; ans.second = seg2.second; minlen = len; }
    return ans;
  }

  double minDistance(pair<cv::Vec3f, cv::Vec3f> &seg, cv::Vec3f &pt) {
    cv::Vec3f nearPt = nearest(seg, pt);
    return cv::norm(nearPt, pt);
  }
  cv::Vec3f nearest(pair<cv::Vec3f, cv::Vec3f> &seg, cv::Vec3f &pt) {
    cv::Vec3f p(seg.second - seg.first), q(pt - seg.first);
    double t = p.dot(q) / p.dot(p);
    if (t <= 0.0 + EPS) return seg.first;
    if (t >= 1.0 - EPS) return seg.second;
    p = p*t + seg.first;
    return p;
  }
  void reflect(pair<cv::Vec3f, cv::Vec3f> &seg, cv::Vec3f &nearPt) {
    cv::Vec3f normal = pos - nearPt;
    double len = cv::norm(normal);
    if (len < 1.0) { // if ball's center is on the segment, add 2 to ball.y
      pos[1] -= 2;
      normal = pos - nearPt;
      len = cv::norm(normal);
    }
    normal /= len;
    if (len<r) pos += (r - len) * normal;
    if (normal.dot(vel) > 0) { // same direction
      vel += 5 * normal;
    }
    else {
      cv::Vec3f u = -vel;
      vel = 2 * normal.dot(u) * normal - u; // reflect
    }
  }
  bool in(cv::Mat &image) {
    return pos[0] >= 0 && pos[0] < image.cols && pos[1] >= -30 && pos[1] < image.rows;
  }
  void draw(cv::Mat &image, int id = -1) {
    cv::Point pt((int)pos[0], (int)pos[1]);
    cv::circle(image, pt, r, cv::Scalar(255, 0, 255), -1);
    stringstream ss; ss << id;
    if (id >= 0) cv::putText(image, ss.str(), pt, cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0, 0, 255));
  }
  string tostring() {
    stringstream ss;
    ss << "ball[" << pos << " " << vel << " " << accel << " " << r << "]";
    return ss.str();
  }
};

#pragma once
#include <time.h>
#include <opencv2/opencv.hpp>
#include "Ball.h"
using namespace std;

class BallFall
{
#define N_BALL	8

 private:
  cv::Mat image;
  vector<Ball> balls;
  cv::Vec3f mouse;
 public:
  BallFall() {
    srand((unsigned int)time(NULL));
  }
  ~BallFall() {}
  void start(int w = 640, int h = 480) {
    image = cv::Mat(h, w, CV_8UC3);
    for (int i = 0; i < N_BALL; i++) {
      Ball ball;
      randomBall(ball);
      balls.push_back(ball);
    }
    cv::namedWindow("ball fall");
    mouse[0] = -1;
  }
  void randomBall(Ball &b) {
    cv::Vec3f pos, v, accel;
    pos[0] = rand() % image.cols;
    pos[1] = 0; pos[2] = 0;
    v[0] = rand() % 4 - 2; v[1] = rand() % 1; v[2] = 0;
    accel[0] = 0; accel[1] = 1.0; accel[2] = 0;
    b.setPos(pos); b.setV(v); b.setAccel(accel); b.setR(30);
  }
  void step(vector<pair<cv::Vec3f, cv::Vec3f>>& segments) {
    for (int i = 0; i < balls.size(); i++) balls[i].step();
    for (int i = 0; i < balls.size(); i++) {
      for (int j = i + 1; j < balls.size(); j++)
	balls[i].bounce(balls[j]);
    }
    for (int i = 0; i < balls.size(); i++) {
      for (int j = 0; j < segments.size(); j++)
	balls[i].bounce(segments[j]);
      if (!balls[i].in(image)) randomBall(balls[i]);
    }
  }
  void draw(vector<pair<cv::Vec3f, cv::Vec3f>>& segments) {
    cv::rectangle(image, cv::Rect(0, 0, image.cols, image.rows), cv::Scalar(50, 50, 50), -1);
    draw(segments, image);
    cv::imshow("ball fall", image);
  }

  void draw(vector<pair<cv::Vec3f, cv::Vec3f>>& segments, cv::Mat& image) {
    for (int i = 0; i < balls.size(); i++) {
      balls[i].draw(image);
    }
    for (int i = 0; i < segments.size(); i++) {
      cv::Point p((int)segments[i].first[0], (int)segments[i].first[1]);
      cv::Point q((int)segments[i].second[0], (int)segments[i].second[1]);
      cv::line(image, p, q, cv::Scalar(255, 255, 0), 8);
    }
  }
};

#include <iostream>
#include <sstream>

#include "NtKinect.h"
#include "BallFall.h"

const int segment[] = {
  JointType_SpineBase, JointType_SpineMid,           // spine
  JointType_SpineMid, JointType_SpineShoulder,
  JointType_SpineShoulder, JointType_Neck,           // head
  JointType_Neck, JointType_Head,

  JointType_SpineShoulder, JointType_ShoulderLeft,   // left hand
  JointType_ShoulderLeft, JointType_ElbowLeft,
  JointType_ElbowLeft, JointType_WristLeft,
  JointType_WristLeft, JointType_HandLeft,
  JointType_HandLeft, JointType_HandTipLeft,
  JointType_HandLeft, JointType_ThumbLeft,

  JointType_SpineShoulder, JointType_ShoulderRight,  // right hand
  JointType_ShoulderRight, JointType_ElbowRight,
  JointType_ElbowRight, JointType_WristRight,
  JointType_WristRight, JointType_HandRight,
  JointType_HandRight, JointType_HandTipRight,
  JointType_HandRight, JointType_ThumbRight,
  
  JointType_SpineBase,  JointType_HipLeft,           // left leg
  JointType_HipLeft,  JointType_KneeLeft,
  JointType_KneeLeft,  JointType_AnkleLeft,
  JointType_AnkleLeft,  JointType_FootLeft,

  JointType_SpineBase,  JointType_HipRight,          // right leg
  JointType_HipRight,  JointType_KneeRight,
  JointType_KneeRight,  JointType_AnkleRight,
  JointType_AnkleRight,  JointType_FootRight,
};
const int segmentSize = sizeof(segment) / sizeof(int);

using namespace std;

void doJob() {
  NtKinect kinect;
  BallFall bf;
  bool flag = false;
  kinect.setRGB();
  bf.start(kinect.rgbImage.cols, kinect.rgbImage.rows);
  vector<pair<cv::Vec3f, cv::Vec3f>> skel;
  while (1) {
    kinect.setRGB();
    kinect.setSkeleton();
    skel.clear();
    for (int i = 0; i < kinect.skeleton.size(); i++) {
      for (int j = 0; j < segmentSize/2; j++) {
	Joint joint1 = kinect.skeleton[i][segment[j * 2]];
	Joint joint2 = kinect.skeleton[i][segment[j * 2 + 1]];
	if (joint1.TrackingState != TrackingState_NotTracked
	    && joint2.TrackingState != TrackingState_NotTracked) {
	  ColorSpacePoint cp;
	  kinect.coordinateMapper->MapCameraPointToColorSpace(joint1.Position, &cp);
	  cv::Vec3f p1 = cv::Vec3f(cp.X, cp.Y, 0.0f);
	  kinect.coordinateMapper->MapCameraPointToColorSpace(joint2.Position, &cp);
	  cv::Vec3f p2 = cv::Vec3f(cp.X, cp.Y, 0.0f);
	  skel.push_back(pair<cv::Vec3f, cv::Vec3f>(p1,p2));
	}
      }
    }
    bf.step(skel);
    cv::Mat image;
    if (flag) image = kinect.rgbImage.clone();
    else {
      image = cv::Mat(kinect.rgbImage.rows,kinect.rgbImage.cols,CV_8UC3);
      cv::rectangle(image,cv::Rect(0,0,image.cols,image.rows),cv::Scalar(0,0,0), -1);
    }
    bf.draw(skel, image);
    cv::imshow("ball fall", image);
    auto key = cv::waitKey(1);
    if (key == 'q') break;
    if (key == 'v') flag = !flag;
  }
}

int main(int argc, char** argv) {
  try {
    doJob();
  } catch (exception &ex) {
    cout << ex.what() << endl;
    string s;
    cin >> s;
  }
  return 0;
}