/*****************************************************************
 *
 * This file is part of the GMAPPING project
 *
 * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
 * Cyrill Stachniss, and Wolfram Burgard
 *
 * This software is licensed under the 3-Clause BSD License
 * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
 * and Wolfram Burgard.
 * 
 * Further information on this license can be found at:
 * https://opensource.org/licenses/BSD-3-Clause
 * 
 * GMAPPING is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied 
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  
 *
 *****************************************************************/


#include "gmapping/carmenwrapper/carmenwrapper.h"

using namespace GMapping;
using namespace std;

//static vars for the carmenwrapper
SensorMap CarmenWrapper::m_sensorMap;
deque<RangeReading> CarmenWrapper::m_rangeDeque;
pthread_mutex_t CarmenWrapper::m_mutex;  
sem_t CarmenWrapper::m_dequeSem;  
pthread_mutex_t CarmenWrapper::m_lock;  
pthread_t CarmenWrapper::m_readingThread;
RangeSensor* CarmenWrapper::m_frontLaser=0;
RangeSensor* CarmenWrapper::m_rearLaser=0;
bool CarmenWrapper::m_threadRunning=false;
OrientedPoint CarmenWrapper::m_truepos;
bool CarmenWrapper::stopped=true;


void CarmenWrapper::initializeIPC(const char* name) {
  carmen_ipc_initialize(1,(char **)&name);
}




int CarmenWrapper::registerLocalizationMessages(){
  lock();
  IPC_RETURN_TYPE err;

  /* register globalpos message */
  err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_NAME, IPC_VARIABLE_LENGTH, 
		      CARMEN_LOCALIZE_GLOBALPOS_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_GLOBALPOS_NAME);

  /* register robot particle message */
  err = IPC_defineMsg(CARMEN_LOCALIZE_PARTICLE_NAME, IPC_VARIABLE_LENGTH, 
		      CARMEN_LOCALIZE_PARTICLE_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_PARTICLE_NAME);

/*
  carmen_localize_subscribe_initialize_placename_message(NULL, 
							 (carmen_handler_t)
							 carmen_localize_initialize_placename_handler,
							 CARMEN_SUBSCRIBE_LATEST);

  // register map request message 
  err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_QUERY_NAME, IPC_VARIABLE_LENGTH,
		      CARMEN_LOCALIZE_MAP_QUERY_FMT);
  carmen_test_ipc_exit(err, "Could not define", 
		       CARMEN_LOCALIZE_MAP_QUERY_NAME);

  err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_NAME, IPC_VARIABLE_LENGTH,
		      CARMEN_LOCALIZE_MAP_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_MAP_NAME);

  // subscribe to map request message 
  err = IPC_subscribe(CARMEN_LOCALIZE_MAP_QUERY_NAME, map_query_handler, NULL);
  carmen_test_ipc(err, "Could not subscribe", CARMEN_LOCALIZE_MAP_QUERY_NAME);
  IPC_setMsgQueueLength(CARMEN_LOCALIZE_MAP_QUERY_NAME, 1);


  // register globalpos request message 
  err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 
		      IPC_VARIABLE_LENGTH,
		      CARMEN_DEFAULT_MESSAGE_FMT);
  carmen_test_ipc_exit(err, "Could not define", 
		       CARMEN_LOCALIZE_MAP_QUERY_NAME);

  // subscribe to globalpos request message 
  err = IPC_subscribe(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 
		      globalpos_query_handler, NULL);
  carmen_test_ipc(err, "Could not subscribe", 
		  CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME);
  IPC_setMsgQueueLength(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 1);
*/
  unlock();
  return 0;
}

