/****************************************************************************/
/* Summary	: Data file split and decimation routines for BEDS				*/	
/* Filename : decimate.c													*/
/* Author	: Robert Herlien (rah)											*/
/* Project	: BEDS (Benthic Event Detection System)							*/
/* Revision : 1.0															*/
/* Created	: 09/19/2012													*/
/****************************************************************************/
/* Modification History:													*/
/* 19sept2013 rah - created													*/
/* $Log$
 */
/****************************************************************************/

#include <mbariTypes.h>			/* MBARI type definitions				*/
#include <mbariConst.h>			/* MBARI constants						*/
#include <cfxpico.h>			// Persistor PicoDOS Definitions
#include <beds.h>				/* BEDS general definitions				*/
#include <file.h>				/* BEDS file I/O definitions			*/
#include <syslog.h>				/* System logger						*/
#include <fileUtils.h>			/* BEDS file utilities definitions		*/

#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <time.h>
#include <errno.h>

/* We check the validity of an inertial record by checking to make sure */
/* that the quaternion is normalized.  We do it all with integer		*/
/* arithmetic, rather than converting to float.	 We do this by noting	*/
/* that each quaternion component is the integer value of q[n]*(2**14). */
/* Thus the sum of the squares is normalized to 2**28.	We compare this */
/* sum to 2**28, plus or minus	a small value (epsilon).  The epsilon	*/
/* chosen below is the integer value equal to a floating point epsilon of 0.03*/

#define QUAT_UNITY		(0x10000000)			/* 2**28				*/
#define QUAT_EPSILON	16347719L				/* ((1.03**2)-1) * 2**28*/
#define QUAT_MIN		(QUAT_UNITY - QUAT_EPSILON)
#define QUAT_MAX		(QUAT_UNITY + QUAT_EPSILON)


/********************************/
/*		External Data			*/
/********************************/

Extern Nat16	rcdSizes[];

MLocal char		*oFileErr = "Cannot create output file";
MLocal char		*fileIOErr = "File I/O error";


/************************************************************************/
/* Function	   : writeDecimatedDataRec									*/
/* Purpose	   : Write an InertialRcd by averaging given InertialRcds	*/
/* Inputs	   : FILE ptr, InertialRcd array, number of recs in array	*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
MLocal Errno writeDecimatedDataRec(FILE *fp, InertialRcd *dat, int nrecs)
{
	int			i, n, valid;
	Byte		*p;
	Int32		accel[3], accelAccum[3], val;
	Nat32		sum;
	Int16		quat;
	MBool		copiedQuat;
	InertialRcd idata;

	if (nrecs <= 0)
		return(OK);

	LoopModeMemSetBytes(&idata, 0, sizeof(idata));
	copiedQuat = FALSE;
	idata.recType = InertialDataType;
	accelAccum[0] = accelAccum[1] = accelAccum[2] = 0;

	for (n = valid = 0; n < nrecs; n++)
	{
		p = &dat[n].accel;
		for (i = 0; i < 3; i++)
		{
			accel[i] = (Int32)(((unsigned long)p[0] << 16) +
							   ((unsigned long)p[1] << 8) +
							   ((unsigned long)p[2]));
			if (accel[i] & 0x800000)
				accel[i] |= 0xff000000;
			p += 3;
		}

		sum = 0L;
		for (i = 0; i < 4; i++)
		{
			/* Code only works for QUAT_SIZE == 2 */
			quat = (Int16)(((unsigned long)p[0] << 8) +
						   (unsigned long)p[1]);
			p += QUAT_SIZE;
			sum += (long)quat*(long)quat;
		}

		if ((sum >= QUAT_MIN) && (sum <= QUAT_MAX))
		{
			valid++;
			for (i = 0; i < 3; i++)
				accelAccum[i] += accel[i];
			if (!copiedQuat && (n >= nrecs/2))
			{
				LoopModeMemCopyBytes(&idata.quat, &dat[n].quat, 4*QUAT_SIZE);
				copiedQuat = TRUE;
			}
		}
	}

	p = &idata.accel;
	if (valid > 0)
	{
		for (i = 0; i < 3; i++, p+=3)
		{
			val = accelAccum[i] / valid;
			LoopModeMemCopyBytes(p, (Byte *)(&val) + 1, 3);
		}

		if (fwrite(&idata, sizeof(idata), 1, fp) < 1)
			return(ERROR);
	}

	return(OK);

} /* writeDecimatedDataRec() */


