//作った人:ニコニコ技術部　akira_you
//ライセンス:勝手にコピペして使ってＯＫ　
//ソース弄ったら弄ったよ！ってこのファイルの先頭に書いてね

#include "marker.h"
using namespace cv;
static const int maxMarker=24; //マーカーの数（ノイズ対策に数個増やしてもＯＫ）
static void triangle(Mat &img,int x1,int y1,int x2,int y2,int x3,int y3,const Scalar &s){
  Point p1 [3];
  p1[0].x=x1;p1[0].y=y1;
  p1[1].x=x2;p1[1].y=y2;
  p1[2].x=x3;p1[2].y=y3;
  Point *ppt[]={p1};
  int npt[]={3};
  fillPoly(img,(const Point **)ppt,(int *)npt,1,Scalar(0.5));
}

static void filter(Mat &mix,Mat &mini){
  double rate=8.0;
  //miniでおおよその位置を探して,mixで詳細位置を探す
  erode(mix,mix,Mat(),Point(-1,-1),3);
  for(int i=0;i<3;i++)blur(mix,mix,Size(5,5));
  resize(mix,mini,Size(0,0),1.0/rate,1.0/rate,INTER_AREA);


  static Mat mask;
  dilate(mini,mini,Mat());
  //  erode(mini,mask,Mat(),Point(-1,-1),3);
  dilate(mini,mask,Mat(),Point(-1,-1),1);
  erode(mask,mask,Mat(),Point(-1,-1),4);
  mini=mini-mask;
  mini=min(mini,0);
  normalize(mini,mini,1,0,NORM_MINMAX);//デバグで見るときはONに


  //miniの処理
  //光量変動キャンセル
  static Mat b;
  blur(mini,b,Size(5,5));
  blur((mini-b)*2+0.5 ,mini,Size(3,3));
  blur(mini,mini,Size(3,3));


  Mat tmp;//アップサンプリング
  resize(mini,tmp,Size(0,0),rate,rate,INTER_CUBIC);
  mix=tmp;

  //namedWindow("tmp");imshow("tmp",mini);
  //namedWindow("mask");imshow("mask",mask);

}
static void getMarker(std::vector<cv::Point2f> &now_pts,cv::Mat &mix,cv::Mat &mini){
  static int rate=8;
  //一番くらい点を順に追加
  Point minLoc,minLoc2;double minVal;
  now_pts.clear();
  now_pts.reserve(maxMarker);
  for(int i=0;i<maxMarker;i++){
    minMaxLoc(mini,&minVal,NULL,&minLoc,NULL);
    //if(0.4<minVal)break;
    rectangle(mini,minLoc+Point(-24/rate,-24/rate),minLoc+Point(24/rate,24/rate),Scalar(80),CV_FILLED);
    //realスケールで探す
    Point pos=minLoc*rate;
    minMaxLoc(	
	      mix(Rect(pos,Size(rate,rate)))
	      ,&minVal,NULL,&minLoc2,NULL);
    minLoc2+=minLoc*rate;
    now_pts.push_back(minLoc2);
  }
  //namedWindow("mm");imshow("mm",mini);
}
static void updateMarker(Mat &mix,Mat &mini,vector<Point2f> &now_pts,vector<Point2f> &ave_pts,vector<Point2f> &base_pts,vector<int> &tarid){
  getMarker(now_pts,mix,mini);
  if(base_pts.empty()){
    ave_pts=base_pts=now_pts;
    tarid.resize(base_pts.size());
  }
  static vector<int> tarCount(maxMarker);
  int nofBase=base_pts.size();
  int nofMarker=now_pts.size();
  fill(tarCount.begin(),tarCount.begin()+nofMarker,0);
  auto idit=tarid.begin();
  for(auto bit=ave_pts.begin();bit!=ave_pts.end();bit++,idit++){
    double minLen=1000;
    idit[0] = -1;
    for(int t=0;t<nofMarker;t++){
      double len=norm(now_pts[t]- bit[0]);
      if(len<minLen){
	minLen=len;
	idit[0]=t;
      }
    }
    if(50<minLen)idit[0]=-1;
    if(0<=idit[0])tarCount[idit[0]]++;
  }
  //重複を解消(滅多に通らないので重くてok)
  for(int t=0;t<nofMarker;t++){
    if(tarCount[t]<=1)continue;
    int minId=0;
    double minLen=10000;
    for(int i=0;i<nofBase;i++){
      if(tarid[i]!=t)continue;
      double len=norm(ave_pts[i]- now_pts[tarid[i]]);
      if(len<minLen){
	minLen=len;
	minId=i;
      }
    }
    for(int i=0;i<nofBase;i++){
      if(tarid[i]!=t|| i==minId)continue;
      tarid[i] = -1;
    }
  }

  {//base_ptsを修正
    double bnorm=0;
    int c=0;
    for(int i=0;i<nofBase;i++){
      if(tarid[i]<0)continue;
      c++;
      double b=norm(ave_pts[i] - now_pts[tarid[i]]);
      bnorm+=b*b;
    }
    bnorm=sqrt(bnorm);
    if(bnorm< 1.5){
      //std::cerr <<"base resetiing"<<std::endl;
      for(int i=0;i<nofBase;i++){
	if(tarid[i]<0)continue;
	base_pts[i]=     base_pts[i]*0.7+now_pts[tarid[i]]*0.3;  
      }
    }
  }
  //ave_ptsを修正
  for(int i=0;i<nofBase;i++){
    if(tarid[i]<0){ //自分のターゲットが見つからなかったのでbaseに帰って行く
      ave_pts[i]= ave_pts[i]*0.95+base_pts[i]*0.05;
    }else{
      //平均位置
      ave_pts[i]= ave_pts[i]*0.65+now_pts[tarid[i]]*0.35;      
      //ave_pts[i]= ave_pts[i]*0.3+now_pts[tarid[i]]*0.7;      
    }
  }
}
static void conv(const cv::Mat &tmp,Mat &mix,Mat &mini){
  for(int y=0;y<480;y++){
    const uchar * it=tmp.ptr(y);
    float * mit=mix.ptr<float>(y);
    for(int x=0;x<640;x++){
      it++;
      int a= *it;it++;
      a += *it;it++;
      *mit = (float)a/255.0f/2.0f;	
      mit++;
    }
  }
  filter(mix,mini);
}

OppaiMarker::OppaiMarker(){
  mix=Mat(Size(640,480),CV_32F);
  now_pts.reserve(maxMarker);
  ave_pts.reserve(maxMarker);
  base_pts.reserve(maxMarker);
  now_cpts.reserve(maxMarker);
  ave_cpts.reserve(maxMarker);
  base_cpts.reserve(maxMarker);
  tarid.reserve(maxMarker);
}
void OppaiMarker::kick(cv::Mat &tmp){
  conv(tmp,mix,mini);
  //namedWindow("mini");
  //imshow("mini",mini);
  updateMarker(mix,mini,now_pts,ave_pts,base_pts,tarid);
  now_cpts.clear();
  ave_cpts.clear();
  base_cpts.clear();
  for(int i=0;i<base_pts.size();i++){
    if(tarid[i]<0)continue;
    now_cpts.push_back(now_pts[tarid[i]]);
    ave_cpts.push_back(ave_pts[i]);
    base_cpts.push_back(base_pts[i]);
  }
}
