//作った人:ニコニコ技術部　akira_you
//ライセンス:勝手にコピペして使ってＯＫ　
//ソース弄ったら弄ったよ！ってこのファイルの先頭に書いてね
//
//使い方:
//cv::VideoCapture vcap(0); の所書き換えて使うカメラor動画を指定
//
//キー操作（ウインドウにフォーカスをあわせてから実行してください)
//m:マウス機能ON
//s:さわさわセンサー機能ON
//p:圧力センサー機能ON
//r:全ての機能をOFF
//q:プログラム終了
//
//ビルド方法
//　MacportsからOpenCVインストールしておく
//　ターミナルから、make mouseでビルドされるはず
//　./mouseで実行されるはず

#include<iostream>
#include "stopwatch.h"
#include "marker.h"// マーカー検出用のライブラリ
//以下2行はMacのマウス制御用コード
#include <CoreGraphics/CGDirectDisplay.H>
#include <ApplicationServices/ApplicationServices.h>

using namespace std;
//実行モード　順に:なし、マウス、さわさわセンサー、圧力センサー
enum RunMode{MODE_NONE=0,MODE_MOUSE=1,MODE_SENS=2,MODE_PRESS=4};
int mode=MODE_NONE;



//////////////////////////////////////
//圧力センサー機能
/////////////////////////////////////
cv::Point2f getDirec(float x,float y,const OppaiMarker &marker){
  const Pts &tmp_pts=marker.now();
  const Pts &base_pts=marker.base();
  double R=50;//圧力マップの平滑化半径
  double wsum=0.0;
  cv::Point2f sum(0,0);
  for(int i=0;i<base_pts.size();i++){
    cv::Point2f d=tmp_pts[i]-base_pts[i];
    double w=norm(base_pts[i]-cv::Point2f(x,y));
    w/=R;w=exp(-w*w/2.0);

    sum+= d*w;
    wsum+=w;
  } 
  sum*=1.0/wsum;
  return sum;
}

void press(const OppaiMarker marker,cv::Mat &tmp){
  for(int y=15;y<480;y+=50){    
    for(int x=20;x<640;x+=50){
      cv::Point2f dx=getDirec(x+5,y,marker) - getDirec(x,y,marker);
      cv::Point2f dy=getDirec(x,y+5,marker) - getDirec(x,y,marker);
      cv::Point2f dd(dx.x,dy.y);
      //dd.x=max(dd.x,0.0f);dd.y=max(dd.y,0.0f);
      float l=dx.x+dy.y;
      l*=10;
     
      cv::Point lt(x-l,y-l);cv::Point rb(x+l,y+l);
      rectangle(tmp,lt,rb,l<0?cv::Scalar(255,100,100):cv::Scalar(100,100,255),CV_FILLED);     
    }
  }
}



//////////////////////////////////////
//さわさわセンサー機能
/////////////////////////////////////
struct SensData{
  SensData(){
    ave=0;base=0;
  }
  SensData(float in_ave,float in_base){
    ave=in_ave;
    base=in_base;
  }
  float ave;
  float base;
};

void sens(const OppaiMarker &marker,cv::Mat &tmp){
  //中心から4個評価点を探す
  const Pts &tmp_pts=marker.now();
  const Pts &base_pts=marker.base();
  const Pts &ave_pts=marker.ave();
  
  const static cv::Point2f center(320,240);
  const int nofCenter=8;
  static vector<int> centerId;
  static deque< SensData > log(150);
  if(centerId.empty()){
    vector<pair<float,int> >len;
    for(int i=0;i<base_pts.size();i++){
      len.push_back(pair<float,int>( norm(base_pts[i]-center ) ,i));
    }
    partial_sort(len.begin(),len.begin()+nofCenter,len.end());
    for(int i=0;i<nofCenter;i++)centerId.push_back(len[i].second);
  }
  cv::Point2f ave(0,0);
  cv::Point2f base(0,0);
  for(auto idit=centerId.begin();idit!=centerId.end();idit++){
    line(tmp, tmp_pts[*idit]+cv::Point2f(0,8), tmp_pts[*idit]+cv::Point2f(0,-8), cv::Scalar(0,0,255),4);
    ave+=tmp_pts[*idit] -ave_pts[*idit];
    base+=ave_pts[*idit] -base_pts[*idit];
  }
  ave*=1.0/nofCenter;
  base*=1.0/nofCenter;
  log.push_front(SensData(norm(ave),norm(base)));
  log.pop_back();
  //過去5フレームの
  //baseのmax,aveの平均
  bool isTickle=false;
  {
    float b=0,a=0;
    for(int i=0;i<15;i++){
      b=max(log[i].base,b);
    }
    for(int i=0;i<6;i++){
      a+=log[i].ave;
    }
    a/=6;
    //cerr << a << "\t"<< a/b << "\t"<<b<<endl;
    if(0.2<a  && b<2.0){
      isTickle=true;
    }
  }
  if(isTickle){
    cv::circle(tmp,cv::Point(120,350),40,cv::Scalar(150,150,255),-1);
    cv::circle(tmp,cv::Point(500,350),40,cv::Scalar(150,150,255),-1);
  }

  line(tmp, cv::Point(160,400), cv::Point(160+300,400), cv::Scalar(0,0,0),1);
  for(int i=0;i<log.size()-1;i++){
    line(tmp, cv::Point(160+i*2,400-10*log[i].ave), cv::Point(160+i*2+2,400-10*log[i+1].ave), cv::Scalar(0,255,0),1);
    line(tmp, cv::Point(160+i*2,400-2*log[i].base), cv::Point(160+i*2+2,400-2*log[i+1].base), cv::Scalar(255,255,0),1);
  }
}



