#include <cstring>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <list>
#include <gmapping/utils/point.h>

#define MAX_LINE_LENGHT (1000000)

using namespace GMapping;
using namespace std;

struct Record{
	unsigned int dim;
	double time;
	virtual ~Record(){}
	virtual void read(istream& is)=0;
	virtual void write(ostream& os){};
};

struct CommentRecord: public Record{
	string text;
	virtual void read(istream& is){
		char buf[MAX_LINE_LENGHT];
		memset(buf,0, MAX_LINE_LENGHT*sizeof(char));
		is.getline(buf, MAX_LINE_LENGHT);
		text=string(buf);
	}
	virtual void write(ostream& os){
		os << "#GFS_COMMENT: " << text << endl;
	}
};

struct PoseRecord: public Record{
	PoseRecord(bool ideal=false){
		truePos=ideal;
	}
	bool truePos;
	OrientedPoint pose;
	void read(istream& is){
		is >> pose.x >> pose.y >> pose.theta;
		time = 0;
		if (is)
			is >> time;
	}
	virtual void write(ostream& os){
		if (truePos) 
			os << "POS-CORR";
		else
			os << "POS ";
		// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
		os << "0 0: ";
		os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta <<  endl;
	}
};

struct NeffRecord: public Record{
	double neff;
	void read(istream& is){
		is >> neff;
	}
	virtual void write(ostream& os){
		os << "NEFF " << neff << endl;
	}
};


struct OdometryRecord: public Record{
	vector<OrientedPoint> poses;
	virtual void read(istream& is){
		is >> dim;
		for (unsigned int i=0; i< dim; i++){
			OrientedPoint p;
			double w;
			is >> p.x;
			is >> p.y;
			is >> p.theta;
			is >> w;
			poses.push_back(p);
		}
		time = 0;
		if (is)
			is >> time;
	}
};


struct ScanMatchRecord: public Record{
	vector<OrientedPoint> poses;
	vector<double> weights;
	virtual void read(istream& is){
		is >> dim;
		for (unsigned int i=0; i< dim; i++){
			OrientedPoint p;
			double w;
			is >> p.x;
			is >> p.y;
			is >> p.theta;
			is >> w;
			poses.push_back(p);
			weights.push_back(w);
		}
	}
};

struct LaserRecord: public Record{
	vector<double> readings;
	OrientedPoint pose;
	virtual void read(istream& is){
		is >> dim;
		for (unsigned int i=0; i< dim; i++){
			double r;
			is >> r;
			readings.push_back(r);
		}
		is >> pose.x;
		is >> pose.y;
		is >> pose.theta;
		time = 0;
		if (is)
			is >> time;
	}
	
	//	dummy, &sec, &usec, &nLas, &nVal, &range ) == EOF) {

	virtual void write(ostream& os){
		os << "POS ";
		// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
		os << "0 0: ";
		os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta <<  endl;
		
		os << "LASER-RANGE ";
		// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
		os << " 0 0 0 " << dim << " 180. : ";
		for (unsigned int i=0; i< dim; i++){
			os <<" "<< readings[i]*100 ;
		}
		os << endl;
	};
};

struct ResampleRecord: public Record{
	vector<unsigned int> indexes;
	virtual void read(istream& is){
		is >> dim;
		for (unsigned int i=0; i< dim; i++){
			unsigned int j;
			is >> j;
			indexes.push_back(j);
		}
	}
};

struct RecordList: public list<Record*>{
	mutable int sampleSize;
	
	istream& read(istream& is){
		while(is){
			char buf[8192];
			is.getline(buf, 8192);
			istringstream lineStream(buf);
			string recordType;
			lineStream >> recordType;
			Record* rec=0;
			if (recordType=="LASER_READING"){
				rec=new LaserRecord;
				cout << "l" << flush;
			}
			if (recordType=="ODO_UPDATE"){
				rec=new OdometryRecord;
				cout << "o" << flush;
			}
			if (recordType=="SM_UPDATE"){
				rec=new ScanMatchRecord;
				cout << "m" << flush;
			}
			if (recordType=="SIMULATOR_POS"){
				rec=new PoseRecord(true);
				cout << "t" << flush;
			}
			if (recordType=="RESAMPLE"){
				rec=new ResampleRecord;
				cout << "r" << flush;
			}
			if (recordType=="NEFF"){
				rec=new NeffRecord;
				cout << "n" << flush;
			}
			if (recordType=="COMMENT"){
				rec=new CommentRecord;
				cout << "c" << flush;
			}
			if (rec){
				rec->read(lineStream);
				push_back(rec);
			}
		}
		return is;
	}
	