bool CarmenWrapper::start(const char* name){
	if (m_threadRunning)
		return false;
	carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)robot_frontlaser_handler, CARMEN_SUBSCRIBE_LATEST);
	carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)robot_rearlaser_handler, CARMEN_SUBSCRIBE_LATEST);
	carmen_simulator_subscribe_truepos_message(NULL,(carmen_handler_t) simulator_truepos_handler, CARMEN_SUBSCRIBE_LATEST);

	IPC_RETURN_TYPE err;

	err = IPC_subscribe(CARMEN_NAVIGATOR_GO_NAME, navigator_go_handler,  NULL);
	carmen_test_ipc_exit(err, "Could not subscribe", 
			     CARMEN_NAVIGATOR_GO_NAME);
	IPC_setMsgQueueLength(CARMEN_NAVIGATOR_GO_NAME, 1);

	err = IPC_subscribe(CARMEN_NAVIGATOR_STOP_NAME, navigator_stop_handler,  NULL);
	carmen_test_ipc_exit(err, "Could not subscribe", 
			     CARMEN_NAVIGATOR_STOP_NAME);
	IPC_setMsgQueueLength(CARMEN_NAVIGATOR_STOP_NAME, 1);



	signal(SIGINT, shutdown_module);
	pthread_mutex_init(&m_mutex, 0);
	pthread_mutex_init(&m_lock, 0);
	sem_init(&m_dequeSem, 0, 0);
	m_threadRunning=true;
	pthread_create (&m_readingThread,0,m_reading_function,0);
	return true; 
}

void CarmenWrapper::lock(){
	//cerr <<"LOCK" << endl;
	pthread_mutex_lock(&m_lock);
}

void CarmenWrapper::unlock(){
	//cerr <<"UNLOCK" << endl;
	pthread_mutex_unlock(&m_lock);
}


bool CarmenWrapper::sensorMapComputed(){
	pthread_mutex_lock(&m_mutex);
	bool smok=m_frontLaser;
	pthread_mutex_unlock(&m_mutex);
	return smok;
}

const SensorMap& CarmenWrapper::sensorMap(){
	return m_sensorMap;
}
 
bool CarmenWrapper::isRunning(){
	return m_threadRunning;
}

bool CarmenWrapper::isStopped(){
	return stopped;
}

int CarmenWrapper::queueLength(){
	int ql=0;
	pthread_mutex_lock(&m_mutex);
	ql=m_rangeDeque.size();
	pthread_mutex_unlock(&m_mutex);
	return ql;
}

OrientedPoint CarmenWrapper::getTruePos(){
	return m_truepos;
}

bool CarmenWrapper::getReading(RangeReading& reading){
	bool present=false;
	sem_wait(&m_dequeSem);
	pthread_mutex_lock(&m_mutex);
	if (!m_rangeDeque.empty()){
//		cerr << __func__ << ": queue size=" <<m_rangeDeque.size() << endl;
		reading=m_rangeDeque.front();
		m_rangeDeque.pop_front();
		present=true;
	}
	int sval;
	sem_getvalue(&m_dequeSem,&sval);
//	cerr << "fetch. elements= "<< m_rangeDeque.size() << " sval=" << sval <<endl;
	pthread_mutex_unlock(&m_mutex);
	return present;
}

void CarmenWrapper::addReading(RangeReading& reading){
	pthread_mutex_lock(&m_mutex);
	m_rangeDeque.push_back(reading);
	pthread_mutex_unlock(&m_mutex);
	sem_post(&m_dequeSem);
	int sval;
	sem_getvalue(&m_dequeSem,&sval);
//	cerr << "post. elements= "<< m_rangeDeque.size() << " sval=" << sval <<endl;
}


//RangeSensor::RangeSensor(std::string name, unsigned int beams_num, unsigned int res, const OrientedPoint& position, double span, double maxrange): 

void CarmenWrapper::robot_frontlaser_handler(carmen_robot_laser_message* frontlaser) {
/*	if (! m_rangeSensor){
		double res=0;
		if (frontlaser->num_readings==180 || frontlaser->num_readings==181)
			res=M_PI/180;
		if (frontlaser->num_readings==360 || frontlaser->num_readings==361)
			res=M_PI/360;
		assert(res>0);
		m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9);
		m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor));
		
		cout << __func__ 
		     << ": FrontLaser configured." 
		     << " Readings " << m_rangeSensor->beams().size() 
		     << " Resolution " << res << endl;
	}
	
	RangeReading reading(m_rangeSensor, frontlaser->timestamp);
	reading.resize(m_rangeSensor->beams().size());
	for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){
		reading[i]=(double)frontlaser->range[i];
	}
	reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta));
*/	
	RangeReading reading=carmen2reading(*frontlaser);
	addReading(reading);
}

