//
//
// The MIT License (MIT)
//
// Copyright (c) 2015  Michael J. Wouters
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
// Credits: the nitty gritty of this is based on code by Peter Fisk and Bruce Warrington
//

#include <stdio.h>
#include <cstdlib>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <cstring>
#include <cmath>

#include <algorithm>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <vector>

#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/split.hpp>

#include "Antenna.h"
#include "Application.h"
#include "Debug.h"
#include "GPS.h"
#include "HexBin.h"
#include "Javad.h"
#include "ReceiverMeasurement.h"
#include "SVMeasurement.h"
#include "Timer.h"

extern ostream *debugStream;
extern Application *app;

// JAVAD types
#define I1 char
#define I2 short
#define I4 int
#define U1 unsigned char
#define U2 unsigned short
#define U4 unsigned int
#define F4 float
#define F8 double

#define MAX_CHANNELS 32

// Receiver message flags
#define AZ_MSG 0x01
#define EL_MSG 0x02
#define F1_MSG 0x04
#define F2_MSG 0x08
#define FC_MSG 0x10
#define R1_r1_1R_1r_MSG 0x20
#define R2_r2_2R_2r_MSG 0x40
#define RC_rc_MSG 0x80
#define RD_MSG 0x100
#define RT_MSG 0x200
#define SI_MSG 0x400
#define SS_MSG 0x800
#define TO_MSG 0x1000
#define YA_MSG 0x2000
#define ZA_MSG 0x4000

Javad::Javad(Antenna *ant,string m):Receiver(ant)
{
  modelName=m;
	manufacturer="Javad";
	swversion="0.1";
	constellations=GNSSSystem::GPS;
	dualFrequency=false;
	codes=GNSSSystem::C1;
	channels=32;
	if (modelName=="HE_GD"){
		dualFrequency=true;
		codes = GNSSSystem::C1 | GNSSSystem::P1 | GNSSSystem::P2;
	}
	else{
		app->logMessage("Unknown receiver model: " + modelName);
		app->logMessage("Assuming generic single frequency receiver");
		modelName="generic";
	}
}

Javad::~Javad()
{
}

