//calfunc02.cpp. 
/* 
 Modified 6 Dec 2004. 
   Changed open_input_ff and open_output_ff.


Modified 25 May 2004 to set column descriptor units to "nT"
*/
/*
 Modified to interpolate on offsets;  
 Changed functions calibrate, sub_off_cal, sub_off_SC_cal;
 Also changed calfunc02.h.
 -JW, 19 Jan 2004
*/
//Modified version of CalFunc01.cpp, Joyce, 5 June 2001.
//Functions changed are open_input_ff, open_output_ff,
//	put_status, calibrate, read_cal.

//include statements modified by jw 9/2/99

#include <string.h>
#include <iostream.h>
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <io.h>

#ifndef CAL_MAG
#include "cal_mag.h"
#endif

#ifndef CALFUNC02
#include "calfunc02.h"
#endif

extern "C" {
#ifndef FF_IGPP
#include "..\ff_user\time_igpp.h"
#include "..\ff_user\ff_igpp.h"
#endif
}

#define CAS_COORD_ID  3  
//This constant is used by put_status. It denotes the body-fixed
// Cassini spacecraft coordinate system.  -jw, June 2001.

//------------------------------------------------------------------
	FF_ID * open_input_ff (char * ffname)  {
/*
	2 Dec 2004. If file open fails and standard input is console,
	user is asked whether to try again to enter input filename.
*/

//Opens input flatfile header and data files
//Returns toplevel flatfile pointer.
//Now returns instead of exiting if file open fails. -jw, june 2001.

		extern EPOCH epoch58;

		FF_ID *id;
		FF_O_PARAM  param;

		param.status = FF_READ | FF_EXIST;
		param.ncols = 0;
		param.recl = 0;
		param.nbufs = 1;
		param.epoch = epoch58;

		id = ff_open (ffname, &param);

		int tryagain = _isatty(_fileno(stdin));
		int ntries = 0;

		while (id == NULL && tryagain && ntries < 5)  {
			cout << endl << "Cannot open " << ffname << endl;
			cout << "Enter 1 to try again, 0 to exit: ";
			cin >> tryagain;
			if (tryagain) {
				cout << "Enter Input Flatfile name: ";
				cin.get();
				cin.getline (ffname, 255);
				id = ff_open (ffname, &param);
				ntries ++;
			} /* end if */
		} /* end while */

		return (id);
	}  /* End open_input_ff() */

//-------------------------------------------------------------------
	FF_ID * open_output_ff (char * ffname, int ncols, int recl)  {

//Opens output flatfile header and data files
//Returns toplevel flatfile pointer.
//Now returns instead of exiting if file open fails. -jw, june 2001.

		extern EPOCH epoch58;
		FF_ID *id;
		FF_O_PARAM  param;

		param.status = FF_CREATE | FF_WRITE | FF_EXIST;
		param.ncols = ncols;
		param.recl = recl;
		param.nbufs = 1;
		param.epoch = epoch58;

		id = ff_open (ffname, &param);

		int tryagain = _isatty(_fileno(stdin));
		int ntries = 0;
		while (id == NULL && tryagain && ntries < 7)  {
			cout << endl << "Cannot open Output " << ffname << endl;
			cout << "Enter 1 to try again, 0 to exit: ";
			cin >> tryagain;
			if (tryagain) {
				cout << "Enter Output Flatfile name: ";
				cin.get();
				cin.getline (ffname, 255);
				id = ff_open (ffname, &param);
				ntries ++;
			} /* end if */
		} /* end while */


		return (id);
	}  /* End open_output_ff() */

//-------------------------------------------------------------------
	long get_nrows (FF_ID *id) {
	return (id ->ffd->nrows);
	}
//-------------------------------------------------------------------
	int get_ncols (FF_ID *id) {
	return (id ->ffd->ncols);
	}
//-------------------------------------------------------------------
	int get_recl (FF_ID *id) {
	return (id ->ffd->recl);
	}
//-------------------------------------------------------------------
	int copy_col_desc (FF_ID *idin, FF_ID *idout)   {
// Copies column descriptors from input flatfile to output flatfile
//  Returns no. of column descriptors copied.		.

	int ncopied = 0;
	int ncols = idin->ffd->ncols;
	int ierror;

	FF_COL_DESC coldesc;

	for (int n = 1; n <= ncols; n++)  {
		coldesc.ncol = n;
		ierror = ff_hget(idin->ffh, &coldesc);
		if (ierror != 72) break;
		if (n>1 && n<5)
			strcpy (coldesc.units, "nT");
		ierror = ff_hput(idout->ffh, &coldesc);
		if (ierror != 72) break;
		ncopied++;
	}

	return (ncopied);
	}
