// + HTK exporter
// Mingyu Chen @ Aug 23 2011
//
// 6DMG_loader + matlab exporter
// Mingyu Chen @ Apr 29 2011
#pragma comment(lib, "libmat.lib")
#pragma comment(lib, "libmx.lib")
/* -------------------------------------------------- */
/* -----   Defines for export example options   ----- */
/* -----   uncomment to export to MAT/HTK       ----- */
/* -------------------------------------------------- */
//#define EXPORT_MAT
//#define EXPORT_HTK

/* -------------------------------------------------- */
/* -----     Defines for HTK export format      ----- */
/* -------------------------------------------------- */
#define HTK_EXP		(HTK_POS | HTK_VEL | HTK_ORI | HTK_ACC | HTK_W)		// flag to control what to export
#define HTK_ACC		0x0001
#define HTK_POS		0x0002	// relative pos
#define HTK_VEL		0x0004	// displacement between every sample
#define HTK_W		0x0008
#define HTK_ORI		0x0010  // relative orientation

/* -------------------------------------------------- */
/* ----- Private type definition for HTKWrite() ----- */
/* -------------------------------------------------- */
typedef struct {
  unsigned int nSamples;	// sizeof(int) = 4 = sizeof(float)
  unsigned int sampPeriod;  // in 100ns units
  unsigned short sampSize;  // sizeof(short)= 2
  unsigned short parmKind;
} htk_header_t;

#define H_LPC       1
#define H_LPREFC    2
#define H_LPCEPSTRA 3
#define H_MFCC      6
#define H_FBANK     7
#define H_USER      9

#define HASENERGY   0x0040
#define HASNOE      0x0080
#define HASDELTA    0x0100
#define HASZMEAN    0x0800

// Utils to swap endian (big <-> little)
inline void endian_swap(unsigned short& x){
    x = (x>>8) | 
        (x<<8);
}

inline void endian_swap(unsigned int& x){
    x = (x>>24) | 
        ((x<<8) & 0x00FF0000) |
        ((x>>8) & 0x0000FF00) |
        (x<<24);
}


// ----------------------------------------------------
#define N_ELEM 14           // total elements per sample
#define SAMP_PERIOD 166666	// 60 Hz

#define M_PI		3.14159265358979323846	// pi
#define EPSILON		0.01 // threshold or quaternion conversion

#include <iostream>
#include <stdio.h>
#include <string.h> // For strcmp()
#include <stdlib.h> // For EXIT_FAILURE, EXIT_SUCCESS
#include <vector>   // For STL
#include <math.h>
#include "GestureDef.h"
#include "Database.h"
#include "mat.h"

using namespace std;
using namespace GestureDef;

int GestToMat(char* file, Gesture& g);
int GestToHTK(char* file, Gesture& g);
int preprocessHTK(Gesture& g, unsigned short nElem, float* buff);