bool Javad::readLog(string fname,int mjd,int startTime,int stopTime,int rinexObsInterval)
{
	Timer timer;

	timer.start();

	DBGMSG(debugStream,INFO,"reading " << fname);

	ifstream infile (fname.c_str());
	string line;
	int linecount=0;

	string msgid,currpctime,pctime,msg,gpstime;

	U4 gpsTOD;
	F8 rxTimeOffset;
	F4 sawtooth;

	vector<string> rxid;

	vector<SVMeasurement *> gpsmeas;
	gotIonoData = false;
	gotUTCdata=false;

	U1 uint8buf;
	I1 sint8buf;
	I2 sint16buf;
	I4 sint32buf;
	U4 u4buf;

	unsigned int currMsgs=0,reqdMsgs;
	unsigned int nSats;
	unsigned char trackedSVs[MAX_CHANNELS];
	unsigned char navStatus[MAX_CHANNELS];
	unsigned char elevs[MAX_CHANNELS];
	unsigned char azimuths[MAX_CHANNELS];

	F8 CApr[MAX_CHANNELS];
	unsigned char CAlockFlags[MAX_CHANNELS*2];

	F8 P1pr[MAX_CHANNELS];
	F8 relP1pr[MAX_CHANNELS];
	unsigned char P1lockFlags[MAX_CHANNELS*2];

	F8 P2pr[MAX_CHANNELS];
	F8 relP2pr[MAX_CHANNELS];
	unsigned char P2lockFlags[MAX_CHANNELS*2];

	I2 i2bufarray[MAX_CHANNELS];
	I4 i4bufarray[MAX_CHANNELS];
	F4 f4bufarray[MAX_CHANNELS];
	F8 f8bufarray[MAX_CHANNELS];

	F8 smoothingOffset;
	int rcCnt=0,RCcnt=0;
	int R1cnt=0,r1Cnt=0,m1RCnt=0,m1rCnt=0;
	int R2cnt=0,r2Cnt=0,m2RCnt=0,m2rCnt=0;
	unsigned int errorCount=0,badC1Measurements=0,badP1Measurements=0,badP2Measurements=0,numSVmeasurements=0;
	U2 RDyyyy;
	U1 RDmm,RDdd;

	reqdMsgs = AZ_MSG | EL_MSG | FC_MSG | RC_rc_MSG| RT_MSG | SI_MSG | SS_MSG | TO_MSG | YA_MSG| ZA_MSG;
	if (dualFrequency)
		reqdMsgs |= R1_r1_1R_1r_MSG | R2_r2_2R_2r_MSG | F1_MSG | F2_MSG;

  if (infile.is_open()){
    while ( getline (infile,line) ){
			linecount++;

			if (line.size()==0) continue; // skip empty line
			if ('#' == line.at(0)) continue; // skip comments
			if ('%' == line.at(0)) continue;

			if ('@' == line.at(0)){
				size_t pos;
				if (string::npos != (pos = line.find("RXID",2 )) ){
					rxid.push_back(line.substr(pos+4,line.size()-pos-4));
				}
				continue;
			}
			stringstream sstr(line);

			// Basic check on the format
			if ( (line.size() < 16) || // too short
				(line.at(2) != ' ') || // missing delimiter
				(line.at(5) != ':') || // missing delimiter
				(line.at(8) != ':') ||
				(line.at(11) != ' ')){
				errorCount++;
				continue;
			}
			sstr >> msgid >> currpctime >> msg;
			if (sstr.fail()){
				DBGMSG(debugStream,WARNING," bad data at line " << linecount);
				errorCount++;
				continue;
			}

			int hh,mm,ss;
			if ((3==sscanf(currpctime.c_str(),"%d:%d:%d",&hh,&mm,&ss))){
				int ts = hh*3600+mm*60+ss;
				if (ts < startTime || ts > stopTime)
					continue;
			}

			// FIXME could improve parser by converting the msgid to a hex value?
			// Some messages we just don't want
			if (msgid == "NP"){
				continue;
			}

			// FIXME Valid checksum?
			// If we compute a valid checksum, then checking the message size is a bit paranoid
			//HexToBin(hexdata,count/2,rawdata);
			//sprintf(temp,"%s%03X",command,count/2);
			//for (i=0;i<count/2;i++) temp[i+5] = rawdata[i];
			//if (cs(temp,count/2 + 5 - 1 ) != rawdata[count/2 - 1]) continue;

			//
			// Messages that occur once per second
			//

			//
			// The Receiver Date message starts each second
			//

			if(msgid=="RD"){ // Receiver Date (RD) message

				if ((currMsgs == reqdMsgs) && (rcCnt <= 1) && (RCcnt <= 1)){ // save measurements

					if ((dualFrequency &&
						!((R1cnt<=1) && (r1Cnt<=1) && (m1RCnt<=1) && (m1rCnt<=1) &&
						(R2cnt<=1) && (r2Cnt<=1) && (m2RCnt<=1) && (m2rCnt<=1))))
					{
						DBGMSG(debugStream,INFO,"Too many P1/2 messages");
						pctime=currpctime;
						currMsgs=0;
						rcCnt=RCcnt=0;
						R1cnt=r1Cnt=m1RCnt=m1rCnt=0;
						R2cnt=r2Cnt=m2RCnt=m2rCnt=0;
						continue;
					}

					ReceiverMeasurement *rmeas = new ReceiverMeasurement();
					numSVmeasurements += nSats;

					// For each tracked satellite, get the pseudorange for each code
					for (unsigned int chan=0; chan < nSats; chan++){

						// FIXME ignore GLONASS for the present
						// GPS USID 1..37, GLONASS 38..69,70
						if (trackedSVs[chan] > 37){
							continue;
						}

						if (codes & GNSSSystem::C1){
							bool ok=true;
							// Check that PLLs are locked for each channel before we use the data
							if (CAlockFlags[chan*2] != 83){
								DBGMSG(debugStream,WARNING," C/A unlocked at line " << linecount << "(prn=" << (int) trackedSVs[chan] << ")");
								badC1Measurements++;
								ok=false;
							}

							// Simple sanity check in case pseudoranges or the receiver time offset are wild
							if (((CApr[chan]-rxTimeOffset)<0.05) || ((CApr[chan]-rxTimeOffset)>0.10)){
								DBGMSG(debugStream,WARNING," C/A pseudorange too large at line " << linecount << "(" << CApr[chan]-rxTimeOffset << ")");
								if (ok) badC1Measurements++; // don't count it twice
								ok=false;
							}

							if (ok){
								SVMeasurement *svm = new SVMeasurement(trackedSVs[chan],GNSSSystem::GPS,GNSSSystem::C1,CApr[chan]-rxTimeOffset,rmeas); // pseudorange is corrected for rx offset
								svm->dbuf3 = CApr[chan];
								rmeas->meas.push_back(svm);
							}

						} // if codes & C1

						if (codes & GNSSSystem::P1){
							bool ok=true;
							if (P1lockFlags[chan*2] != 83){
								DBGMSG(debugStream,WARNING," P1 unlocked at line " << linecount << "(prn=" << (int) trackedSVs[chan] << ")");
								badP1Measurements++;
								ok=false;
							}

							// FIXME better sanity check
							ok = ok && !isnan(P1pr[chan]);

							if (ok){
								SVMeasurement *svm = new SVMeasurement(trackedSVs[chan],GNSSSystem::GPS,GNSSSystem::P1,P1pr[chan]-rxTimeOffset,rmeas); // pseudorange is corrected for rx offset
								rmeas->meas.push_back(svm);
							}

						} // if codes & P1

						if (codes & GNSSSystem::P2){
							bool ok = true;
							if (P2lockFlags[chan*2] != 83){
								DBGMSG(debugStream,WARNING," P2 unlocked at line " << linecount << "(prn=" << (int) trackedSVs[chan] << ")");
								badP2Measurements++;
								ok=false;
							}

							// FIXME better sanity check
							ok = ok && !isnan(P2pr[chan]);

							if (ok){
								SVMeasurement *svm = new SVMeasurement(trackedSVs[chan],GNSSSystem::GPS,GNSSSystem::P2,P2pr[chan]-rxTimeOffset,rmeas); // pseudorange is corrected for rx offset
								rmeas->meas.push_back(svm);
							}

						} // if codes & P2

					}

					if (rmeas->meas.size() > 0){ // FIXME check other codes
						int pchh,pcmm,pcss;
						if ((3==sscanf(pctime.c_str(),"%d:%d:%d",&pchh,&pcmm,&pcss))){
							rmeas->pchh=pchh;
							rmeas->pcmm=pcmm;
							rmeas->pcss=pcss;

							// One more check
							if ((gpsTOD/1000.0+rxTimeOffset) > -0.1 && (gpsTOD/1000.0+rxTimeOffset < 86400.1)){
								int igpsTOD = gpsTOD/1000;
								int hh = (int) (igpsTOD/3600);
								int mm = (int) (igpsTOD - hh*3600)/60;

								rmeas->tmGPS.tm_sec= igpsTOD - hh*3600 - mm*60;
								rmeas->tmGPS.tm_min= mm;
								rmeas->tmGPS.tm_hour=hh ;
								rmeas->tmGPS.tm_mday=RDdd;
								rmeas->tmGPS.tm_mon=RDmm-1;
								rmeas->tmGPS.tm_year=RDyyyy-1900;
								rmeas->tmGPS.tm_isdst=0;
								mktime(&(rmeas->tmGPS)); // this sets wday (note: TZ=UTC enforced in Main.cpp) so DST stays correct
								rmeas->gpstow = 86400*rmeas->tmGPS.tm_wday+igpsTOD;
								rmeas->tmfracs = rxTimeOffset;

								// The time offset can be negative so have to account for rollovers
// 								time_t ttGPS = (time_t)(mktime(&(rmeas->tmGPS)) + rxTimeOffset);
// 								struct tm *tmGPS = gmtime(&ttGPS);
// 								rmeas->tmGPS=*tmGPS;
// 								rmeas->tmfracs = rxTimeOffset;
// 								if (rmeas->tmfracs < 0)
// 									rmeas->tmfracs += 1.0;

								// YA and TO messages can roll over at different times - handle this case
								// Typically, the difference between the smoothed and receiver time offsets is <= 1 ns
								if ((smoothingOffset - rxTimeOffset)> 5e-4) smoothingOffset-=1e-3;
								if ((smoothingOffset - rxTimeOffset)<-5e-4) smoothingOffset+=1e-3;

								rmeas->sawtooth=sawtooth-(smoothingOffset-rxTimeOffset); // added to the counter measurement
																																				 // the term (moothingOffset-rxTimeOffset) is typically zero
								rmeas->timeOffset = rxTimeOffset; // just used for diagnostics

								// All OK
								measurements.push_back(rmeas);
								DBGMSG(debugStream,TRACE,rmeas->meas.size() << " measurements at "  << (int) gpsTOD << " "
									<< hh << ":" << mm << ":" << (int) rmeas->tmGPS.tm_sec << " (GPS), "
									<< pchh << ":" << pcmm << ":" << pcss << " (PC)");
							}
							else{
								errorCount++;
								DBGMSG(debugStream,WARNING,"GPS TOD out of range at " << pchh << ":" << pcmm << ":" << pcss << " (PC)");
							}
						}
						else{
							errorCount++;
							DBGMSG(debugStream,WARNING,"Unreadable PC time " << currpctime);
						}
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING,"No useable GPS measurements at " << currpctime);
						delete rmeas;
					}
				}

				if (msg.size() == 6*2){
					HexToBin((char *) msg.c_str(),sizeof(U2),(unsigned char *) &RDyyyy);
					HexToBin((char *) msg.substr(2*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &RDmm);
					HexToBin((char *) msg.substr(3*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &RDdd);
					HexToBin((char *) msg.substr(4*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &uint8buf);
					DBGMSG(debugStream,TRACE," RD " << (int) RDyyyy << ":" << (int) RDmm << ":" << (int) RDdd << " rx ref time=" << (int) uint8buf);
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," RD msg wrong size at line " << linecount);
				}

				pctime=currpctime;
				currMsgs=0;
				rcCnt=RCcnt=0;
				R1cnt=r1Cnt=m1RCnt=m1rCnt=0;
				R2cnt=r2Cnt=m2RCnt=m2rCnt=0;
				continue;
			}

			if (msgid == "~~"){
				if (msg.size() == 5*2 ){
					HexToBin((char *) msg.substr(0,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &gpsTOD);
					currMsgs |= RT_MSG;
					DBGMSG(debugStream,TRACE," RT " << (int) gpsTOD);
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," bad data at line " << linecount);
				}
				continue;
			}

			if(msgid=="SI"){ // Satellite Indices (SI) message
				if (currMsgs & SI_MSG){
					currMsgs=0; // unexpected SI message
					rcCnt=RCcnt=0;
					R1cnt=r1Cnt=m1RCnt=m1rCnt=0;
					R2cnt=r2Cnt=m2RCnt=m2rCnt=0;
					continue;
				}
				// Can't check the message size!
				currMsgs |= SI_MSG;
				nSats=(msg.size() - 2) / 2;
				HexToBin((char *) msg.c_str(),nSats,trackedSVs);
				continue;
			}

			if(msgid=="TO"){ // Reference Time to Receiver Time Offset (TO) message
				if (msg.size() == 9*2){
					HexToBin((char *) msg.c_str(),sizeof(F8),(unsigned char *) &rxTimeOffset);
					// Discard outliers
					if ((fabs(rxTimeOffset)>0.001) || (fabs(rxTimeOffset)<1E-10)){
						badC1Measurements++;
						DBGMSG(debugStream,WARNING," TO outlier at line " << linecount << ":" << rxTimeOffset);
					}
					else{
						currMsgs |= TO_MSG;
						DBGMSG(debugStream,TRACE," TO " << rxTimeOffset *1.0E9 << " ns");
					}
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," TO msg wrong size at line " << linecount);
				}
				continue;
			}

			if(msgid=="YA"){ // smoothing offset (YA) message - assuming we are using pps A
				if (msg.size() == 10*2){
					HexToBin((char *) msg.c_str(),sizeof(F8),(unsigned char *) &smoothingOffset);
					// Discard outliers. YA is occasionally reported as zero following a tracking glitch.
					if ((fabs(smoothingOffset)>0.001) || (smoothingOffset==0)){
						badC1Measurements++;
						DBGMSG(debugStream,WARNING," YA outlier at line " << linecount << ":" << smoothingOffset);
					}
					else{
						currMsgs |= YA_MSG;
						DBGMSG(debugStream,TRACE," YA " << smoothingOffset *1.0E9 << " ns");
					}
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," YA msg wrong size at line " << linecount << ":" << msg.size());
				}
				continue;
			}

			if(msgid=="ZA"){ // PPS offset (ZA) message - assuming we are using pps A
				if (msg.size() == 5*2){
					HexToBin((char *) msg.c_str(),sizeof(F4),(unsigned char *) &sawtooth); // units are ns
					// Discard outliers
					if (fabs(sawtooth)> 50.0){
						badC1Measurements++;
						DBGMSG(debugStream,WARNING," ZA outlier at line " << linecount << ":" << sawtooth);
					}
					else{
						DBGMSG(debugStream,TRACE," ZA " << sawtooth << " ns");
						sawtooth *= 1.0E-9;
						currMsgs |= ZA_MSG;
					}
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," ZA msg wrong size at line " << linecount);
				}
				continue;
			}

			// Need the SI message to parse the following messages
			if (!(currMsgs & SI_MSG)) continue;

			if(msgid == "SS"){ //  Navigation Status (SS) message
				unsigned int ssnSats = (msg.size() - 4) / 2;
				if (ssnSats == nSats){
					HexToBin((char *) msg.c_str(),nSats,navStatus);
					currMsgs |= SS_MSG;
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," SS msg wrong size at line " << linecount << ":" << ssnSats << " " << nSats);
				}
				continue;
			}

			if(msgid == "EL"){ //  Satellite Elevations (EL) message
				unsigned int elnSats = (msg.size() - 2) / 2;
				if (elnSats == nSats){
					HexToBin((char *) msg.c_str(),nSats,elevs);
					currMsgs |= EL_MSG;
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," EL msg wrong size at line " << linecount);
				}
				continue;
			}

			if(msgid == "AZ"){ //  Satellite Azimuths (AZ) message
				unsigned int aznSats = (msg.size() - 2) / 2;
				if (aznSats == nSats){
					HexToBin((char *) msg.c_str(),nSats,azimuths);
					currMsgs |= AZ_MSG;
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," AZ msg wrong size at line " << linecount);
				}
				continue;
			}

			// L1C measurements
			if(msgid == "rc"){ // Delta C/A Pseudoranges (rc) message
				if (RCcnt) continue; // full pseudoranges take precedence
				unsigned int rcnSats = (msg.size() - 2) / 8;
				if (rcnSats == nSats){
					HexToBin((char *) msg.c_str(),nSats*sizeof(I4),(unsigned char *) (i4bufarray));
					for (unsigned int i=0;i<nSats;i++)
						CApr[i] = (double)(i4bufarray[i])*1e-11 + 0.075;
					currMsgs |= RC_rc_MSG;
					rcCnt++;
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," rc msg wrong size at line " << linecount);
				}
				continue;
			}

			if(msgid == "RC"){ // Full C/A Pseudoranges (RC) message
				unsigned int RCnSats = (msg.size() - 2) / 16;
				if (RCnSats == nSats){
					HexToBin((char *) msg.c_str(),nSats*sizeof(F8),(unsigned char *) (f8bufarray));
					for (unsigned int i=0;i<nSats;i++)
						CApr[i] = (double) f8bufarray[i];
					currMsgs |= RC_rc_MSG;
					RCcnt++;
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," RC msg wrong size at line " << linecount);
				}
				continue;
			}

			if(msgid == "FC"){ // F/A Signal Lock Flags (FC) message
				unsigned int FCnSats = (msg.size() - 2) / 4;
				if (FCnSats == nSats){
					HexToBin((char *) msg.c_str(),nSats*sizeof(U2),(unsigned char *) (CAlockFlags));
					currMsgs |= FC_MSG;
				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING," FC msg wrong size at line " << linecount);
				}
				continue;
			}

			// P1 & P2 messages
			if (dualFrequency){

				// Four pseudorange messages for each signal
				if(msgid == "R1"){ // Full P1 pseudorange (R1) message
					unsigned int msgSats = (msg.size() - 2) / (2*sizeof(F8));
					if (msgSats == nSats){
						HexToBin((char *) msg.c_str(),nSats*sizeof(F8),(unsigned char *) (f8bufarray));
						for (unsigned int i=0;i<nSats;i++)
							P1pr[i] = (double) f8bufarray[i];
						currMsgs |= R1_r1_1R_1r_MSG;
						R1cnt++;
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING," R1 msg wrong size at line " << linecount);
					}
					continue;
				}

				if(msgid == "r1"){ // Short P1 pseudoranges (r1) message
					if (R1cnt) continue; // full pseudoranges take precedence
					unsigned int msgSats = (msg.size() - 2) / (2*sizeof(I4));
					if (msgSats == nSats){
						HexToBin((char *) msg.c_str(),nSats*sizeof(I4),(unsigned char *) (i4bufarray));
						for (unsigned int i=0;i<nSats;i++)
							P1pr[i] = (double)(i4bufarray[i])*1e-11 + 0.075;
						currMsgs |= R1_r1_1R_1r_MSG;
						r1Cnt++;
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING," r1 msg wrong size at line " << linecount);
					}
					continue;
				}

				if(msgid == "1R"){ // Relative P1 pseudoranges (1R) message
					unsigned int msgSats = (msg.size() - 2) / (2*sizeof(F4));
					if (msgSats == nSats){
						HexToBin((char *) msg.c_str(),nSats*sizeof(F4),(unsigned char *) (f4bufarray));
						for (unsigned int i=0;i<nSats;i++)
							relP1pr[i] = (double) f4bufarray[i];
						currMsgs |= R1_r1_1R_1r_MSG;
						m1RCnt++;
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING," 1R msg wrong size at line " << linecount);
					}
					continue;
				}

				if(msgid == "1r"){ // Short relative P1 pseudoranges (1r) message
					if (m1RCnt) continue; // full relative pseudoranges take precedence
					unsigned int msgSats = (msg.size() - 2) / (2*sizeof(I2));
					if (msgSats == nSats){
						HexToBin((char *) msg.c_str(),nSats*sizeof(I2),(unsigned char *) (i2bufarray));
						for (unsigned int i=0;i<nSats;i++)
							relP1pr[i] = (double)(i2bufarray[i])*1e-11 + 2.0e-7;
						currMsgs |= R1_r1_1R_1r_MSG;
						m1rCnt++;
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING," 1r msg wrong size at line " << linecount);
					}
					continue;
				}

				if(msgid == "R2"){ // Full P2 pseudorange (R2) message
					unsigned int msgSats = (msg.size() - 2) / (2*sizeof(F8));
					if (msgSats == nSats){
						HexToBin((char *) msg.c_str(),nSats*sizeof(F8),(unsigned char *) (f8bufarray));
						for (unsigned int i=0;i<nSats;i++)
							P2pr[i] = (double) f8bufarray[i];
						currMsgs |= R2_r2_2R_2r_MSG;
						R2cnt++;
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING," R2 msg wrong size at line " << linecount);
					}
					continue;
				}

				if(msgid == "r2"){ // Short P2 pseudoranges (r2) message
					if (R2cnt) continue; // full pseudoranges take precedence
					unsigned int msgSats = (msg.size() - 2) / (2*sizeof(I4));
					if (msgSats == nSats){
						HexToBin((char *) msg.c_str(),nSats*sizeof(I4),(unsigned char *) (i4bufarray));
						for (unsigned int i=0;i<nSats;i++)
							P2pr[i] = (double)(i4bufarray[i])*1e-11 + 0.075;
						currMsgs |= R2_r2_2R_2r_MSG;
						r2Cnt++;
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING," r2 msg wrong size at line " << linecount);
					}
					continue;
				}

				if(msgid == "2R"){ // Relative P2 pseudoranges (2R) message
					unsigned int msgSats = (msg.size() - 2) / (2*sizeof(F4));
					if (msgSats == nSats){
						HexToBin((char *) msg.c_str(),nSats*sizeof(F4),(unsigned char *) (f4bufarray));
						for (unsigned int i=0;i<nSats;i++)
							relP2pr[i] = (double) f4bufarray[i];
						currMsgs |= R2_r2_2R_2r_MSG;
						m2RCnt++;
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING," 2R msg wrong size at line " << linecount);
					}
					continue;
				}

				if(msgid == "2r"){ // Short relative P2 pseudoranges (2r) message
					if (m2RCnt) continue; // full delta pseudoranges take precedence
					unsigned int msgSats = (msg.size() - 2) / (2*sizeof(I2));
					if (msgSats == nSats){
						HexToBin((char *) msg.c_str(),nSats*sizeof(I2),(unsigned char *) (i2bufarray));
						for (unsigned int i=0;i<nSats;i++)
							relP2pr[i] = (double)(i2bufarray[i])*1e-11 + 2.0e-7;
						currMsgs |= R2_r2_2R_2r_MSG;
						m2rCnt++;
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING," 2r msg wrong size at line " << linecount);
					}
					continue;
				}

				if(msgid == "F1"){ // P1 Lock Flags (F1) message
					unsigned int msgSats = (msg.size() - 2) /(2*sizeof(U2));
					if (msgSats == nSats){
						HexToBin((char *) msg.c_str(),nSats*sizeof(U2),(unsigned char *) (P1lockFlags));
						currMsgs |= F1_MSG;
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING," F1 msg wrong size at line " << linecount);
					}
					continue;
				}

				if(msgid == "F2"){ // P2 Lock Flags (F2) message
					unsigned int msgSats = (msg.size() - 2) /(2*sizeof(U2));
					if (msgSats == nSats){
						HexToBin((char *) msg.c_str(),nSats*sizeof(U2),(unsigned char *) (P2lockFlags));
						currMsgs |= F2_MSG;
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING," F2 msg wrong size at line " << linecount);
					}
					continue;
				}

			} // if dualFrequency

			//
			// Intermittent  messages - parse last
			//

			if (!gotIonoData){
				if (msgid=="IO"){
					if (msg.size()==39*2){
						HexToBin((char *) msg.substr(6*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.a0));
						HexToBin((char *) msg.substr(10*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.a1));
						HexToBin((char *) msg.substr(14*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.a2));
						HexToBin((char *) msg.substr(18*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.a3));
						HexToBin((char *) msg.substr(22*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.B0));
						HexToBin((char *) msg.substr(26*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.B1));
						HexToBin((char *) msg.substr(30*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.B2));
						HexToBin((char *) msg.substr(34*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.ionoData.B3));
						gotIonoData=true;
						DBGMSG(debugStream,TRACE,"ionosphere parameters: a0=" << gps.ionoData.a0);
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING,"Bad I0 message size");
					}
					continue;
				}
			}

			if (!gotUTCdata){
				if (msgid=="UO"){
					if (msg.size()==24*2){
						HexToBin((char *) msg.substr(0*2,2*sizeof(F8)).c_str(),sizeof(F8),(unsigned char *) &(gps.UTCdata.A0));
						HexToBin((char *) msg.substr(8*2,2*sizeof(F4)).c_str(),sizeof(F4),(unsigned char *) &(gps.UTCdata.A1));
						HexToBin((char *) msg.substr(12*2,2*sizeof(U4)).c_str(),sizeof(U4),(unsigned char *) &u4buf);
						gps.UTCdata.t_ot=u4buf;
						HexToBin((char *) msg.substr(16*2,2*sizeof(U2)).c_str(),sizeof(U2),(unsigned char *) &(gps.UTCdata.WN_t));
						HexToBin((char *) msg.substr(18*2,2*sizeof(I1)).c_str(),sizeof(I1),(unsigned char *) &(sint8buf));
						gps.UTCdata.dtlS=sint8buf;
						HexToBin((char *) msg.substr(19*2,2*sizeof(U1)).c_str(),sizeof(U1),(unsigned char *) &uint8buf);
						gps.UTCdata.DN=uint8buf;
						HexToBin((char *) msg.substr(20*2,2*sizeof(U2)).c_str(),sizeof(U2),(unsigned char *) &(gps.UTCdata.WN_LSF));
						HexToBin((char *) msg.substr(22*2,2*sizeof(I1)).c_str(),sizeof(I1),(unsigned char *) &sint8buf);
						gps.UTCdata.dt_LSF=sint8buf;
						DBGMSG(debugStream,TRACE,"UTC parameters: dtLS=" << gps.UTCdata.dtlS << ",dt_LSF=" << gps.UTCdata.dt_LSF);
						gotUTCdata = gps.currentLeapSeconds(mjd,&leapsecs);
					}
					else{
						errorCount++;
						DBGMSG(debugStream,WARNING,"Bad U0 message size");
					}
					continue;
				}
			}

			if(msgid == "GE"){  // GPS ephemeris
				if (msg.length() == 123*2){
					GPS::EphemerisData *ed = new GPS::EphemerisData;
 					HexToBin((char *) msg.substr(0*2,2*sizeof(UINT8)).c_str(), sizeof(UINT8),  (unsigned char *) &(ed->SVN));
					HexToBin((char *) msg.substr(1*2,2*sizeof(UINT32)).c_str(),sizeof(UINT32), (unsigned char *) &(u4buf));
 					ed->t_ephem=u4buf;

					HexToBin((char *) msg.substr(6*2,2*sizeof(SINT16)).c_str(),sizeof(SINT16), (unsigned char *) &(sint16buf));
						ed-> IODC=sint16buf;
					HexToBin((char *) msg.substr(8*2,2*sizeof(SINT32)).c_str(),sizeof(SINT32), (unsigned char *) &(sint32buf));
					ed->t_OC=sint32buf;
					HexToBin((char *) msg.substr(12*2,2*sizeof(SINT8)).c_str(), sizeof(SINT8),  (unsigned char *) &(sint8buf));
					ed->SV_accuracy_raw = sint8buf;
					ed->SV_accuracy = GPS::URA[ed->SV_accuracy_raw];
					HexToBin((char *) msg.substr(13*2,2*sizeof(UINT8)).c_str(), sizeof(UINT8),  (unsigned char *) &(ed->SV_health));
					HexToBin((char *) msg.substr(14*2,2*sizeof(SINT16)).c_str(),sizeof(SINT16), (unsigned char *) &sint16buf);
					ed->week_number=(UINT16) sint16buf;
					HexToBin((char *) msg.substr(16*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->t_GD));

					HexToBin((char *) msg.substr(20*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->a_f2));
					HexToBin((char *) msg.substr(24*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->a_f1));
					HexToBin((char *) msg.substr(28*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->a_f0));
					HexToBin((char *) msg.substr(32*2,2*sizeof(SINT32)).c_str(),sizeof(SINT32), (unsigned char *) &sint32buf);
					ed->t_oe=sint32buf;
					HexToBin((char *) msg.substr(36*2,2*sizeof(SINT16)).c_str(), sizeof(SINT16),  (unsigned char *) &sint16buf);
					ed->IODE=(UINT8) sint16buf; // WARNING! Truncated
					HexToBin((char *) msg.substr(38*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->sqrtA));

					HexToBin((char *) msg.substr(46*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->e));
					HexToBin((char *) msg.substr(54*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->M_0));
					ed->M_0 *= M_PI;
					HexToBin((char *) msg.substr(62*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->OMEGA_0));
					ed->OMEGA_0 *= M_PI;
					HexToBin((char *) msg.substr(70*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->i_0));
					ed->i_0 *= M_PI;
					HexToBin((char *) msg.substr(78*2,2*sizeof(DOUBLE)).c_str(),sizeof(DOUBLE), (unsigned char *) &(ed->OMEGA));
					ed->OMEGA *= M_PI;
					HexToBin((char *) msg.substr(86*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->delta_N));
					ed->delta_N *=M_PI;
					HexToBin((char *) msg.substr(90*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->OMEGADOT));
					ed->OMEGADOT *= M_PI;
					HexToBin((char *) msg.substr(94*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->IDOT));
					ed->IDOT *= M_PI;
					HexToBin((char *) msg.substr(98*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_rc));
					HexToBin((char *) msg.substr(102*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_rs));
					HexToBin((char *) msg.substr(106*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_uc));
					HexToBin((char *) msg.substr(110*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_us));
					HexToBin((char *) msg.substr(114*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_ic));
					HexToBin((char *) msg.substr(118*2,2*sizeof(F4)).c_str(),sizeof(F4), (unsigned char *) &(ed->C_is));

					int pchh,pcmm,pcss;
					if ((3==sscanf(pctime.c_str(),"%d:%d:%d",&pchh,&pcmm,&pcss)))
						ed->tLogged = pchh*3600 + pcmm*60 + pcss;
					else
						ed->tLogged = -1;

					DBGMSG(debugStream,TRACE,"Ephemeris: SVN="<< (unsigned int) ed->SVN << ",toe="<< ed->t_oe << ",IODE=" << (int) ed->IODE);
					gps.addEphemeris(ed);

				}
				else{
					errorCount++;
					DBGMSG(debugStream,WARNING,"Bad GE message size");
				}
				continue;
			}

		}
	}
	else{
		app->logMessage("Unable to open "+fname);
		return false;
	}

	infile.close();

	if (!gotIonoData){
		app->logMessage("failed to find ionosphere parameters - no IO messages");
		return false;
	}

	if (!gotUTCdata){
		app->logMessage("failed to find UTC parameters - no U0 messages");
		return false;
	}

	// Post load cleanups

	interpolateMeasurements();

	// Calculate UTC time of measurements, now that the number of leap seconds is known
	for (unsigned int i=0;i<measurements.size();i++){
		time_t tUTC = mktime(&(measurements[i]->tmGPS));
		tUTC -= leapsecs;
		struct tm *tmUTC = gmtime(&tUTC);
		measurements[i]->tmUTC=*tmUTC;
	}

	// Extract the receiver id
	if (rxid.size() !=0) {
		if ((rxid.size() % 4 == 0)){
			int idx = (rxid.size() / 4) -1;
			string rxinfo = rxid.at(idx) + rxid.at(idx+1) + rxid.at(idx+2) + rxid.at(idx+3);
			rxinfo.erase(std::remove(rxinfo.begin(),rxinfo.end(),'{'), rxinfo.end());
			rxinfo.erase(std::remove(rxinfo.begin(),rxinfo.end(),'}'), rxinfo.end());
			rxinfo.erase(std::remove(rxinfo.begin(),rxinfo.end(),'\"'), rxinfo.end());
			std::vector<std::string> vals;
			boost::split(vals, rxinfo,boost::is_any_of(","), boost::token_compress_on);
			boost::algorithm::trim_left(vals.at(0));
			serialNumber = vals.at(0);
			modelName = vals.at(1);
			boost::algorithm::trim_left(vals.at(4));
			swversion = vals.at(4)+" "+vals.at(5)+" "+vals.at(6);
			DBGMSG(debugStream,INFO,"rx s/n " << vals.at(0) << ",model " << modelName << ",sw " << swversion);

		}
		else{
			DBGMSG(debugStream,INFO, "Unable to find receiver ID");
		}
	}

	timer.stop();

	DBGMSG(debugStream,INFO,"done: read " << linecount << " lines");
	DBGMSG(debugStream,INFO,measurements.size() << " measurements read");
	DBGMSG(debugStream,INFO,gps.ephemeris.size() << " GPS ephemeris entries read");
	DBGMSG(debugStream,INFO,errorCount << " errors in input file");
	if (codes & GNSSSystem::C1){
		DBGMSG(debugStream,INFO,badC1Measurements  << " SV C1 measurements rejected");
	}
	if (codes & GNSSSystem::P1){
		DBGMSG(debugStream,INFO,badP1Measurements  << " SV P1 measurements rejected");
	}
	if (codes & GNSSSystem::P2){
		DBGMSG(debugStream,INFO,badP2Measurements  << " SV P2 measurements rejected");
	}
	DBGMSG(debugStream,INFO,"elapsed time: " << timer.elapsedTime(Timer::SECS) << " s");
	return true;

}