//-------------------------------------------------------------------

	void copy_info_abs (FF_ID *idin, FF_ID *idout, double time_first, 
		double time_last) {

/*Copies info parameters from one flatfile header file to another, except
		that info.first_time and info.last_time are not copied but are set to
			time_first and time_last.  Also copies the abstract, that is,
			all header records after the column descriptors that are not 
			included in the info keywords. */

	FF_H_INFO info;

	if (ff_hgetinfo(idin->ffh, &info) < 0) {
		cout << "Error from ff_hgetinfo.\nExit.\n";
		exit(1);
	}

	strcpy (info.owner, "MAG_TEAM");
	info.first_time = time_first;
	info.last_time = time_last;

	if (ff_hputinfo (idout->ffh, &info) < 0) {
		cout << "error from ff_hputinfo.\nExit.\n";
		exit(1);
	}

	if (ff_hcopyabstract (idin->ffh, idout->ffh) < 0)  {
		cout << "Error from ff_copyabstract.\nExit.\n";
		exit(1);
	}

	}  // End copy_info_abs().

//--------------------------------------------------------------------------------

	int get_magdata (FF_ID *id, MAGDATA *p, int swap)  {

// Gets next data record from open MAG vector flatfile; swaps bytes if swap != 0.
// Returns 1 if successful.

		FILE *ffd = id->ffd->fp; // pointer to data file
		int recl = get_recl(id);

		int numread = fread (p, recl, 1, ffd);
//		if (numread > 0 && swap) swap_magdata (p);
		if (numread > 0 && swap)
			FFSwapRecords ((char *)p, recl);
		return (numread);

	}  // End get_magdata().


//--------------------------------------------------------------------

	int put_magdata (FF_ID *id, MAGDATA *p, int swap)  {

// Writes MAG vector data record to current position in open flatfile;
//  swaps bytes if swap != 0.  Returns 1 if write was successful.

	FILE *ffd = id->ffd->fp;  //pointer to data file
	int recl = get_recl(id);

//	if (swap) swap_magdata (p);
	if (swap) FFSwapRecords ((char *)p, recl);
	int numwrote = fwrite (p, recl, 1, ffd);
	return (numwrote);
	}  //End put_magdata().


//--------------------------------------------------------------------
/////////////////////////////////////////////////////////////////
unsigned getbits (unsigned x, unsigned p, unsigned n) {

//Returns n bits of x starting at position p, right justified.
//Assume that p=31 is most significant bit and p=0 is LSB.
//From Kernigan and Ritchie, 1978, p. 45.

	return ((x >> (p+1-n)) & ~(~0 << n));
}
//////////////////////////////////////////////////////////////////

unsigned putbits (unsigned x, unsigned p, unsigned n, unsigned y) {

/*Returns x, with rightmost n bits of y put into x at p.
  Example: putbits(x,4,3,y) returns x with right 3 bits of y
	//  put into x at bits 4,3,2.                        */

	unsigned mask = ~(~0 << n);        // zeros with ones in n right bits.
	unsigned yy = (y & mask) << (p+1-n); //zero left 32-n of y; shift to p
	mask = mask << (p+1-n);            // zeros with n ones at p.
	return ((x & ~mask) | yy); 
}

///////////////////////////////////////////////////////////////////

int get_range (MAGDATA *p, int sensor)  {

	int range;
	if (sensor == 1) range = getbits(p->senstat, 31, 1);
	if (sensor == 0) range = getbits(p->senstat, 31, 2);
	return (range);
}

//---------------------------------------------------------------
void put_status (MAGDATA *pdata_rec, unsigned id_calfile) {
//Modified 5 June 2001, Joyce.
//Replaces CalibID field of sensor status word (VHMStatus or
//	FGMStatus) by the record number (mod 256) of the calibration 
//	source file that was used. The rightmost 8 bits of
//	id_calfile will replace Bits 15-8 of the sensor
//	status word. Bit 31 is assumed to be the most
//  significant bit (leftmost).

//Also replaces CoordID field of sensor status word.
//	Bits 7-0 of VHMStatus or FGMStatus are replaced 
//  by bits 7-0 of CAS_COORD_ID, defined as 3 at start of this file.  


	pdata_rec->senstat = putbits(pdata_rec->senstat,15, 8,
		id_calfile);

	pdata_rec->senstat = putbits(pdata_rec->senstat,7, 8,
		CAS_COORD_ID);

	return;
} //End put_status()


