/*
  Bag-of-features 生成のサンプルコード
  入力：JPEGファイル名のリスト
  出力：各JPEGファイルに対応するBoFベクトル
*/

#include "main.h"


#include <stdio.h>
#include <stdlib.h>
#include <string.h>

const int DIM_SURF=128;     /* SURF特徴の次元数 */
const int MAX_SURF=1000000; /* 特徴の最大数 */
const int MAX_IMG =10000;   /* 画像の最大枚数 */
const int ITERATION=100;    /* k-Meansの最大反復回数 */
const int CODEBOOK_SIZE=500;/* コードブックのサイズ */
const int GRID=20;          /* grid samplingのピクセル間隔 */
const int scales[]={30,23,18,15,12,0}; /* grind samplingでの抽出スケール */

void listmake(){
		FILE *file;
		file = fopen("./database.txt","w");
		for(int i=1;i<=200;i++)
		fprintf(file,"./database/image (%d).JPG\n",i);
		fclose(file);
		return;
}

float distOfVec(cv::Mat vec1, cv::Mat vec2)
{
	float sum = 0.0;
	for (int i = 0; i < vec1.cols; i++) {
		sum += (vec1.at<float>(0, i) - vec2.at<float>(0, i)) * (vec1.at<float>(0, i) - vec2.at<float>(0, i));
	}

	return sqrt(sum);

}