//=============================================================================================================
/**
 * @file     fiff_proj.cpp
 * @author   Lorenz Esch <lesch@mgh.harvard.edu>;
 *           Matti Hamalainen <msh@nmr.mgh.harvard.edu>;
 *           Christoph Dinh <chdinh@nmr.mgh.harvard.edu>
 * @version  dev
 * @date     July, 2012
 *
 * @section  LICENSE
 *
 * Copyright (C) 2012, Lorenz Esch, Matti Hamalainen, Christoph Dinh. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
 *       the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
 *       to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * @brief    Definition of the FiffProj Class.
 *
 */

//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "fiff_proj.h"
#include <stdio.h>
#include <utils/mnemath.h>

//=============================================================================================================
// EIGEN INCLUDES
//=============================================================================================================

#include <Eigen/SVD>

//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace UTILSLIB;
using namespace FIFFLIB;
using namespace Eigen;

//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

FiffProj::FiffProj()
: kind(-1)
, active(false)
, desc("")
, data(new FiffNamedMatrix)
{

}

//=============================================================================================================

FiffProj::FiffProj(const FiffProj& p_FiffProj)
: kind(p_FiffProj.kind)
, active(p_FiffProj.active)
, desc(p_FiffProj.desc)
, data(p_FiffProj.data)
{

}