	double getLogWeight(unsigned int i) const{
		double weight=0;
		unsigned int currentIndex=i;
		for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
			ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it); 
			if (scanmatch){
				weight+=scanmatch->weights[currentIndex];
			}
			ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it); 
			if (resample){
				currentIndex=resample->indexes[currentIndex];
			}
		}
		return weight;
	}
	unsigned int getBestIdx() const {
		if (empty())
			return 0;
		const ScanMatchRecord* scanmatch=0;
		const_reverse_iterator it=rbegin();
		while(!scanmatch){
			scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
			it++;
		}
		unsigned int dim=scanmatch->dim;
		sampleSize=(int)dim;
		double bestw=-1e200;
		unsigned int best=scanmatch->dim+1;
		for (unsigned i=0; i<dim; i++){
			double w=getLogWeight(i);
			if (w>bestw){
				best=i;
				bestw=w;
			}
		}
		return best;
	}
	
	void printPath(ostream& os, unsigned int i, bool err=false) const{
		unsigned int currentIndex=i;
		OrientedPoint p(0,0,0);
	
		RecordList rl;
		
		//reconstruct a  path
		for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
			const NeffRecord* neff=dynamic_cast<const NeffRecord*>(*it);
			if (neff){
				NeffRecord* n=new NeffRecord(*neff);
				rl.push_front(n);
			}
			const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
			if (scanmatch){
				PoseRecord* pose=new PoseRecord;
				pose->dim=0;
				p=pose->pose=scanmatch->poses[currentIndex];
				rl.push_front(pose);
			}
			const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
			if (odometry){
				PoseRecord* pose=new PoseRecord;
				pose->dim=0;
				p=pose->pose=odometry->poses[currentIndex];
				pose->time=odometry->time;
				rl.push_front(pose);
			}
			const PoseRecord* tpose=dynamic_cast<const PoseRecord*>(*it);
			if (tpose){
				PoseRecord* pose=new PoseRecord(*tpose);
				rl.push_front(pose);
			}
			const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it); 
			if (laser){
				LaserRecord* claser=new LaserRecord(*laser);
				claser->pose=p;
				rl.push_front(claser);
			}
			const CommentRecord* comment=dynamic_cast<const CommentRecord*>(*it); 
			if (comment){
				CommentRecord* ccomment=new CommentRecord(*comment);
				rl.push_front(ccomment);
			}
			const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it); 
			if (resample){
				currentIndex=resample->indexes[currentIndex];
				ResampleRecord* r= new ResampleRecord(*resample);
				rl.push_front(r);
			}
		}
		bool started=false;
		double ox=0, oy=0, rxx=0, rxy=0, ryx=0, ryy=0, rth=0;
		bool computedTransformation=false;
		bool truePosFound=false;
		OrientedPoint truePose(0,0,0);
		OrientedPoint currPose(0,0,0);
		bool tpf=false;
		double neff=0;
		unsigned int count=0;
		for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){
			NeffRecord* neffr=dynamic_cast<NeffRecord*>(*it);
			if (neffr)
				neff=neffr->neff/(double)sampleSize;
			started=started || dynamic_cast<const LaserRecord*>(*it)?true:false;
			if (started && ! truePosFound){
				PoseRecord* tpose=dynamic_cast<PoseRecord*>(*it);
				if (tpose && tpose->truePos){
					truePosFound=true;
					tpf=true;
					truePose=tpose->pose;
					os << "# ";
					(*it)->write(os);
				}
			}
			if (started && truePosFound && ! computedTransformation){
				PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
				if (pos && !pos->truePos){
					OrientedPoint pose=pos->pose;
					rth=truePose.theta-pose.theta;
					double s=sin(rth), c=cos(rth);
					rxx=ryy=c;
					rxy=-s; ryx=s;
					ox=truePose.x-(rxx*pose.x+rxy*pose.y);
					oy=truePose.y-(ryx*pose.x+ryy*pose.y);
					computedTransformation=true;
					os << "# ";
					(*it)->write(os);
					
				}
			}
			ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it);
			if(resample){
				os << "MARK-POS 0 0: " <<currPose.x*100 << " " << currPose.y*100 << " 0 " << count++ << endl;
			} 
			if (computedTransformation){
				PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
				if (pos){
					if (pos->truePos){
						tpf=true;
						truePose=pos->pose;
					} else {
						if (tpf){
							tpf=false;
							OrientedPoint pose=pos->pose;
							double ex, ey, eth=truePose.theta-pose.theta-rth;
							ex=truePose.x-(ox+rxx*pose.x+rxy*pose.y);
							ey=truePose.y-(oy+ryx*pose.x+ryy*pose.y);
							eth=atan2(sin(eth), cos(eth));
							if (! err)
								os << "# ERROR ";
							os << neff << " "
							   << ex << " " << ey << " " << eth 
							   << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl;
						}
					}
				}
				
			}
			PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
			if (pos)
				currPose=pos->pose;
			
			if (! err)
				(*it)->write(os);
			delete *it;
		}
	}
};



int main (int argc, const char * const * argv){
	if (argc<3){
		cout << "usage gfs2rec [-err] <infilename> <outfilename>" << endl;
		return -1;
	}
	bool err=0;
	bool neff=0;
	unsigned int  c=1;
	if (!strcmp(argv[c],"-err")){
		err=true;
		c++;
	}
	if (!strcmp(argv[c],"-neff")){
		neff=true;
		c++;
	}
	ifstream is(argv[c]);
	if (!is){
		cout << "could read file "<< endl;
		return -1;
	}
		c++;
	RecordList rl;
	rl.read(is);
	unsigned int bestidx=rl.getBestIdx();
	cout << endl << "best index = " <<  bestidx<< endl;
	ofstream os(argv[c]);
	if (! os){
		cout << "could write file "<< endl;
		return -1;
	}
	rl.printPath(os,bestidx,err);
	os.close();
	return 0;
}