//////////////////////////////////////
//マウス機能
/////////////////////////////////////

cv::Point2f pointMul(const cv::Point2f &a,const cv::Point2f &b){
  return cv::Point2f(a.x*b.x,a.y*b.y);
}
void mouseMode(const OppaiMarker &marker,cv::Mat &tmp){
  const Pts &tmp_pts=marker.now();
  const Pts &base_pts=marker.base();
  const Pts &ave_pts=marker.ave();

  int nofBase=base_pts.size();
  float m0=0;
  cv::Point2f m1(0,0);
  cv::Point2f m2(0,0);
  for(int i=0;i<nofBase;i++){
    cv::Point2f diff=tmp_pts[i]-base_pts[i];
    m1+=diff;
    m2+=pointMul(diff,diff);
    m0+=1;
  }
  cv::Point2f ave=m1*(1.0f/m0);
  cv::Point2f std=(m2 - pointMul(m1,m1)*(1.0f/m0))*(1.0f/m0);
  std=cv::Point2f(sqrt(std.x),sqrt(std.y));
  int w=cv::norm(std)+1;
  if(200<w)w=200;
  line(tmp,cv::Point2f(320,240),cv::Point2f(320,240)+ave*10, cv::Scalar(0,255,0),w);
    
  if(2 <= norm(ave) ){
    // 空のイベントオブジェクトを作って現在のマウスカーソルの位置を取得する
    CGEventRef event = CGEventCreate(NULL);
    CGPoint location = CGEventGetLocation(event);
    CFRelease(event);
    location.x+=ave.y*3;
    location.y-=ave.x*3;
    // マウス左ボタンのダウンイベントとアップイベントを発行する
    //CGEventRef mouseDown = CGEventCreateMouseEvent(NULL, kCGEventLeftMouseDown, location, 0);
    //CGEventRef mouseUp = CGEventCreateMouseEvent(NULL, kCGEventLeftMouseUp, location, 0);
    CGEventRef mouseMove = CGEventCreateMouseEvent(NULL, kCGEventMouseMoved, location, 0);
    //CGEventPost(kCGHIDEventTap, mouseDown);
    //CGEventPost(kCGHIDEventTap, mouseUp);
    CGEventPost(kCGHIDEventTap, mouseMove);
    CFRelease(mouseMove);
    //CFRelease(mouseDown);
    //CFRelease(mouseUp);
      
  }
}








//////////////////////////////////////
//メインルーチン
/////////////////////////////////////
int main(int argvc,char *argv[]){
  OppaiMarker marker;
  //cv::VideoCapture vcap("test.avi");
  cv::VideoCapture vcap(0);
  vcap.set(CV_CAP_PROP_FRAME_WIDTH, 640);
  vcap.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
  vcap.set(CV_CAP_PROP_FPS, 15); //15fpsだといいな（多分無視される） 

  cv::VideoWriter vrec("demo.avi",  CV_FOURCC_DEFAULT,15, cv::Size(640,480),true);
  // カメラがオープンできたかの確認
  if(!vcap.isOpened()) return -1;
  cv::namedWindow("check",0);
  int count=0;
  cv::Mat tmp;

  //画像が安定するまでスキップ
  for(int i=0;i<3*15;i++)vcap >> tmp;
  
  while(1) {
    if(!vcap.grab())vcap.set(CV_CAP_PROP_POS_AVI_RATIO,0);
    vcap >> tmp;
    cv::flip(tmp, tmp, 1);    
    TIMER_START();  
    marker.kick(tmp);

    {
      const Pts & tmp_pts=marker.nowRaw();
      for(int i=0;i<tmp_pts.size();i++){
	line(tmp, tmp_pts[i]+cv::Point2f(8,8), tmp_pts[i]+cv::Point2f(-8,-8), cv::Scalar(0,0,255),1);
	line(tmp, tmp_pts[i]+cv::Point2f(8,-8), tmp_pts[i]+cv::Point2f(-8,8), cv::Scalar(0,0,255),1);  
      }
    }
    {
      const Pts & now=marker.now();
      const Pts & ave=marker.ave();
      const Pts & base=marker.base();
      for(int i=0;i<now.size();i++){
	line(tmp,base[i], now[i], cv::Scalar(0,255,0),2);
	line(tmp,ave[i] , now[i], cv::Scalar(255,0,0),2);
      }
    }
    
    if(MODE_MOUSE & mode)mouseMode(marker,tmp);
    if(MODE_SENS & mode)sens(marker,tmp);
    if(MODE_PRESS & mode)press(marker,tmp);


    TIMER_END();
    cerr<<count<<":\t";
    TIMER_PRINT();
    
    imshow("check",tmp);
    vrec << tmp;
    int key=cv::waitKey(1);
    if(key=='m')mode |= MODE_MOUSE;
    if(key=='s')mode |= MODE_SENS;
    if(key=='p')mode |= MODE_PRESS;
    if(key=='r')mode=MODE_NONE;

    if(key=='q')break;
    count++;
  }
  return 0;
}