/////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////


FF_ID * calibrate(FF_ID * Cal_id, VHM_CAL *pcal_rec, MAGDATA *pdata_rec, int irange)
{

/* Modifed by Joyce, 19 Jan 2004, to interpolate on offsets */
//Modified by Joyce, 5 June 2001
//Originally written by Lee Wigglesworth, August 1998.
//Now keeps track of records read and does not attempt to read 
//	past last record. 

	static int nrows;  //Number of calib recs
	static int nread = 0;  //Number of calib recs that have been read
	static VHM_CAL calsave; /*Storage for previous Calibration Record */
	static VHM_CAL *psave = &calsave;

	double fac; /*Interpolation factor */
	double tmid; /* Midpoint of current calib rec time interval */
	double tsave; /* Midpoint of previous calib rec time interval */


	//Check the status of the calibration file pointer
	//if pointer equal to NULL open the calibration file
	if (Cal_id == NULL) {
		Cal_id = open_cal();
		if (Cal_id == NULL) 
			return (Cal_id);  // Can't open, so don't do anything.
		nrows = get_nrows(Cal_id);
		read_cal(Cal_id,pcal_rec);
		/* Initialize saved calib rec */
		memcpy (psave, pcal_rec, sizeof(calsave));
		nread = 1;
	}	

/* If data time is greater than midpt of current calib rec, 
	save current calib rec and read new one */
	tmid = 0.5*(pcal_rec->dStartTime + pcal_rec->dStopTime);
	tsave = 0.5*(psave->dStartTime + psave->dStopTime);
	while(pdata_rec->time > tmid && nread < nrows) {
		memcpy (psave, pcal_rec, sizeof(calsave));
		tsave = tmid;
		read_cal(Cal_id,pcal_rec);
		tmid = 0.5*(pcal_rec->dStartTime + pcal_rec->dStopTime);
		nread++;
	}
	
/* Calculate linear interpolation factor for offsets */
	if (tsave == tmid) fac = 1.0;
	else fac = (pdata_rec->time - tsave)/(tmid - tsave);
	if (fac < 0.) fac = 0.;
	if (fac > 1.) fac = 1.;


	sub_off_cal(fac, psave, pcal_rec, pdata_rec, irange);

/* No interpolation for rotation matrices; 
   Use either the current calib rec or the previous one */
	if (pdata_rec->time >= pcal_rec->dStartTime)  {
		rot_Ort_Sen_cal(pcal_rec, pdata_rec, irange);
		rot_SC_cal(pcal_rec, pdata_rec);
	}
	else {
		rot_Ort_Sen_cal(psave, pdata_rec, irange);
		rot_SC_cal(psave, pdata_rec);
	}

/* Interpolation on SpaceCraft Field offsets is also appropriate */
	sub_off_SC_cal(fac, psave,pcal_rec, pdata_rec);

	put_status (pdata_rec, nread);	

	return (Cal_id);
}

//------------------------------------------------------------------
//
//   open_cal
//	 Asks what calibration file you want to open and
//	 calls open_input_ff. It returns the pointer the the calibration file
//
	FF_ID * open_cal()
	{
		FF_ID * open_input_ff (char * ffname);
		FF_ID *Cal_id ;

		char ffname[256];
//		cout << endl << "Input Calibration FlatFile, default is VHM_GND_CAL";
		cout << endl;
		cout << "Enter Calibration Flatfile name: ";
		cin.getline (ffname, 255);
//		if (strlen(ffname) == 0)			//Sets default file
//		strcpy (ffname, "VHM_GND_CAL");
		Cal_id = open_input_ff (ffname);	//Opens Flatfile and returns id pointer

		if (Cal_id == NULL)
			cout << "Could not open " << ffname << endl;

		return (Cal_id);

	}
//------------------------------------------------------------------
//
//	read_cal
//	Reads a calibration flatfile record
//
	void read_cal(FF_ID *Cal_id, VHM_CAL *pcal_rec) {
	
//	int read_ok;   //jw, 5 June 2001
		FILE *ffd = Cal_id->ffd->fp;	//Pointer to flatfile data file
		int recl = Cal_id->ffd->recl;
		fread ((char *)pcal_rec, recl, 1, ffd);
//		cout << "READ_OK  " << read_ok << endl;   //jw, 5 June 2001
		return;
	}