int main()
{
	char ch;
	
	// initialize the database and connect to SQLite
	Database* pDatabase = new Database();
	if (pDatabase->Open()){		
		
		// Example 1: load all testers
		vector<TesterInfo> testersList = pDatabase->LoadTesters();
		for (int i=0; i<testersList.size(); i++){
			TesterInfo tester = testersList.at(i);
			string hand = tester.rightHand ? "right-handed" : "left -handed";
			cout << tester.name << " is " << hand << endl;
		}		

		/*
		// Example 2: load the first tester's 5th trial of V_SHAPE, and print the timestamp + pos
		Gesture g2;
		if (pDatabase->LoadOneGesture(getGestureName(V_SHAPE), testersList.at(5).name, 5, g2)){			
			printf("\n\ntime(ms) pos.x pos.y pos.z\n");
			for (int i=0; i<g2.data.size(); i++){			
				Sample s = g2.data.at(i);
				printf("%6.2f\t %4.3f %4.3f %4.3f\n", s.timestamp, s.pos.x, s.pos.y, s.pos.z);							
			}
			cout << g2.name << " by " << g2.tester << ": trial No. " << g2.trial << endl;
		}		
		*/

		// Example 3: MATLAB exporter		
		Gesture g3;
		if (pDatabase->LoadOneGesture(getGestureName(V_SHAPE), testersList.at(5).name, 5, g3)){
			char filename[] = "test.mat";
			GestToMat(filename, g3);			
		}		

#ifdef EXPORT_MAT		
		// Example 4: MATLAB exporter for all		
		for (int i=0; i<TOTAL_GESTURES; i++){			// gesture idx
			for (int j=0; j<testersList.size(); j++){	// tester  idx		
				string name = getGestureName(i);
				string tester = testersList.at(j).name;
				int trialNum = pDatabase->GetCurrentTrial(name, tester);
				for (int k=1; k<trialNum; k++){
					Gesture gest;
					char filename[30];
					if (testersList.at(j).rightHand)
						sprintf(filename, "../matR/g%02d_%s_t%02d.mat", i, tester.c_str(), k);
					else
						sprintf(filename, "../matL/g%02d_%s_t%02d.mat", i, tester.c_str(), k);

					if (pDatabase->LoadOneGesture(name, tester, k, gest)){
						GestToMat(filename, gest);
					}
				}
			}
		}
#endif

		// Example 5: HTK exporter
		Gesture g5;
		if (pDatabase->LoadOneGesture(getGestureName(SWIPE_RIGHT), "B1", 2, g5)){
			char filename[] = "test.htk";
			GestToHTK(filename, g5);
		}
		
#ifdef EXPORT_HTK		
		// Example 6: HTK exporter for all
		for (int i=0; i<TOTAL_GESTURES; i++){			// gesture idx
			for (int j=0; j<testersList.size(); j++){	// tester  idx		
				string name = getGestureName(i);
				string tester = testersList.at(j).name;
				int trialNum = pDatabase->GetCurrentTrial(name, tester);
				for (int k=1; k<trialNum; k++){
					Gesture gest;
					char filename[30];
					sprintf(filename, "../htk/g%02d_%s_t%02d.htk", i, tester.c_str(), k);					

					if (pDatabase->LoadOneGesture(name, tester, k, gest)){
						GestToHTK(filename, gest);
					}
				}
			}
		}
#endif		
	}

	cout << "Press enter to continue.";
	cin.get(ch);
	return 0;
}

int GestToMat(char* file, Gesture& g)
{
	MATFile *pmat;
	mxArray *pa, *pa2, *pa3;	

	pmat = matOpen(file, "w");
	if (pmat == NULL){
		cout << "GestToMat fails at creating file " << file << endl;
		return -1;
	}

	int num = g.data.size();
	pa  = mxCreateNumericMatrix(N_ELEM, num, mxSINGLE_CLASS, mxREAL); // (row, col) or (tuple, sample #)
	pa2 = mxCreateNumericMatrix(3,1, mxSINGLE_CLASS, mxREAL);	
	pa3 = mxCreateNumericMatrix(3,1, mxSINGLE_CLASS, mxREAL);
	if (pa == NULL || pa2 == NULL || pa3 == NULL){
		cout << "GestToMat fails at creating mxArray" << endl;
		return -1;
	}

	// fill in mxArrays	
	float *ptr = (float*)mxGetData(pa);
	for (int i=0; i<num; i++){	
		Sample s = g.data.at(i);
		memcpy((void*)ptr, (void*)&s, sizeof(Sample));
		ptr += N_ELEM;
	}
	memcpy(mxGetData(pa2), (void*)&(g.noise), sizeof(YPR));
	memcpy(mxGetData(pa3), (void*)&(g.bias),  sizeof(YPR));

	// save mxArrays
	if (matPutVariable(pmat, "gest", pa) != 0){	
		cout << "GestToMat fails at saving samples: " << file << endl;		
		return -1;
	}
	if (matPutVariable(pmat, "noise", pa2) != 0){
		cout << "GestToMat fails at saving noise " << file << endl;
		return -1;
	}	
	if (matPutVariable(pmat, "bias", pa3) != 0){
		cout << "GestToMat fails at saving bias " << file << endl;
		return -1;
	}

	// clean up
	mxDestroyArray(pa);
	if (matClose(pmat) != 0) {
		cout << "GestToMat fails at closing file " << file << endl;
		return -1;
	}
	
	cout << "Export to " << file << endl;
	return 0;
}


