/*
* This file is part of robotreality
*
* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
* http://opensource.cit-ec.de/projects/robotreality
*
* This file may be licensed under the terms of the
* GNU General Public License Version 3 (the ``GPL''),
* or (at your option) any later version.
*
* Software distributed under the License is distributed
* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
* express or implied. See the GPL for the specific language
* governing rights and limitations.
*
* You should have received a copy of the GPL along with this
* program. If not, go to http://www.gnu.org/licenses/gpl.html
* or write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The development of this software was supported by the
* Excellence Cluster EXC 277 Cognitive Interaction Technology.
* The Excellence Cluster EXC 277 is a grant of the Deutsche
* Forschungsgemeinschaft (DFG) in the context of the German
* Excellence Initiative.
*
*/

#include "HeadTracker.h"

#define DEBUG_MODE_NO_IMU 0

HeadTracker::HeadTracker(ConfigOptions *c){
	ximu = NULL;
	cfg = c;
	
	
#if DEBUG_MODE_NO_IMU
	return;
#else
	//file source or live imu?
	if (strncmp(cfg->imu_data_source.c_str(), "/dev/", 5) == 0){
		datasource_is_file = false;
	}else{
		datasource_is_file = true;
	}
	
	if (datasource_is_file){
		//import data from dumpfile:
		if(!read_dump(cfg->imu_data_source)){
			printf("> error reading imu dump file '%s'\n",cfg->imu_data_source.c_str());
			exit(1);
		}
	}else{
		//open ximu:
		ximu = new XIMU(cfg->imu_data_source.c_str(), XIMU::XIMU_LOGLEVEL_LOG);
		if (!ximu->get_device_detected()){
			printf("> ERROR: no XIMU was found on port '%s'\n", cfg->imu_data_source.c_str());
			exit(1);
		}
	}
#endif
}

bool HeadTracker::read_dump(string filename){
	int linenum = 0;
	int dataset_index = 0;
	string line;
	//tro to slurp datase
	printf("> reading imu dump file '%s'\n",filename.c_str());

	ifstream inFile(filename.c_str());
	
	while (getline (inFile, line)){
		string::size_type i = line.find_first_not_of ( " \t\n\v" );
		//printf("> got '%s'\n",line.c_str());
		if ((i != string::npos) && (line[i] == '#')){
			//ignore comments
		}else{
			//remove ! 
			if (line.at(0) == '!'){
				line = line.substr(1);
			}

			//remove ' '
			while (line.at(0) == ' '){
				line = line.substr(1);
			}

			istringstream linestream(line);
			//printf("processing '%s'\n", line.c_str());
			string item;
			int idx = 0;
			imu_data_t rot_data;
			while(getline(linestream, item, ' ')){
				if (idx == 0){
					int indexpos = atoi(item.data());
					if (indexpos != dataset_index){
						printf("> dataset mismatch, expected index %d but got %d\n",dataset_index, indexpos);
						return false;
					}
				}else if (idx == 1){
					rot_data.x = atof(item.data());
				}else if (idx == 2){
					rot_data.y = atof(item.data());
				}else if (idx == 3){
					rot_data.z = atof(item.data());
				}
				idx++;
			}
			
			//insert into vector:
			imu_data_vector.push_back(rot_data);
			dataset_index++;
		}
		linenum++;
	}
	return true;
}


void HeadTracker::set_zero_rot(){
#if DEBUG_MODE_NO_IMU
		return;
#else
	//store zero rotation:
	if (datasource_is_file){
		cfg->imu_zero_rotation.x = 0.0;
		cfg->imu_zero_rotation.y = 0.0;
		cfg->imu_zero_rotation.z = 0.0;
		cfg->imu_zero_rotation = get_euler_angles();
	}else{
		//tell ximu to tare:
		ximu->send_command_algorithm_tare();
	}
#endif
}


imu_data_t HeadTracker::get_euler_angles(){
	imu_data_t euler = {0.0, 0.0, 0.0};
	int frame_id = cfg->frame_number;

	#if DEBUG_MODE_NO_IMU
		//dummy angles:
		euler.x = 1.0;
		euler.y = 2.0;
		euler.z = 3.0;
	#else
	if (datasource_is_file){
		if (frame_id < imu_data_vector.size()){
			//fetch data from data vector (slurped from dump file)
			euler.x = imu_data_vector[frame_id].x - cfg->imu_zero_rotation.x;
			euler.y = imu_data_vector[frame_id].y - cfg->imu_zero_rotation.y;
			euler.z = imu_data_vector[frame_id].z - cfg->imu_zero_rotation.z;
			return euler;
		}else{
			return euler;
		}
	}else{
		//fetch data from IMU
		double e[3];
		ximu->get_euler(e);
		euler.x = e[0];
		euler.y = e[1];
		euler.z = e[2];
		return euler;
	}
	#endif
	
	return euler;
}