void LoadCodeBook(string pathToCodeBook, cv::Mat *codeBook)
{
	ifstream ifs(pathToCodeBook.c_str());
	if(!ifs)
		std::cerr << "file open error" << std::endl;

	char ch[100];
	float tmp;

	for (int i = 0; i < 500; i++) {
		ifs >>ch;

		for (int j = 0; j < DIM_SURF; j++) {
			ifs >> ch;
			tmp = atof(ch);
			//cout << "j:" << j << endl;
			codeBook[i].at<float>(0, j) = tmp;
		}
	}

	return;

}

	
int main(int argc, char** argv)
{

	//プロパティ→構成プロパティ→デバッグ→コマンド引数：読み込むtxtファイル（入力画像のパスを並べたもの）へのパスを設定
	//データベースのBoF表現を作りたいならば，”./database.txt”を設定
	//"main.h"の"#define QUERY"をコメントアウトする．
	//databaseの中には1〜100が山の画像，101〜200までが森の画像として入っている．
	//1~200までの画像それぞれについてまずSURF特徴量を算出し，その全てをクラスタリングして500種類の局所パターン（visual word）とする．
	//この500種類のvisual wordはquery画像のBoFを作る際にも使うので"./codebook.txt"に保存しておく（このvisual wordの集まりをコードブックと言う）．
	//各画像に対し，500種類のvisual wordがそれぞれどのくらいあるか数えてBoFベクトルとする．
	//1~200の画像に対してのBoFベクトルを保存したものが"./bof_result.txt"



	//query画像のBoF表現を作りたいならば，”./query.txt”をコマンド引数に設定．
	//"main.h"の"#define QUERY"のコメントアウトをはずす．
	//queryの中には1〜50が山の画像，51〜100までが森の画像として入っている．
	//まずデータベースから作ったコードブックを読み込む．
	//1~100までの画像それぞれに対し，SURF特徴量を算出し，コードブックと対応付けて500種類のvisual wordがそれぞれどのくらい出てくるかを数えて，
	//BoFベクトルを算出する．
	//1~100の画像に対してのBoFベクトルを保存したものが"./bof_result_query.txt"




	int num_img=0,num_surf=0;
	char fn[256];
	FILE *fp;
	int   i,j,hist[CODEBOOK_SIZE];
	int   *img_id   =(int*)malloc((MAX_SURF+1)*sizeof(int));
	int   *vword_idx=(int*)malloc(MAX_SURF*sizeof(int));
	float *desc     =(float*)malloc(MAX_SURF*DIM_SURF*sizeof(float)),*v;
	    
	CvMat clusters, data;
	CvMemStorage* storage = cvCreateMemStorage(0);
	CvSURFParams  params = cvSURFParams(500, 1); /* 128-dim Extended SURF を指定 */
	CvSeq *imageKeypoints = 0, *imageDescriptors = 0;
	CvSURFPoint *pt=(CvSURFPoint*)malloc(sizeof(CvSURFPoint));
	pt->dir=0;
	   
	if (argc<=1) { 
	    fprintf(stderr,"usage: generate_bof {img_list}\n"); exit(1); 
	}
	if (!(fp=fopen(argv[1],"r"))) {
	    fprintf(stderr,"not found %s\n",argv[1]); exit(1);
	}
	while(fgets(fn,255,fp)){
	    fn[strlen(fn)-1]='\0';
	    IplImage* image = cvLoadImage( fn, CV_LOAD_IMAGE_GRAYSCALE );
	    if (!image) continue;
	    num_img++;
	    /* Grid sampling の grid pointsのリストの作成 */
	    imageKeypoints=cvCreateSeq(0,sizeof(CvSeq),sizeof(CvSURFPoint),storage);
	    int startx = (int)((image->width % GRID) + GRID )/2 -1;
	    int starty = (int)((image->height % GRID) + GRID )/2 -1;
	    int x,y;
	    int const *s=scales;
	    for(;*s>0;s++){
	    for(y=starty;y<image->height;y+=GRID){
	        for(x=startx;x<image->width;x+=GRID){
	        pt->pt.x=x; pt->pt.y=y; pt->size=*s;
	   	cvSeqPush(imageKeypoints,pt);
		}
	}
	    }
	    /* 設定した grid pointsのリストを用いて imageDescriptors を抽出 */
	    /* ExtractSURFの最後のパラメータを0にすると通常のFast HessianによるSURFとなる．*/
	    cvExtractSURF( image, NULL, &imageKeypoints, &imageDescriptors, storage, params, 1);
	    if (imageKeypoints->total + num_surf > MAX_SURF) break;  
	    printf("[%d] Reading %s (#surf points:%d) \n",num_img,fn,imageKeypoints->total);
	 
	    for(i=0;i<imageKeypoints->total;i++)  {
	    v = (float*)cvGetSeqElem( imageDescriptors, i );
	for(j=0;j<128;j++) desc[DIM_SURF*num_surf+j]=v[j]*1000;
	    img_id[num_surf++]=num_img;
	    }
	    cvClearSeq(imageKeypoints); 
	    cvClearSeq(imageDescriptors);
	    cvReleaseImage(&image);
	}
	fclose(fp);
	img_id[num_surf]=0;
	printf("number of images: %d\nnumber of SURF descriptors: %d\n",num_img,num_surf);
	
	cvInitMatHeader(&clusters, num_surf, 1, CV_32SC1, vword_idx, CV_AUTOSTEP );
	cvInitMatHeader(&data, num_surf, DIM_SURF, CV_32FC1, desc, CV_AUTOSTEP);


#ifdef QUERY
	
	cv::Mat codebook[CODEBOOK_SIZE];
	cv::Mat data2(&data);
	for (int j = 0; j < CODEBOOK_SIZE; j++) {
		codebook[j] = cv::Mat::zeros(1, DIM_SURF, CV_32FC1);
	}

	LoadCodeBook("./codebook.txt", codebook);

	/*std::ofstream ofs4("./codebookTest.txt");
	for (int num = 0; num < 500; num++) {
		ofs4 << "[" << num << "] ";
		for (int jj = 0; jj < DIM_SURF; jj++) {
			ofs4 << codebook[num].at<float>(0, jj) << " ";
		}
		ofs4 << endl;
	}*/

	for (int i = 0; i < data.rows; i++) {


		float minNorm = 10000000.0;
		int minNum = -1;

		//codebook探索
		for (int k = 0; k < 500; k++) {
			float norm = distOfVec(data2.row(i), codebook[k]);
			/*cout << "data2 : " << data2.row(i) << endl;
			cout << "codebook["<< k << "] : " << codebook[k] << endl;
			cout << norm << endl;*/
			if (minNorm > norm) {
				minNorm = norm;
				minNum = k;
			}
		}

		vword_idx[i] = minNum;
		//cout << "vword_idx[" << i << "] = " << vword_idx[i] << endl;
	}


#else

	/* k-means によるコードブックの生成 */
	printf("Running k-means clustering ! \n");
	cvKMeans2(&data, CODEBOOK_SIZE, &clusters, cvTermCriteria(
	CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, ITERATION, 0.01 ),1,0,cv::KMEANS_PP_CENTERS,0,0);


	std::ofstream ofs3("./codebook.txt");

	int codebookCount[CODEBOOK_SIZE];
	cv::Mat codebook[CODEBOOK_SIZE];
	cv::Mat data2(&data); 

	//cout << "aaa" << endl;

	for (int j = 0; j < CODEBOOK_SIZE; j++) {
		codebook[j] = cv::Mat::zeros(1, DIM_SURF, CV_32FC1);
		//cout << codebook[j] << endl;
		codebookCount[j] = 0;
	}

	for (int i = 0; i < data.rows; i++) {
		//cout << i << endl;
		cv::Mat tmp_surf = data2.row(i);
		codebook[vword_idx[i]] += tmp_surf;
		codebookCount[vword_idx[i]]++;
	}

	for (int j = 0; j < CODEBOOK_SIZE; j++) {
		//cout << "codebookCount[" << j << "] : " << codebookCount[j] << endl;
		for (int jj = 0; jj < DIM_SURF; jj++) {
			codebook[j].at<float>(0, jj) /= codebookCount[j];
		}

	}

	for (int num = 0; num < CODEBOOK_SIZE; num++) {
		ofs3 << "[" << num << "] ";
		for (int jj = 0; jj < DIM_SURF; jj++) {
			ofs3 << codebook[num].at<float>(0, jj) << " ";
		}
		ofs3 << endl;
	}
	
#endif

	
	/* 得られた各SURF特徴点に対するクラスタ番号を使って BoFベクトルを生成 */
	//bzero(hist,sizeof(int)*CODEBOOK_SIZE);
	for(j=0;j<CODEBOOK_SIZE;j++)
		hist[j] = 0;

#ifdef QUERY
	std::ofstream ofs("./bof_result_query.txt");
#else
	std::ofstream ofs("./bof_result_database.txt");
	

#endif

	for(i=0;i<num_surf && img_id[i]>0;i++){
		hist[vword_idx[i]]++;
		if (img_id[i]!=img_id[i+1]){
			ofs << "[" << img_id[i] << "] ";
			printf("[%d] ",img_id[i]);
			for(j=0;j<CODEBOOK_SIZE;j++) {
				ofs << hist[j] << " ";
				printf("%d ",hist[j]);
			}
			ofs << endl;
			printf("\n");
			//bzero(hist,sizeof(int)*CODEBOOK_SIZE);
			for(j=0;j<CODEBOOK_SIZE;j++)
				hist[j] = 0;
		}
	}
	free(img_id); free(vword_idx); free(desc); free(pt);
}