//-------------------------------------------------------------------
//
//	sub_off_cal
/* Modified 19 Jan 2004 by Joyce to do linear interpolation
   Originally written by Lee Wigglesworth, August 1998 */

//	Subtracts off the instrument offsets from the magnetemeter data
//
	void sub_off_cal(double fac, VHM_CAL *psave, 
		VHM_CAL *cal_rec, MAGDATA *mag_data,int irange) {

	float offi[3]; /* Convenient storage for offsets */

	for (int i=0; i<3; i++) 
		offi[i] = (1.-fac)*psave->fOffset[irange][i] +
						fac*cal_rec->fOffset[irange][i];

	mag_data->bx -= offi[0];
	mag_data->by -= offi[1];
	mag_data->bz -= offi[2];
	
	return;
	}


//-------------------------------------------------------------------
//
//	sub_off_SC_cal
/* Modified 19 Jan 2004 by Joyce to do linear interpolation
   Originally written by Lee Wigglesworth, August 1998 */

//	Subtracts off the SpaceCrafts offsets from the magnetemeter data
//
	void sub_off_SC_cal(double fac, VHM_CAL *psave,
		VHM_CAL *cal_rec, MAGDATA *mag_data){
	mag_data->bx -= (1.-fac)*psave->fSCFld[0] + fac*cal_rec->fSCFld [0];
	mag_data->by -= (1.-fac)*psave->fSCFld[1] + fac*cal_rec->fSCFld [1];
	mag_data->bz -= (1.-fac)*psave->fSCFld[2] + fac*cal_rec->fSCFld [2];
	
	return;
	}

//-------------------------------------------------------------------
//
//	rot_Ort_Sen_cal
//	Rotates the Magnetometer data 
//	by the Orthogonality and Sensitivity Matrix
//
	void rot_Ort_Sen_cal(VHM_CAL *cal_rec, MAGDATA *mag_data,int irange) {

	int i,j,n=3;
	float hold_data[3],rot_data[3];
	hold_data[0] = mag_data->bx;
	hold_data[1] = mag_data->by;
	hold_data[2] = mag_data->bz;

	for(i = 0;i<n;i++)
	{
	  rot_data[i] = 0.0f;
		for(j=0;j<n;j++)
		{
			rot_data[i] += (cal_rec->fOrthog [irange][i][j] * hold_data[j]);
		}
	}
	
	mag_data->bx = rot_data[0];
	mag_data->by = rot_data[1];
	mag_data->bz = rot_data[2];
	return;
	}
//-------------------------------------------------------------------
//
//	rot_SC_cal
//	Rotates the Magnetometer data 
//	by the SpaceCraft Matrix
//
	void rot_SC_cal(VHM_CAL *cal_rec, MAGDATA *mag_data){

	int i,j,n=3;
	float hold_data[3],rot_data[3];

	hold_data[0] = mag_data->bx;
	hold_data[1] = mag_data->by;
	hold_data[2] = mag_data->bz;

	for(i = 0;i<n;i++)
	{
	  rot_data[i] = 0.0f;
		for(j=0;j<n;j++)
		{
			rot_data[i] += (cal_rec->fSCTrans[i][j] * hold_data[j]);
		}
	}

	mag_data->bx = rot_data[0];
	mag_data->by = rot_data[1];
	mag_data->bz = rot_data[2];
	return;
	}

///////////////////////////////////////////////////////////////////////////////

int is_valid (MAGDATA *pdata_rec, int sensor) {

//Performs validity checks on vector (FGM or VHM) data records.
//Returns 1 if valid; 0 if not valid.
//Checks if sensor power on and if components are within scale for range.

	int valid = 0;
	double ftest[4] = {40., 400., 10000., 44000.};
	double vtest[2] = {32., 256.};

	int range = get_range (pdata_rec, sensor);
	int vhm_on = getbits(pdata_rec->magstat, 27, 1);
	int fgm_on = getbits(pdata_rec->magstat, 26, 1);

	double ax = fabs(pdata_rec->bx);
	double ay = fabs(pdata_rec->by);
	double az = fabs(pdata_rec->bz);

	switch (sensor) {
		case 0:     //FGM
			if (fgm_on && ax <= ftest[range] && ay <= ftest[range]
				&& az <= ftest[range])   valid = 1;
			break;
		case 1:     //VHM
			if (vhm_on && ax <= vtest[range] && ay <= vtest[range]
				&& az <= vtest[range])   valid = 1;
	}

	return (valid);
}

////////////////////////////////////////////////////////////////////////////////