/************************************************************************/
/* Function	   : splitAndDecimate										*/
/* Purpose	   : Split and decimate a BEDS data file					*/
/* Inputs	   : File name, num files to split into, decimation factor	*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
Errno splitAndDecimate(char *fname, int nsplit, int ndec)
{
	FILE		*ifp, *ofp;
	int			fnum, nrecs, i;
	MBool		more;
	char		*p;
	FileHdr		ihdr, ohdr;
	time_t		startTime, durationSecs, splitTime;
	RcdUnion	rec;
	char		oname[32];
	static InertialRcd dat[MAX_DECIMATION];

	if ((nsplit < 1) || (ndec < 1))
		return("Parameter error");

	if (nsplit > 100)
		nsplit = 100;
	
	if (ndec > MAX_DECIMATION)
		ndec = MAX_DECIMATION;

	if ((ifp = fopen(fname, "r")) == NULL)
		return("Cannot open file");

	if (!getNextRec(ifp, FileHdrType, &ihdr))
		return(fileIOErr);

	LoopModeMemSetBytes(&rec, 0, sizeof(rec));
	durationSecs = (ihdr.duration + 999)/1000;
	if (nsplit > durationSecs)					/* Because each split file must */
		nsplit = durationSecs;					/* contain at least 1 second	*/
	startTime = ihdr.startTime;

	for (more = TRUE, fnum = 0; more; fnum++)
	{
		splitTime = (durationSecs * (fnum+1) / nsplit) - (durationSecs * fnum / nsplit);
										 /* Distributes remainder among all files*/
		LoopModeMemCopyBytes(&ohdr, &ihdr, sizeof(FileHdr));
		ohdr.dataRate = ihdr.dataRate/ndec;
		ohdr.startTime = startTime;
		ohdr.startMs = fnum ? 0 : ihdr.startMs;

		if (fnum == 0)
			ohdr.duration = 1000*splitTime - ihdr.startMs;
		else if (fnum == nsplit-1)
			ohdr.duration = ihdr.duration - 1000*(startTime - ihdr.startTime) + ihdr.startMs;
		else
			ohdr.duration = 1000*splitTime;

		strncpy(oname, fname, 32);
		if ((p = strchr(oname, '.')) != NULL)
			sprintf(p+2, "%02d", fnum);
		printf("creating file %s\n", oname);
		if ((ofp = fopen(oname, "w")) == NULL)
			return(oFileErr);
		if (fwrite(&ohdr, sizeof(FileHdr), 1, ofp) < 1)
			return(fileIOErr);

		//rec is zeroed before 1st loop, so if we have a second marker, it's from last loop
		if (rec.sec.recType == SecMarkerType)
			fwrite(&rec, sizeof(SecondMarker), 1, ofp);

		nrecs = 0;
		while((more = getAnyRec(ifp, &rec)))
		{
			// If get second marker past end of split time, break to start new
			if ((rec.sec.recType == SecMarkerType) &&
				(rec.sec.rcdTime >= (startTime + splitTime)) &&
				(fnum < nsplit-1))
				break;

			if ((rec.inertial.recType == InertialDataType) && (ndec > 1))
			{
				LoopModeMemCopyBytes(&dat[nrecs], &rec, sizeof(InertialRcd));
				nrecs++;
				if (nrecs >= ndec)
				{
					if (writeDecimatedDataRec(ofp, &dat, nrecs) == ERROR)
					{
						fclose(ofp);
						fclose(ifp);
						return(fileIOErr);
					}
					nrecs = 0;
				}
			}
			else
				if (fwrite(&rec, rcdSizes[rec.sec.recType], 1, ofp) < 1)
				{
					fclose(ofp);
					fclose(ifp);
					return(fileIOErr);
				}
		}

		/* Done with latest split file.	 Write remaining decimated data and close */
		if (writeDecimatedDataRec(ofp, &dat, nrecs) == ERROR)
		{
			fclose(ofp);
			fclose(ifp);
			return(fileIOErr);
		}

		fclose(ofp);

		/* Last rcd should be sec marker, but check just in case */
		/* and remember start time for next split file			*/
		if (rec.sec.recType == SecMarkerType)
			startTime = rec.sec.rcdTime;
	}

	fclose(ifp);
	return(NULL);

} /* splitAndDecimate() */