//=============================================================================================================

FiffProj::FiffProj( fiff_int_t p_kind, bool p_active, QString p_desc, FiffNamedMatrix& p_data)
: kind(p_kind)
, active(p_active)
, desc(p_desc)
, data(new FiffNamedMatrix(p_data))
{

}

//=============================================================================================================

FiffProj::~FiffProj()
{

}

//=============================================================================================================

void FiffProj::activate_projs(QList<FiffProj> &p_qListFiffProj)
{
    // Activate the projection items
    QList<FiffProj>::Iterator it;
    for(it = p_qListFiffProj.begin(); it != p_qListFiffProj.end(); ++it)
        it->active = true;

    printf("\t%d projection items activated.\n", p_qListFiffProj.size());
}

//=============================================================================================================

fiff_int_t FiffProj::make_projector(const QList<FiffProj>& projs, const QStringList& ch_names, MatrixXd& proj, const QStringList& bads, MatrixXd& U)
{
    fiff_int_t nchan = ch_names.size();
    if (nchan == 0)
    {
        printf("No channel names specified\n");//ToDo throw here
        return 0;
    }

//    if(proj)
//        delete proj;
    proj = MatrixXd::Identity(nchan,nchan);
    fiff_int_t nproj = 0;
    U = MatrixXd();

    //
    //   Check trivial cases first
    //
    if (projs.size() == 0)
        return 0;

    fiff_int_t nvec    = 0;
    fiff_int_t k, l;
    for (k = 0; k < projs.size(); ++k)
    {
        if (projs[k].active)
        {
            ++nproj;
            nvec += projs[k].data->nrow;
        }
    }

    if (nproj == 0) {
        printf("FiffProj::make_projector - No projectors nproj=0\n");
        return 0;
    }

    if (nvec <= 0) {
        printf("FiffProj::make_projector - No rows in projector matrices found nvec<=0\n");
        return 0;
    }

    //
    //   Pick the appropriate entries
    //
    MatrixXd vecs = MatrixXd::Zero(nchan,nvec);
    nvec = 0;
    fiff_int_t nonzero = 0;
    qint32 p, c, i, j, v;
    double onesize;
    bool isBad = false;
    RowVectorXi sel(nchan);
    RowVectorXi vecSel(nchan);
    sel.setConstant(-1);
    vecSel.setConstant(-1);
    for (k = 0; k < projs.size(); ++k)
    {
        if (projs[k].active)
        {
            FiffProj one = projs[k];

            QMap<QString, int> uniqueMap;
            for(l = 0; l < one.data->col_names.size(); ++l)
                uniqueMap[one.data->col_names[l] ] = 0;

            if (one.data->col_names.size() != uniqueMap.keys().size())
            {
                printf("Channel name list in projection item %d contains duplicate items",k);
                return 0;
            }

            //
            // Get the two selection vectors to pick correct elements from
            // the projection vectors omitting bad channels
            //
            sel.resize(nchan);
            vecSel.resize(nchan);
            sel.setConstant(-1);
            vecSel.setConstant(-1);
            p = 0;
            for (c = 0; c < nchan; ++c)
            {
                for (i = 0; i < one.data->col_names.size(); ++i)
                {
                    if (QString::compare(ch_names.at(c),one.data->col_names[i]) == 0)
                    {
                        isBad = false;
                        for (j = 0; j < bads.size(); ++j)
                        {
                            if (QString::compare(ch_names.at(c),bads.at(j)) == 0)
                            {
                                isBad = true;
                            }
                        }

                        if (!isBad && sel[p] != c)
                        {
                            sel[p] = c;
                            vecSel[p] = i;
                            ++p;
                        }

                    }
                }
            }
            sel.conservativeResize(p);
            vecSel.conservativeResize(p);
            //
            // If there is something to pick, pickit
            //
            if (sel.cols() > 0)
                for (v = 0; v < one.data->nrow; ++v)
                    for (i = 0; i < p; ++i)
                        vecs(sel[i],nvec+v) = one.data->data(v,vecSel[i]);

            //
            //   Rescale for more straightforward detection of small singular values
            //
            for (v = 0; v < one.data->nrow; ++v)
            {
                onesize = sqrt((vecs.col(nvec+v).transpose()*vecs.col(nvec+v))(0,0));
                if (onesize > 0.0)
                {
                    vecs.col(nvec+v) = vecs.col(nvec+v)/onesize;
                    ++nonzero;
                }
            }
            nvec += one.data->nrow;
        }
    }
    //
    //   Check whether all of the vectors are exactly zero
    //
    if (nonzero == 0)
        return 0;

    //
    //   Reorthogonalize the vectors
    //
    JacobiSVD<MatrixXd> svd(vecs.block(0,0,vecs.rows(),nvec), ComputeFullU);
    //Sort singular values and singular vectors
    VectorXd S = svd.singularValues();
    MatrixXd t_U = svd.matrixU();
    MNEMath::sort<double>(S, t_U);

    //
    //   Throw away the linearly dependent guys
    //
    nproj = 0;
    for(k = 0; k < S.size(); ++k)
        if (S[k]/S[0] > 1e-2)
            ++nproj;

    U = t_U.block(0, 0, t_U.rows(), nproj);

    //
    //   Here is the celebrated result
    //
    proj -= U*U.transpose();

    return nproj;
}


