#include <cstdlib>
#include <iostream>
#include <fstream>

#include <list>
#include "gmapping/scanmatcher/icp.h"

using namespace GMapping;
using namespace std;

typedef std::list<PointPair> PointPairList;

PointPairList generateRandomPointPairs(int size, OrientedPoint t, double noise=0.){
	PointPairList ppl;
	double s=sin(t.theta), c=cos(t.theta);
	for (int i=0; i<size; i++){
		Point noiseDraw(noise*(drand48()-.5),noise*(drand48()-.5));
		PointPair pp;
		pp.first.x=100.*(drand48()-.5)+200;
		pp.first.y=10.*(drand48()-.5);
		pp.second.x= c*pp.first.x-s*pp.first.y;
		pp.second.y= s*pp.first.x+c*pp.first.y;
		pp.second=pp.second+t+noiseDraw;
		//cerr << "p1=" << pp.first.x << " " << pp.first.y << endl;
		//cerr << "p2=" << pp.second.x << " " << pp.second.y << endl;
		ppl.push_back(pp);
	}
	return ppl;
}


int main(int argc, const char ** argv){
	while (1){
		OrientedPoint t;
		int size;
		cerr << "Insert size, t.x, t.y, t.theta" << endl;
		cin >> size >> t.x >> t.y >> t.theta;
		PointPairList ppl=generateRandomPointPairs(size, t, 3);
		OrientedPoint tc;
		OrientedPoint ttot(0.,0.,0.);
		bool method=true;
		while(1){
			char buf[10];
			cerr << "iterate?" << endl;
			cin.getline(buf,10);
			if (buf[0]=='n')
				method=false;
			else if (buf[0]=='l')
				method=true;
			else if (buf[0]!=char(0))
				break;
			cout << "plot '-' w l, '-' w p, '-' w p" << endl;
			for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
				cout << it->first.x << " " << it->first.y<< endl;
				cout << it->second.x << " " << it->second.y<< endl;
				cout << endl;
			}
			cout << "e" << endl;
			for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
				cout << it->first.x << " " << it->first.y<< endl;
			}
			cout << "e" << endl;
			for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
				cout << it->second.x << " " << it->second.y<< endl;
			}
			cout << "e" << endl;
			
			double error;
			if (!method){
				cerr << "Nonlinear Optimization" << endl;
				error=icpNonlinearStep(tc,ppl);
			}else {
				cerr << "Linear Optimization" << endl;
				error=icpStep(tc,ppl);
			}
			cerr << "ICP err=" << error << " t.x=" << tc.x << " t.y=" << tc.y << " t.theta=" << tc.theta << endl;
			cerr << "\t" << error << " ttot.x=" << ttot.x << " ttot.y=" << ttot.y << " ttot.theta=" << ttot.theta << endl;
			double s=sin(tc.theta), c=cos(tc.theta);
			for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
				Point p1(c*it->first.x-s*it->first.y+tc.x,
					 s*it->first.x+c*it->first.y+tc.y);
				it->first=p1;
			}
			ttot.x+=tc.x;
			ttot.y+=tc.y;
			ttot.theta+=tc.theta;
			ttot.theta=atan2(sin(ttot.theta), cos(ttot.theta));
		}
	}
	return 0;
}