int GestToHTK(char* fname, Gesture& g)
{
	unsigned short nElem = 0; // the # of elems we want to export (not ALL!)
	if (HTK_EXP & HTK_ACC)	nElem += 3;
	if (HTK_EXP & HTK_POS)	nElem += 3;
	if (HTK_EXP & HTK_VEL)	nElem += 3;	
	if (HTK_EXP & HTK_W)	nElem += 3;
	if (HTK_EXP & HTK_ORI)	nElem += 4;
	
	FILE* f = fopen(fname, "wb"); // write binary
	htk_header_t header;
	unsigned int num = g.data.size(); // # of samples	
	header.nSamples = num;
	header.sampPeriod = SAMP_PERIOD;	
	header.sampSize = nElem * sizeof(float);
	header.parmKind = H_USER; // user defined data w/o flags (HASENERGY, HASDELTA, etc)

	// Mingyu: swap from little endian (Windows) to big endian (required by HTK)
	endian_swap(header.nSamples);
	endian_swap(header.sampPeriod);
	endian_swap(header.sampSize);
	endian_swap(header.parmKind);

	// Write HTK header
	if (fwrite(&header, sizeof(htk_header_t), 1, f) != 1){
		cout << "Error: write HTK header of " << fname << endl;
		return -1;
	}
		
	// Preprocess the data
	cout << "Export to " << fname << "\t";
	float* buff = new float [nElem * g.data.size()];
	preprocessHTK(g, nElem, buff);
	
	// Write HTK data
	// Mingyu: swap from little endian (Windows) to big endian (required by HTK)
	unsigned int* uint_buff = (unsigned int*)buff;
	for (int j=0; j<nElem * g.data.size(); j++){
		endian_swap(uint_buff[j]);
	}
	if (fwrite(uint_buff, sizeof(unsigned int), nElem*g.data.size(), f) != nElem*g.data.size())
	{	
		cout << "Error: write HTK data at " << fname << endl;
		return -1;
	}

	cout << "OK." << endl;
	delete [] buff;
	fclose(f);
	return 0;
}

// Have to pre-process the data (e.g., use relative displacement)
int preprocessHTK(Gesture& g, unsigned short nElem, float* buff)
{	
	XYZ startPos = g.data.at(0).pos;
	XYZ prevPos  = startPos;
	ORI startOriInv = quatConj(g.data.at(0).ori);
	ORI prevOriInv = startOriInv;
	for (unsigned int i=0; i<g.data.size(); i++)
	{
		Sample s = g.data.at(i);
		XYZ acc, pos, vel;
		YPR w;
		ORI q, dq;

		pos.x = s.pos.x - startPos.x;
		pos.y = s.pos.y - startPos.y;
		pos.z = s.pos.z - startPos.z;
		vel.x = s.pos.x - prevPos.x;
		vel.y = s.pos.y - prevPos.y;
		vel.z = s.pos.z - prevPos.z;
		w.yaw   = s.w.yaw;
		w.pitch = s.w.pitch;
		w.roll  = s.w.roll;
		acc = s.acc;
		q = s.ori;	
		dq = quatNorm(quatMul(s.ori, prevOriInv));	// delta quaternion		

		prevPos = s.pos;
		prevOriInv = quatConj(s.ori);

		// store into buff
		float* ptr = buff + i*nElem;
		if (HTK_EXP & HTK_ACC){
			memcpy(ptr, &acc, sizeof(XYZ));
			ptr += 3;
		}
		if (HTK_EXP & HTK_POS){
			memcpy(ptr, &pos, sizeof(XYZ));
			ptr += 3;
		}
		if (HTK_EXP & HTK_VEL){
			memcpy(ptr, &vel, sizeof(XYZ));
			ptr += 3;
		}
		if (HTK_EXP & HTK_W){
			memcpy(ptr, &w, sizeof(YPR));
			ptr += 3;
		}
		if (HTK_EXP & HTK_ORI){
			memcpy(ptr, &q, sizeof(ORI));
			ptr += 4;
		}
	}
	return 0;
}