/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Mitsuba 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. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <mitsuba/mitsuba.h>
#include <mitsuba/core/util.h>
#include <mitsuba/core/random.h>
#include <mitsuba/core/quad.h>
#include <mitsuba/core/sse.h>
#include <mitsuba/core/frame.h>
#include <boost/bind.hpp>
#include <stdarg.h>
#include <iomanip>
#include <errno.h>

#if defined(__OSX__)
#include <sys/sysctl.h>
#include <mach/mach.h>
#elif defined(__WINDOWS__)
#include <windows.h>
#include <direct.h>
#include <psapi.h>
#else
#include <malloc.h>
#endif

#if defined(__WINDOWS__)
# include <windows.h>
# include <winsock2.h>
# include <ws2tcpip.h>
#else
# include <sys/types.h>
# include <sys/socket.h>
# include <netdb.h>
# include <fenv.h>
#endif

// SSE is not enabled in general when using double precision, however it is
// required in OS X for FP exception handling
#if defined(__OSX__) && !defined(MTS_SSE)
#include <xmmintrin.h>
#undef enable_fpexcept_sse
#undef query_fpexcept_sse
#undef disable_fpexcept_sse

namespace {
inline void enable_fpexcept_sse() {
	_MM_SET_EXCEPTION_MASK(_MM_GET_EXCEPTION_MASK() &
		~(_MM_MASK_INVALID | _MM_MASK_DIV_ZERO));
}
inline unsigned int query_fpexcept_sse() {
	return (~_MM_GET_EXCEPTION_MASK() &
		(_MM_MASK_INVALID | _MM_MASK_DIV_ZERO));
}
inline void disable_fpexcept_sse() {
	_MM_SET_EXCEPTION_MASK(_MM_GET_EXCEPTION_MASK() |
		_MM_MASK_INVALID | _MM_MASK_DIV_ZERO);
}
} // namespace

#endif

MTS_NAMESPACE_BEGIN

// -----------------------------------------------------------------------
//  General utility functions
// -----------------------------------------------------------------------

std::vector<std::string> tokenize(const std::string &string, const std::string &delim) {
	std::string::size_type lastPos = string.find_first_not_of(delim, 0);
	std::string::size_type pos = string.find_first_of(delim, lastPos);
	std::vector<std::string> tokens;

	while (std::string::npos != pos || std::string::npos != lastPos) {
		tokens.push_back(string.substr(lastPos, pos - lastPos));
		lastPos = string.find_first_not_of(delim, pos);
		pos = string.find_first_of(delim, lastPos);
	}

	return tokens;
}

std::string trim(const std::string& str) {
	std::string::size_type
		start = str.find_first_not_of(" \t\r\n"),
		end = str.find_last_not_of(" \t\r\n");

	return str.substr(start == std::string::npos ? 0 : start,
			end == std::string::npos ? str.length() - 1 : end - start + 1);
}

