#include "stdafx.h"
#include <string>
#include <fstream>
#include <iostream>
#include <stdlib.h>
#include <cmath>
#include <iomanip>
#include <vector>
#include <algorithm>

using namespace std;

//Convert degree into radian
inline double D2R(double degree)
{
	return degree/180.0*(4*atan(1.0));
}
//Point
struct Point
{
	int no;
	double time;
	int mode;//GPS state
	double x,y,z;
	double heave,pitch,roll;
	double z_crt;//Attitude corrected z
	double z_crt_flt;//Attitude corrected and filtered z
	void display()//Show the info of point
	{
		cout<<no<<" "<<time<<" "<<mode<<" "<<x<<" "<<y<<" "<<z<<" "<<heave<<" "<<pitch<<" "<<roll<<endl;
		cout<<z_crt<<endl;
	}
};
//Read file names
void ReadFileName(vector<string> &filenames,string str)
{
	string line;
	ifstream file(str);
	if (!file.is_open())
	{
		cout<<"The file doesn't exist!"<<endl;
		system("Pause");
		exit(1);
	}
	while (!file.eof())
	{
		getline(file,line);
		if (!line.empty())
		{
			filenames.push_back(line);
		}
	}
	file.close();
}
//Read data from files
void ReadFromFiles(vector<Point> &points,vector<string> &filenames)
{
	ifstream file;
	string line;
	for (vector<string>::iterator iter=filenames.begin();iter!=filenames.end();++iter)
	{
		file.open(*iter);
		if (!file.is_open())
		{
			cout<<"The file doesn't exist!"<<endl;
			system("Pause");
			exit(1);
		}
		for (int i = 0; i < 4; i++)//Skip useless data
		{
			getline(file,line);
		}
		while (!file.eof())
		{
			Point point;
			getline(file,line);
			if (!line.empty())//Save data splitted by space
			{
				char * cstr = new char [line.length()+1];
				strcpy(cstr, line.c_str());
				char * p = strtok (cstr," ");
				point.no=atoi(p);p = strtok(NULL," ");
				point.time=atof(p);p = strtok(NULL," ");
				point.mode=atoi(p);p = strtok(NULL," ");
				point.x=atof(p);p = strtok(NULL," ");
				point.y=atof(p);p = strtok(NULL," ");
				point.z=atof(p);p = strtok(NULL," ");
				point.heave=D2R(atof(p));p = strtok(NULL," ");
				point.pitch=D2R(atof(p));p = strtok(NULL," ");
				point.roll=D2R(atof(p));
				//Attitude correction
				point.z_crt=point.z-1.2*cos(point.pitch)*cos(point.roll);
				point.z_crt_flt=point.z_crt;
				points.push_back(point);
				delete[] cstr;
			}
		}
		file.close();
	}
}
//Average filter
void AvgFilter(vector<Point> &points)
{
	for (vector<Point>::iterator iter=points.begin()+6;iter!=points.end()-6;++iter)
	{
		double sum=iter->z_crt;
		vector<Point>::iterator iter_front=iter-1;
		vector<Point>::iterator iter_back=iter+1;
		for (; (iter_back-iter_front)<=10; iter_front--,iter_back++)
		{
			sum+=(iter_front->z_crt+iter_back->z_crt);//Sum up data from both sides
		}
		iter->z_crt_flt=sum/11;//Get the average of these data
	}
}
//Median filter
void MedFilter(vector<Point> &points)
{
	for (vector<Point>::iterator iter=points.begin()+6;iter!=points.end()-6;++iter)
	{
		vector<double> val;
		val.push_back(iter->z_crt);
		vector<Point>::iterator iter_front=iter-1;
		vector<Point>::iterator iter_back=iter+1;
		for (; (iter_back-iter_front)<=10; iter_front--,iter_back++)
		{
			val.push_back(iter_back->z_crt);
			val.push_back(iter_front->z_crt);
		}
		sort(val.begin(),val.end());//Arrange data from small to large
		if (val.size()%2==0)
		{
			//If the number of data is even, get the average of the middle 2 data
			iter->z_crt_flt=(val[val.size()/2-1]+val[val.size()/2])/2;
		}
		else
		{
			//Else get the middle data
			iter->z_crt_flt=val[(val.size()-1)/2];
		}
	}
}
//Write the time and filtered z of each GPS point to Result.txt
void WriteToResult(vector<Point> &points,string str)
{
	ofstream out(str);
	out<<"time\tz"<<endl;
	for (vector<Point>::iterator iter=points.begin()+6;iter!=points.end()-6;++iter)
	{
		out<<setiosflags(ios_base::left)<<setw(7)<<iter->time<<" "<<iter->z_crt_flt<<endl;
	}
	out.close();
}
int _tmain(int argc, _TCHAR* argv[])
{
	vector<string> filenames1,filenames4;
	vector<Point> points1,points4,points14;

	ReadFileName(filenames1,"filename1.txt");//Read the name of folder1
	ReadFileName(filenames4,"filename4.txt");//Read the name of folder4

	ReadFromFiles(points1,filenames1);//Write the data into points1
	ReadFromFiles(points4,filenames4);//Write the data into points4

	reverse(points4.begin(),points4.end());//Reverse the GPS data from 11:57 to 14:13

	//Merge point sets
	points14.insert(points14.end(),points1.begin(),points1.end());
	points14.insert(points14.end(),points4.begin(),points4.end());

	//Performing average filtering on point set
	//AvgFilter(points14);
	MedFilter(points14);

	WriteToResult(points14,"result.txt");
	system("Pause");
	return 0;
}