void CarmenWrapper::robot_rearlaser_handler(carmen_robot_laser_message* rearlaser) {
/*	if (! m_rangeSensor){
		double res=0;
		if (frontlaser->num_readings==180 || frontlaser->num_readings==181)
			res=M_PI/180;
		if (frontlaser->num_readings==360 || frontlaser->num_readings==361)
			res=M_PI/360;
		assert(res>0);
		m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9);
		m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor));
		
		cout << __func__ 
		     << ": FrontLaser configured." 
		     << " Readings " << m_rangeSensor->beams().size() 
		     << " Resolution " << res << endl;
	}
	
	RangeReading reading(m_rangeSensor, frontlaser->timestamp);
	reading.resize(m_rangeSensor->beams().size());
	for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){
		reading[i]=(double)frontlaser->range[i];
	}
	reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta));
*/	
	RangeReading reading=carmen2reading(*rearlaser);
	addReading(reading);
}




void CarmenWrapper:: navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) {
  carmen_navigator_go_message msg;
  FORMATTER_PTR formatter;
  IPC_RETURN_TYPE err;

  formatter = IPC_msgInstanceFormatter(msgRef);
  err = IPC_unmarshallData(formatter, callData, &msg,
			   sizeof(carmen_navigator_go_message));  
  IPC_freeByteArray(callData);

  carmen_test_ipc_return
    (err, "Could not unmarshall", IPC_msgInstanceName(msgRef));
  cerr<<"go"<<endl;
  stopped=false;
}


void CarmenWrapper:: navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) {
  carmen_navigator_stop_message msg;
  FORMATTER_PTR formatter;
  IPC_RETURN_TYPE err;

  formatter = IPC_msgInstanceFormatter(msgRef);
  err = IPC_unmarshallData(formatter, callData, &msg,
			   sizeof(carmen_navigator_stop_message));  
  IPC_freeByteArray(callData);

  carmen_test_ipc_return
    (err, "Could not unmarshall", IPC_msgInstanceName(msgRef));
  cerr<<"stop"<<endl;
  stopped=true;
}



void CarmenWrapper::simulator_truepos_handler(carmen_simulator_truepos_message* truepos){
	m_truepos.x=truepos->truepose.x;
	m_truepos.y=truepos->truepose.y;
	m_truepos.theta=truepos->truepose.theta;
}

RangeReading CarmenWrapper::carmen2reading(const carmen_robot_laser_message& msg){
	//either front laser or rear laser
	double dth=msg.laser_pose.theta-msg.robot_pose.theta;
	dth=atan2(sin(dth), cos(dth));

	if (msg.laser_pose.theta==msg.robot_pose.theta && !m_frontLaser){
		double res=0;
		res = msg.config.angular_resolution;
// 		if (msg.num_readings==180 || msg.num_readings==181)
// 			res=M_PI/180;
// 		if (msg.num_readings==360 || msg.num_readings==361)
// 			res=M_PI/360;
		assert(res>0);
		string sensorName="FLASER";
		OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
		OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
		OrientedPoint dp=absoluteDifference(lpose, rpose);
		m_frontLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0, 
					      msg.config.maximum_range);
		m_frontLaser->updateBeamsLookup();
		m_sensorMap.insert(make_pair(sensorName, m_frontLaser));
		
		cout << __func__ 
		     << ": " << sensorName <<" configured." 
		     << " Readings " << m_frontLaser->beams().size() 
		     << " Resolution " << res << endl;
	}
	if (msg.laser_pose.theta!=msg.robot_pose.theta && !m_rearLaser){
		double res=0;
		res = msg.config.angular_resolution;
// 		if (msg.num_readings==180 || msg.num_readings==181)
// 			res=M_PI/180;
// 		if (msg.num_readings==360 || msg.num_readings==361)
// 			res=M_PI/360;
		assert(res>0);
		OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
		OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
		OrientedPoint dp=absoluteDifference(lpose, rpose);
		string sensorName="RLASER";
		m_rearLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0, 
					      msg.config.maximum_range);
		m_rearLaser->updateBeamsLookup();
		m_sensorMap.insert(make_pair(sensorName, m_rearLaser));
		
		cout << __func__ 
		     << ": " << sensorName <<" configured." 
		     << " Readings " << m_rearLaser->beams().size() 
		     << " Resolution " << res << endl;
	}	

	const RangeSensor * rs=(msg.laser_pose.theta==msg.robot_pose.theta)?m_frontLaser:m_rearLaser;
	RangeReading reading(rs, msg.timestamp);
	reading.resize(rs->beams().size());
	for (unsigned int i=0; i< (unsigned int)msg.num_readings; i++){
		reading[i]=(double)msg.range[i];
	}
	reading.setPose(OrientedPoint(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta));
	return reading;
}