std::string indent(const std::string &string, int amount) {
	/* This could probably be done faster (is not
	   really speed-critical though) */
	std::istringstream iss(string);
	std::ostringstream oss;
	std::string str;
	bool firstLine = true;
	while (!iss.eof()) {
		std::getline(iss, str);
		if (!firstLine) {
			for (int i=0; i<amount; ++i)
				oss << "  ";
		}
		oss << str;
		if (!iss.eof())
			oss << endl;
		firstLine = false;
	}
	return oss.str();
}

void * __restrict allocAligned(size_t size) {
#if defined(__WINDOWS__)
	return _aligned_malloc(size, L1_CACHE_LINE_SIZE);
#elif defined(__OSX__)
	/* OSX malloc already returns 16-byte aligned data suitable
	   for AltiVec and SSE computations */
	return malloc(size);
#else
	return memalign(L1_CACHE_LINE_SIZE, size);
#endif
}

void freeAligned(void *ptr) {
#if defined(__WINDOWS__)
	_aligned_free(ptr);
#else
	free(ptr);
#endif
}

static int __cached_core_count = 0;

int getCoreCount() {
	// assumes atomic word size memory access
	if (__cached_core_count)
		return __cached_core_count;

#if defined(__WINDOWS__)
	SYSTEM_INFO sys_info;
	GetSystemInfo(&sys_info);
	__cached_core_count = sys_info.dwNumberOfProcessors;
	return sys_info.dwNumberOfProcessors;
#elif defined(__OSX__)
	int nprocs;
	size_t nprocsSize = sizeof(int);
	if (sysctlbyname("hw.activecpu", &nprocs, &nprocsSize, NULL, 0))
		SLog(EError, "Could not detect the number of processors!");
	__cached_core_count = nprocs;
	return nprocs;
#else
	/* Determine the number of present cores */
	int nCores = sysconf(_SC_NPROCESSORS_CONF);

	/* Don't try to query CPU affinity if running inside Valgrind */
	if (getenv("VALGRIND_OPTS") == NULL) {
		/* Some of the cores may not be available to the user
		   (e.g. on certain cluster nodes) -- determine the number
		   of actual available cores here. */
		int nLogicalCores = nCores;
		size_t size = 0;
		cpu_set_t *cpuset = NULL;
		int retval = 0;

		/* The kernel may expect a larger cpu_set_t than would
		   be warranted by the physical core count. Keep querying
		   with increasingly larger buffers if the
		   pthread_getaffinity_np operation fails */
		for (int i = 0; i<6; ++i) {
			size = CPU_ALLOC_SIZE(nLogicalCores);
			cpuset = CPU_ALLOC(nLogicalCores);
			if (!cpuset) {
				SLog(EWarn, "getCoreCount(): could not allocate cpu_set_t");
				goto done;
			}
			CPU_ZERO_S(size, cpuset);

			int retval = pthread_getaffinity_np(pthread_self(), size, cpuset);
			if (retval == 0)
				break;
			CPU_FREE(cpuset);
			nLogicalCores *= 2;
		}

		if (retval) {
			SLog(EWarn, "getCoreCount(): pthread_getaffinity_np(): could "
				"not read thread affinity map: %s", strerror(retval));
			goto done;
		}

		int availableCores = 0;
		for (int i=0; i<nLogicalCores; ++i)
			availableCores += CPU_ISSET_S(i, size, cpuset) ? 1 : 0;
		nCores = availableCores;
		CPU_FREE(cpuset);
	}

done:
	__cached_core_count = nCores;
	return nCores;
#endif
}

size_t getTotalSystemMemory() {
#if defined(__WINDOWS__)
	MEMORYSTATUSEX status;
	status.dwLength = sizeof(status);
	GlobalMemoryStatusEx(&status);
	return (size_t) status.ullTotalPhys;
#elif defined(__OSX__)
	int mib[2] = { CTL_HW, HW_MEMSIZE };
	uint64_t size;
	size_t len = sizeof(size);
	if (sysctl(mib, 2, &size, &len, NULL, 0) < 0)
		return 0;
	return (size_t) size;
#else
	size_t pages = sysconf(_SC_PHYS_PAGES);
	size_t page_size = sysconf(_SC_PAGE_SIZE);
	return pages * page_size;
#endif
}

size_t getPrivateMemoryUsage() {
#if defined(__WINDOWS__)
	PROCESS_MEMORY_COUNTERS_EX pmc;
	GetProcessMemoryInfo(GetCurrentProcess(), (PROCESS_MEMORY_COUNTERS *) &pmc, sizeof(pmc));
	return (size_t) pmc.PrivateUsage; /* Process-private memory usage (RAM + swap) */
#elif defined(__OSX__)
	struct task_basic_info_64 t_info;
	mach_msg_type_number_t t_info_count = TASK_BASIC_INFO_64_COUNT;

	if (task_info(mach_task_self(), TASK_BASIC_INFO_64,
			(task_info_t)&t_info, &t_info_count) != KERN_SUCCESS)
		return 0;

	return (size_t) t_info.resident_size; /* Not exactly what we want -- oh well.. */
#else
	FILE* file = fopen("/proc/self/status", "r");
	if (!file)
		return 0;

	char buffer[128];
	size_t result = 0;
	while (fgets(buffer, sizeof(buffer), file) != NULL) {
		if (strncmp(buffer, "VmRSS:", 6) != 0 && /* Non-swapped physical memory specific to this process */
		    strncmp(buffer, "VmSwap:", 7) != 0)  /* Swapped memory specific to this process */
			continue;

		char *line = buffer;
		while (*line < '0' || *line > '9')
			++line;
		line[strlen(line)-3] = '\0';
		result += (size_t) atoi(line) * 1024;
	}

	fclose(file);
	return result;
#endif
}

#if defined(__WINDOWS__)
std::string lastErrorText() {
	DWORD errCode = GetLastError();
	char *errorText = NULL;
	if (!FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER
		| FORMAT_MESSAGE_FROM_SYSTEM
		| FORMAT_MESSAGE_IGNORE_INSERTS,
		NULL,
		errCode,
		MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
		(LPTSTR) &errorText,
		0,
		NULL)) {
		return "Internal error while looking up an error code";
	}
	std::string result(errorText);
	LocalFree(errorText);
	return result;
}
#endif

bool enableFPExceptions() {
	bool exceptionsWereEnabled = false;
#if defined(__WINDOWS__)
	_clearfp();
	uint32_t cw = _controlfp(0, 0);
	exceptionsWereEnabled = ~cw & (_EM_INVALID | _EM_ZERODIVIDE | _EM_OVERFLOW);
	cw &= ~(_EM_INVALID | _EM_ZERODIVIDE | _EM_OVERFLOW);
	_controlfp(cw, _MCW_EM);
#elif defined(__OSX__)
	exceptionsWereEnabled = query_fpexcept_sse() != 0;
#else
	exceptionsWereEnabled =
		fegetexcept() & (FE_INVALID|FE_DIVBYZERO|FE_OVERFLOW);
	feenableexcept(FE_INVALID|FE_DIVBYZERO|FE_OVERFLOW);
#endif
	enable_fpexcept_sse();
	return exceptionsWereEnabled;
}

bool disableFPExceptions() {
	bool exceptionsWereEnabled = false;
#if defined(__WINDOWS__)
	_clearfp();
	uint32_t cw = _controlfp(0, 0);
	exceptionsWereEnabled = ~cw & (_EM_INVALID | _EM_ZERODIVIDE | _EM_OVERFLOW);
	cw |= _EM_INVALID | _EM_ZERODIVIDE | _EM_OVERFLOW;
	_controlfp(cw, _MCW_EM);
#elif defined(__OSX__)
	exceptionsWereEnabled = query_fpexcept_sse() != 0;
#else
	exceptionsWereEnabled =
		fegetexcept() & (FE_INVALID|FE_DIVBYZERO|FE_OVERFLOW);
	fedisableexcept(FE_INVALID|FE_DIVBYZERO|FE_OVERFLOW);
#endif
	disable_fpexcept_sse();
	return exceptionsWereEnabled;
}

void restoreFPExceptions(bool oldState) {
	bool currentState;
#if defined(__WINDOWS__)
	uint32_t cw = _controlfp(0, 0);
	currentState = ~cw & (_EM_INVALID | _EM_ZERODIVIDE | _EM_OVERFLOW);
#elif defined(__OSX__)
	currentState = query_fpexcept_sse() != 0;
#else
	currentState = fegetexcept() & (FE_INVALID|FE_DIVBYZERO|FE_OVERFLOW);
#endif
	if (oldState != currentState) {
		if (oldState)
			enableFPExceptions();
		else
			disableFPExceptions();
	}
}

std::string getHostName() {
	char hostName[128];
	if (gethostname(hostName, sizeof(hostName)) != 0)
#if defined(__WINDOWS__)
		SLog(EError, "Could not retrieve the computer's host name: %s!",
			lastErrorText().c_str());
#else
		SLog(EError, "Could not retrieve the computer's host name : %s!",
			strerror(errno));
#endif
	return hostName;
}