void CarmenWrapper::publish_globalpos(carmen_localize_summary_p summary)
{
  lock();
  static carmen_localize_globalpos_message globalpos;
  IPC_RETURN_TYPE err;
  
  globalpos.timestamp = carmen_get_time();
  globalpos.host = carmen_get_host();
  globalpos.globalpos = summary->mean;
  globalpos.globalpos_std = summary->std;
  globalpos.globalpos_xy_cov = summary->xy_cov;
  globalpos.odometrypos = summary->odometry_pos;
  globalpos.converged = summary->converged;
  err = IPC_publishData(CARMEN_LOCALIZE_GLOBALPOS_NAME, &globalpos);
  carmen_test_ipc_exit(err, "Could not publish", 
		       CARMEN_LOCALIZE_GLOBALPOS_NAME);  
  unlock();
}

/* publish a particle message */

void CarmenWrapper::publish_particles(carmen_localize_particle_filter_p filter, 
		       carmen_localize_summary_p summary)
{
  lock();
  static carmen_localize_particle_message pmsg;
  IPC_RETURN_TYPE err;

  pmsg.timestamp = carmen_get_time();
  pmsg.host = carmen_get_host();
  pmsg.globalpos = summary->mean;
  pmsg.globalpos_std = summary->mean;
  pmsg.num_particles = filter->param->num_particles;
  pmsg.particles = (carmen_localize_particle_ipc_p)filter->particles;
  err = IPC_publishData(CARMEN_LOCALIZE_PARTICLE_NAME, &pmsg);
  carmen_test_ipc_exit(err, "Could not publish", 
		       CARMEN_LOCALIZE_PARTICLE_NAME);  
  fprintf(stderr, "P");
  unlock();
}





void * CarmenWrapper::m_reading_function(void*){
	while (true) {
		lock();
		IPC_listen(100);
		unlock();
		usleep(20000);
	}    
	return 0;
}

void CarmenWrapper::shutdown_module(int sig){
  if(sig == SIGINT) {
      carmen_ipc_disconnect();

      fprintf(stderr, "\nDisconnecting (shutdown_module(%d) called).\n",sig);
      exit(0);
  }
}
/*
typedef struct {
  int num_readings;
  float *range;
  char *tooclose;
  double x, y, theta;//position of the laser on the robot
  double odom_x, odom_y, odom_theta; //position of the center of the robot
  double tv, rv;
  double forward_safety_dist, side_safety_dist;
  double turn_axis;
  double timestamp;
  char host[10];
} carmen_robot_laser_message;
*/

carmen_robot_laser_message CarmenWrapper::reading2carmen(const RangeReading& reading){
	carmen_robot_laser_message frontlaser;
	frontlaser.num_readings=reading.size();
	frontlaser.range = new float[frontlaser.num_readings];
	frontlaser.tooclose=0;
	frontlaser.laser_pose.x=frontlaser.robot_pose.x=reading.getPose().x;
	frontlaser.laser_pose.y=frontlaser.robot_pose.y=reading.getPose().y;
	frontlaser.laser_pose.theta=frontlaser.robot_pose.theta=reading.getPose().theta;
	frontlaser.tv=frontlaser.rv=0;
	frontlaser.forward_safety_dist=frontlaser.side_safety_dist=0;
	frontlaser.turn_axis=0;
	frontlaser.timestamp=reading.getTime();
	for (unsigned int i=0; i< reading.size(); i++){
		frontlaser.range[i]=(float)reading[i];
	}
	return frontlaser;
}

carmen_point_t CarmenWrapper::point2carmen (const OrientedPoint& p){
	return (carmen_point_t){p.x,p.y,p.theta};
}

OrientedPoint CarmenWrapper::carmen2point (const carmen_point_t& p){
	return OrientedPoint(p.x, p.y, p.theta);
}


/*
int main (int argc, char** argv) {
	CarmenWrapper::start(argc, argv);
	while(1){
		sleep(2);
		RangeReading reading(0,0);
		while(CarmenWrapper::getReading(reading)){
			cout << "FLASER " <<  reading.size();
			for (int i=0; i<reading.size(); i++)
				cout << " " << reading[i];
			cout << reading.getPose().x << " "
			     << reading.getPose().y << " "
			     << reading.getPose().theta << " 0 cazzo 0" << endl;
		}
		cout << endl;
	}
	return 1;
}
*/