std::string getFQDN() {
	struct addrinfo *addrInfo = NULL, hints;
	memset(&hints, 0, sizeof(addrinfo));
	// Only look for IPv4 addresses -- perhaps revisit this later
	hints.ai_family = AF_INET; // AF_UNSPEC
	hints.ai_socktype = SOCK_STREAM;
	hints.ai_protocol = IPPROTO_TCP;

	int retVal = getaddrinfo(getHostName().c_str(), NULL, &hints, &addrInfo);
	if (addrInfo == NULL || retVal != 0) {
		SLog(EWarn, "Could not retrieve the computer's fully "
			"qualified domain name: could not resolve host address \"%s\"!",
			getHostName().c_str());
		return getHostName();
	}

	char fqdn[NI_MAXHOST];
	retVal = getnameinfo(addrInfo->ai_addr, sizeof(struct sockaddr_in),
		fqdn, NI_MAXHOST, NULL, 0, 0);
	if (retVal != 0) {
		freeaddrinfo(addrInfo);
#if defined(__WINDOWS__)
		SLog(EWarn, "Could not retrieve the computer's fully "
			"qualified domain name: error %i!", WSAGetLastError());
#else
		SLog(EWarn, "Could not retrieve the computer's fully "
			"qualified domain name: error %i!", gai_strerror(retVal));
#endif
		return getHostName();
	}

	freeaddrinfo(addrInfo);

	return fqdn;
}

std::string formatString(const char *fmt, ...) {
	char tmp[512];
	va_list iterator;

#if defined(__WINDOWS__)
	va_start(iterator, fmt);
	size_t size = _vscprintf(fmt, iterator) + 1;

	if (size >= sizeof(tmp)) {
		char *dest = new char[size];
		vsnprintf_s(dest, size, size-1, fmt, iterator);
		va_end(iterator);
		std::string result(dest);
		delete[] dest;
		return result;
	}

	vsnprintf_s(tmp, size, size-1, fmt, iterator);
	va_end(iterator);
#else
	va_start(iterator, fmt);
	size_t size = vsnprintf(tmp, sizeof(tmp), fmt, iterator);
	va_end(iterator);

	if (size >= sizeof(tmp)) {
		/* Overflow! -- dynamically allocate memory */
		char *dest = new char[size+1];
		va_start(iterator, fmt);
		vsnprintf(dest, size+1, fmt, iterator);
		va_end(iterator);

		std::string result(dest);
		delete[] dest;
		return result;
	}
#endif

	return std::string(tmp);
}

// -----------------------------------------------------------------------
//  Numerical utility functions
// -----------------------------------------------------------------------

bool solveQuadratic(Float a, Float b, Float c, Float &x0, Float &x1) {
	/* Linear case */
	if (a == 0) {
		if (b != 0) {
			x0 = x1 = -c / b;
			return true;
		}
		return false;
	}

	Float discrim = b*b - 4.0f*a*c;

	/* Leave if there is no solution */
	if (discrim < 0)
		return false;

	Float temp, sqrtDiscrim = std::sqrt(discrim);

	/* Numerically stable version of (-b (+/-) sqrtDiscrim) / (2 * a)
	 *
	 * Based on the observation that one solution is always
	 * accurate while the other is not. Finds the solution of
	 * greater magnitude which does not suffer from loss of
	 * precision and then uses the identity x1 * x2 = c / a
	 */
	if (b < 0)
		temp = -0.5f * (b - sqrtDiscrim);
	else
		temp = -0.5f * (b + sqrtDiscrim);

	x0 = temp / a;
	x1 = c / temp;

	/* Return the results so that x0 < x1 */
	if (x0 > x1)
		std::swap(x0, x1);

	return true;
}

bool solveQuadraticDouble(double a, double b, double c, double &x0, double &x1) {
	/* Linear case */
	if (a == 0) {
		if (b != 0) {
			x0 = x1 = -c / b;
			return true;
		}
		return false;
	}

	double discrim = b*b - 4.0f*a*c;

	/* Leave if there is no solution */
	if (discrim < 0)
		return false;

	double temp, sqrtDiscrim = std::sqrt(discrim);

	/* Numerically stable version of (-b (+/-) sqrtDiscrim) / (2 * a)
	 *
	 * Based on the observation that one solution is always
	 * accurate while the other is not. Finds the solution of
	 * greater magnitude which does not suffer from loss of
	 * precision and then uses the identity x1 * x2 = c / a
	 */
	if (b < 0)
		temp = -0.5f * (b - sqrtDiscrim);
	else
		temp = -0.5f * (b + sqrtDiscrim);

	x0 = temp / a;
	x1 = c / temp;

	/* Return the results so that x0 < x1 */
	if (x0 > x1)
		std::swap(x0, x1);

	return true;
}

bool solveLinearSystem2x2(const Float a[2][2], const Float b[2], Float x[2]) {
	Float det = a[0][0] * a[1][1] - a[0][1] * a[1][0];

	if (std::abs(det) <= RCPOVERFLOW)
		return false;

	Float inverse = (Float) 1.0f / det;

	x[0] = (a[1][1] * b[0] - a[0][1] * b[1]) * inverse;
	x[1] = (a[0][0] * b[1] - a[1][0] * b[0]) * inverse;

	return true;
}

void stratifiedSample1D(Random *random, Float *dest, int count, bool jitter) {
	Float invCount = 1.0f / count;

	for (int i=0; i<count; i++) {
		Float offset = jitter ? random->nextFloat() : 0.5f;
		*dest++ = (i + offset) * invCount;
	}
}

void stratifiedSample2D(Random *random, Point2 *dest, int countX, int countY, bool jitter) {
	Float invCountX = 1.0f / countX;
	Float invCountY = 1.0f / countY;

	for (int x=0; x<countX; x++) {
		for (int y=0; y<countY; y++) {
			Float offsetX = jitter ? random->nextFloat() : 0.5f;
			Float offsetY = jitter ? random->nextFloat() : 0.5f;
			*dest++ = Point2(
				(x + offsetX) * invCountX,
				(y + offsetY) * invCountY
			);
		}
	}
}

void latinHypercube(Random *random, Float *dest, size_t nSamples, size_t nDim) {
	Float delta = 1 / (Float) nSamples;
	for (size_t i = 0; i < nSamples; ++i)
		for (size_t j = 0; j < nDim; ++j)
			dest[nDim * i + j] = (i + random->nextFloat()) * delta;
	for (size_t i = 0; i < nDim; ++i) {
		for (size_t j = 0; j < nSamples; ++j) {
			size_t other = random->nextSize(nSamples);
			std::swap(dest[nDim * j + i], dest[nDim * other + i]);
		}
	}
}

Vector sphericalDirection(Float theta, Float phi) {
	Float sinTheta, cosTheta, sinPhi, cosPhi;

	math::sincos(theta, &sinTheta, &cosTheta);
	math::sincos(phi, &sinPhi, &cosPhi);

	return Vector(
		sinTheta * cosPhi,
		sinTheta * sinPhi,
		cosTheta
	);
}

void coordinateSystem(const Vector &a, Vector &b, Vector &c) {
	if (std::abs(a.x) > std::abs(a.y)) {
		Float invLen = 1.0f / std::sqrt(a.x * a.x + a.z * a.z);
		c = Vector(a.z * invLen, 0.0f, -a.x * invLen);
	} else {
		Float invLen = 1.0f / std::sqrt(a.y * a.y + a.z * a.z);
		c = Vector(0.0f, a.z * invLen, -a.y * invLen);
	}
	b = cross(c, a);
}

void computeShadingFrame(const Vector &n, const Vector &dpdu, Frame &frame) {
	frame.n = n;
	frame.s = normalize(dpdu - frame.n
		* dot(frame.n, dpdu));
	frame.t = cross(frame.n, frame.s);
}

void computeShadingFrameDerivative(const Vector &n, const Vector &dpdu, const Vector &dndu, const Vector &dndv, Frame &du, Frame &dv) {
	Vector s = dpdu - n * dot(n, dpdu);
	Float invLen_s = 1.0f / s.length();
	s *= invLen_s;

	du.s = invLen_s * (-dndu * dot(n, dpdu) - n * dot(dndu, dpdu));
	dv.s = invLen_s * (-dndv * dot(n, dpdu) - n * dot(dndv, dpdu));

	du.s -= s * dot(du.s, s);
	dv.s -= s * dot(dv.s, s);

	du.t = cross(dndu, s) + cross(n, du.s);
	dv.t = cross(dndv, s) + cross(n, dv.s);

	du.n = dndu;
	dv.n = dndv;
}

Point2 toSphericalCoordinates(const Vector &v) {
	Point2 result(
		std::acos(v.z),
		std::atan2(v.y, v.x)
	);
	if (result.y < 0)
		result.y += 2*M_PI;
	return result;
}

Float fresnelDielectric(Float cosThetaI, Float cosThetaT, Float eta) {
	if (EXPECT_NOT_TAKEN(eta == 1))
		return 0.0f;

	Float Rs = (cosThetaI - eta * cosThetaT)
			 / (cosThetaI + eta * cosThetaT);
	Float Rp = (eta * cosThetaI - cosThetaT)
			 / (eta * cosThetaI + cosThetaT);

	/* No polarization -- return the unpolarized reflectance */
	return 0.5f * (Rs * Rs + Rp * Rp);
}

Float fresnelDielectricExt(Float cosThetaI_, Float &cosThetaT_, Float eta) {
	if (EXPECT_NOT_TAKEN(eta == 1)) {
		cosThetaT_ = -cosThetaI_;
		return 0.0f;
	}

	/* Using Snell's law, calculate the squared sine of the
	   angle between the normal and the transmitted ray */
	Float scale = (cosThetaI_ > 0) ? 1/eta : eta,
	      cosThetaTSqr = 1 - (1-cosThetaI_*cosThetaI_) * (scale*scale);

	/* Check for total internal reflection */
	if (cosThetaTSqr <= 0.0f) {
		cosThetaT_ = 0.0f;
		return 1.0f;
	}

	/* Find the absolute cosines of the incident/transmitted rays */
	Float cosThetaI = std::abs(cosThetaI_);
	Float cosThetaT = std::sqrt(cosThetaTSqr);

	Float Rs = (cosThetaI - eta * cosThetaT)
			 / (cosThetaI + eta * cosThetaT);
	Float Rp = (eta * cosThetaI - cosThetaT)
			 / (eta * cosThetaI + cosThetaT);

	cosThetaT_ = (cosThetaI_ > 0) ? -cosThetaT : cosThetaT;

	/* No polarization -- return the unpolarized reflectance */
	return 0.5f * (Rs * Rs + Rp * Rp);
}

Float fresnelConductorApprox(Float cosThetaI, Float eta, Float k) {
	Float cosThetaI2 = cosThetaI*cosThetaI;

	Float tmp = (eta*eta + k*k) * cosThetaI2;

	Float Rp2 = (tmp - (eta * (2 * cosThetaI)) + 1)
	          / (tmp + (eta * (2 * cosThetaI)) + 1);

	Float tmpF = eta*eta + k*k;

	Float Rs2 = (tmpF - (eta * (2 * cosThetaI)) + cosThetaI2) /
	            (tmpF + (eta * (2 * cosThetaI)) + cosThetaI2);

	return 0.5f * (Rp2 + Rs2);
}

Spectrum fresnelConductorApprox(Float cosThetaI, const Spectrum &eta, const Spectrum &k) {
	Float cosThetaI2 = cosThetaI*cosThetaI;

	Spectrum tmp = (eta*eta + k*k) * cosThetaI2;

	Spectrum Rp2 = (tmp - (eta * (2 * cosThetaI)) + Spectrum(1.0f))
	             / (tmp + (eta * (2 * cosThetaI)) + Spectrum(1.0f));

	Spectrum tmpF = eta*eta + k*k;

	Spectrum Rs2 = (tmpF - (eta * (2 * cosThetaI)) + Spectrum(cosThetaI2)) /
	               (tmpF + (eta * (2 * cosThetaI)) + Spectrum(cosThetaI2));

	return 0.5f * (Rp2 + Rs2);
}

Float fresnelConductorExact(Float cosThetaI, Float eta, Float k) {
	/* Modified from "Optics" by K.D. Moeller, University Science Books, 1988 */

	Float cosThetaI2 = cosThetaI*cosThetaI,
	      sinThetaI2 = 1-cosThetaI2,
		  sinThetaI4 = sinThetaI2*sinThetaI2;

	Float temp1 = eta*eta - k*k - sinThetaI2,
	      a2pb2 = math::safe_sqrt(temp1*temp1 + 4*k*k*eta*eta),
	      a     = math::safe_sqrt(0.5f * (a2pb2 + temp1));

	Float term1 = a2pb2 + cosThetaI2,
	      term2 = 2*a*cosThetaI;

	Float Rs2 = (term1 - term2) / (term1 + term2);

	Float term3 = a2pb2*cosThetaI2 + sinThetaI4,
	      term4 = term2*sinThetaI2;

	Float Rp2 = Rs2 * (term3 - term4) / (term3 + term4);

	return 0.5f * (Rp2 + Rs2);
}

Spectrum fresnelConductorExact(Float cosThetaI, const Spectrum &eta, const Spectrum &k) {
	/* Modified from "Optics" by K.D. Moeller, University Science Books, 1988 */

	Float cosThetaI2 = cosThetaI*cosThetaI,
	      sinThetaI2 = 1-cosThetaI2,
		  sinThetaI4 = sinThetaI2*sinThetaI2;

	Spectrum temp1 = eta*eta - k*k - Spectrum(sinThetaI2),
	         a2pb2 = (temp1*temp1 + k*k*eta*eta*4).safe_sqrt(),
	         a     = ((a2pb2 + temp1) * 0.5f).safe_sqrt();

	Spectrum term1 = a2pb2 + Spectrum(cosThetaI2),
	         term2 = a*(2*cosThetaI);

	Spectrum Rs2 = (term1 - term2) / (term1 + term2);

	Spectrum term3 = a2pb2*cosThetaI2 + Spectrum(sinThetaI4),
	         term4 = term2*sinThetaI2;

	Spectrum Rp2 = Rs2 * (term3 - term4) / (term3 + term4);

	return 0.5f * (Rp2 + Rs2);
}

Vector reflect(const Vector &wi, const Normal &n) {
	return 2 * dot(wi, n) * Vector(n) - wi;
}

Vector refract(const Vector &wi, const Normal &n, Float eta, Float cosThetaT) {
	if (cosThetaT < 0)
		eta = 1 / eta;

	return n * (dot(wi, n) * eta + cosThetaT) - wi * eta;
}

Vector refract(const Vector &wi, const Normal &n, Float eta) {
	if (EXPECT_NOT_TAKEN(eta == 1))
		return -wi;

	Float cosThetaI = dot(wi, n);
	if (cosThetaI > 0)
		eta = 1 / eta;

	/* Using Snell's law, calculate the squared sine of the
	   angle between the normal and the transmitted ray */
	Float cosThetaTSqr = 1 - (1-cosThetaI*cosThetaI) * (eta*eta);

	/* Check for total internal reflection */
	if (cosThetaTSqr <= 0.0f)
		return Vector(0.0f);

	return n * (cosThetaI * eta - math::signum(cosThetaI)
		* std::sqrt(cosThetaTSqr)) - wi * eta;
}

Vector refract(const Vector &wi, const Normal &n, Float eta, Float &cosThetaT, Float &F) {
	Float cosThetaI = dot(wi, n);
	F = fresnelDielectricExt(cosThetaI, cosThetaT, eta);

	if (F == 1.0f) /* Total internal reflection */
		return Vector(0.0f);

	if (cosThetaT < 0)
		eta = 1 / eta;

	return n * (eta * cosThetaI + cosThetaT) - wi * eta;
}

namespace {
	/// Integrand used by fresnelDiffuseReflectance
	inline Float fresnelDiffuseIntegrand(Float eta, Float xi) {
		return fresnelDielectricExt(std::sqrt(xi), eta);
	}
};

Float fresnelDiffuseReflectance(Float eta, bool fast) {
	if (fast) {
		/* Fast mode: the following code approximates the
		 * diffuse Frensel reflectance for the eta<1 and
		 * eta>1 cases. An evalution of the accuracy led
		 * to the following scheme, which cherry-picks
		 * fits from two papers where they are best.
		 */
		if (eta < 1) {
			/* Fit by Egan and Hilgeman (1973). Works
			   reasonably well for "normal" IOR values (<2).

			   Max rel. error in 1.0 - 1.5 : 0.1%
			   Max rel. error in 1.5 - 2   : 0.6%
			   Max rel. error in 2.0 - 5   : 9.5%
			*/
			return -1.4399f * (eta * eta)
				  + 0.7099f * eta
				  + 0.6681f
				  + 0.0636f / eta;
		} else {
			/* Fit by d'Eon and Irving (2011)
			 *
			 * Maintains a good accuracy even for
			 * unrealistic IOR values.
			 *
			 * Max rel. error in 1.0 - 2.0   : 0.1%
			 * Max rel. error in 2.0 - 10.0  : 0.2%
			 */
			Float invEta = 1.0f / eta,
				  invEta2 = invEta*invEta,
				  invEta3 = invEta2*invEta,
				  invEta4 = invEta3*invEta,
				  invEta5 = invEta4*invEta;

			return 0.919317f - 3.4793f * invEta
				 + 6.75335f * invEta2
				 - 7.80989f * invEta3
				 + 4.98554f * invEta4
				 - 1.36881f * invEta5;
		}
	} else {
		GaussLobattoIntegrator quad(1024, 0, 1e-5f);
		return quad.integrate(
			boost::bind(&fresnelDiffuseIntegrand, eta, _1), 0, 1);
	}

	return 0.0f;
}

std::string timeString(Float time, bool precise) {
	if (std::isnan(time) || std::isinf(time))
		return "inf";

	char suffix = 's';
	if (time > 60) {
		time /= 60; suffix = 'm';
		if (time > 60) {
			time /= 60; suffix = 'h';
			if (time > 12) {
				time /= 12; suffix = 'd';
			}
		}
	}

	std::ostringstream os;
	os << std::setprecision(precise ? 4 : 1)
	   << std::fixed << time << suffix;

	return os.str();
}

std::string memString(size_t size, bool precise) {
	Float value = (Float) size;
	const char *suffixes[] = {
		"B", "KiB", "MiB", "GiB", "TiB", "PiB"
	};
	int suffix = 0;
	while (suffix < 5 && value > 1024.0f) {
		value /= 1024.0f; ++suffix;
	}

	std::ostringstream os;
	os << std::setprecision(suffix == 0 ? 0 : (precise ? 4 : 1))
	   << std::fixed << value << " " << suffixes[suffix];

	return os.str();
}

MTS_NAMESPACE_END