// program to read STEREO Packet A and output Level 1 intensities

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
// #include <conio.h>   DOS only
#include <math.h>
#include "STEREOsecs.h"

#define BLK 80
#define NPACKETBYTES   261

char tbuf[BLK];

FILE *infi,*outfi,*outfi2;
char ifp[]="payload_telemetry.std",ofp[]="PacketA.bin",ofp2[]="PacketA.txt";

short int little_endianflag;
int little_endian(void);  // returns 1 if little_endian, else returns 0
void reverse4bytes(unsigned char *p);
void reverse2bytes(unsigned char *p);
long long_rate(unsigned short packed);  /* Unpack to long */
double dbl_rate(unsigned short packed); /* Unpack to double */
void uncompress_hdr(char *buf);
void uncompress_APacket(char *buf);

unsigned char buffer[NPACKETBYTES+11];  // 272 bytes in each STEREO binary data packet, of which the first 11 bytes are the CCSDS header
										// presently reading CCSDS header (11 bytes) and science data (261 bytes) separately

struct uncomp_hdr_CCSDS     // CCSDS hdr 
{
	unsigned short version; 
	unsigned short type;
	unsigned short hdr_flg;
	short ApId;
	unsigned short grp_flg;
	short seq_cnt;
	unsigned short pck_len;
	int secs;
	unsigned short subsecs;
}; 
 
struct uncomp_hdr_CCSDS *puncomp_hdr,uncomp_hdr;

struct PacketA			// 590 (dec)  24e (hex)
{  // A data packet without CCSDS header
	unsigned char    HET_mode;
	unsigned char    TBD0;
	unsigned short   MajorFrame;   // not log-compressed
	unsigned short   LivetimeX;
	unsigned short   TriggerRateX;
	unsigned short   CoincRateX;
	unsigned short	 NEventsTotalX;
	unsigned short   NSinglesQueuedX;
	unsigned short   NStopQueuedX;
	unsigned short   NPenQueuedX;
	unsigned short   NStopHX;
	unsigned short   NStopHeX;
	unsigned short   NStopZgt2X;
	unsigned short   NPenHX;
	unsigned short   NPenHeX;
	unsigned short   NPenZgt2X;
	unsigned short   NInvalidX[4];
	unsigned short   NStimX;
	unsigned short   SWCtrX[109];
	unsigned char    TBD1;
	unsigned char    checksum;   
};

struct PacketA  *pAPacket,APacket;
int numread,numwritten,Npackets,NApackets;

double StopGF_HiRate_eHHe;
double StopGF_LoRate_eHHe;
double PenGF_HiRate;
double PenGF_LoRate; 

long	MajorFrame;
long	Livetime;
long	TriggerRate;
long	CoincRate;
long	NEventsTotal;
long	NSinglesQueued;
long	NStopQueued;
long	NPenQueued;
long	NStopH;
long	NStopHe;
long	NStopZgt2;
long	NPenH;
long	NPenHe;
long	NPenZgt2;
long	NInvalid[4];
long	NStim;
double	SWCtr[109];

char ts[25];

int main()
{
	little_endianflag=little_endian();

//	printf("\nEnter the name of the binary input file containing STEREO data packets (e.g., payload_telemetry.std): ");
//	scanf("%s",ifp);
	if(!(infi=fopen(ifp,"rb")))
	{
		printf("\nCan't open %s; CR to exit ",ifp);
		getchar();
		exit(1);
	}


//	printf("\nEnter the name of the output binary data file: ");
//	scanf("%s",ofp);
	if(!(outfi=fopen(ofp,"wb")))
	{
		printf("\nCan't open %s; CR to exit ",ofp);
		getchar();
		exit(1);
	}

	if(!(outfi2=fopen(ofp2,"wt")))
	{
		printf("\nCan't open %s; CR to exit ",ofp);
		getchar();
		exit(1);
	}



	puncomp_hdr=&uncomp_hdr;
	pAPacket=&APacket;

	numread = fread( buffer, sizeof( char ), 32, infi );  // dump non-recurrent header	

	Npackets=0;
	NApackets=0;
	numwritten=0;

	while(!feof(infi))
	{
		numread = fread( buffer, sizeof( char ), 26, infi);  // dump ground receipt header
		numread = fread( buffer, sizeof( char ), 11, infi ); // read CCSDS header


		if(feof(infi))
			break;

		uncompress_hdr(buffer);

		if((puncomp_hdr->ApId)==(short)590)
			numwritten += fwrite(buffer,sizeof(char),11,outfi);
		numread = fread( buffer, sizeof( char ), NPACKETBYTES, infi );
		if(numread!=NPACKETBYTES)
			break;

		if((puncomp_hdr->ApId) == (short)590)   // HET Packet A 
		{
			// uncompress packet and write out corresponding data
			
			Npackets++;
			NApackets++;

 			if(NApackets==1)
			{
				sec58_ts(puncomp_hdr->secs,ts);
				fprintf(outfi2,"\n1st A packet starts %s (sec58=%ul)\n",ts,puncomp_hdr->secs);
				numwritten += fwrite(buffer,sizeof(char),NPACKETBYTES,outfi);
			}

			uncompress_APacket(buffer); 
			
			break;    // just dumping first A packet
		}
		else
		{
			Npackets++;
			if(Npackets==1)
			{
				sec58_ts(puncomp_hdr->secs,ts);
				fprintf(outfi2,"\n1st packet starts %s (sec58=%ul)\n",ts);
			}
//			if((puncomp_hdr->ApId)>590 && (puncomp_hdr->ApId)<625)
//			{
//				sec58_ts(puncomp_hdr->secs,ts);
//				printf("\nApID = %hd, Npackets=%d, NApackets=%d, t=%s\n",puncomp_hdr->ApId,Npackets,NApackets,ts);
//			}
		}
	}

	fprintf(outfi2,"\nNpackets=%d, NApackets=%d, # of bytes written: %d\n",Npackets,NApackets,numwritten);	

	fclose(infi);
	fclose(outfi);
	fclose(outfi2);

	printf("\nOutput data is in binary file %s and text file %s\n\n",ofp,ofp2);

	return 1;
}




int little_endian(void)		// returns 0 if in a big-endian environment and
							// returns 1 if in a little endian environment
{
	union
	{
		int intunion;
		unsigned char  chunion[4];
	} test;
	
	test.chunion[0]=(unsigned char)2;
	test.chunion[1]=(unsigned char)4;
	test.chunion[2]=(unsigned char)8;
	test.chunion[3]=(unsigned char)0;

	if(test.intunion == 525314)   // =0x80402
		return 1;
	else
		return 0;
}


void reverse4bytes(unsigned char *p)
{
	unsigned char temp[4];

	temp[3]=p[0];
	temp[2]=p[1];
	temp[1]=p[2];
	temp[0]=p[3];

	p[0]=temp[0];
	p[1]=temp[1];
	p[2]=temp[2];
	p[3]=temp[3];
}


void reverse2bytes(unsigned char *p)
{
	unsigned char temp[2];

	temp[1]=p[0];
	temp[0]=p[1];

	p[0]=temp[0];
	p[1]=temp[1];
}

/* Unpacking (not required in flight code) */

long long_rate(unsigned short packed)  /* Unpack to long */
{
        int power;
        long out;

        power= packed>>11;
        if (power>1)
        {
                out=((packed&0x07ff)|0x0800);
                out=out<<(power-1);
        }
        else
                out=packed;
        return out;
}


double dbl_rate(unsigned short packed) /* Unpack to double */
{
        int power;
        double out;

        power= packed>>11;
        if (power>1)
        {
                out=((packed&0x07ff)|0x0800);
                out=out*pow(2.,(double)(power-1));
        }
        else
                out=packed;
        return out;
}




// Given the first 11 bytes (CCSDS header), this routine will uncompress the values into a structure
// The CCSDS header has the bytes in big-endian order
// The code should work equally well on big-endian and little-endian machines (??? needs to be verified) 
void uncompress_hdr(char* buf)
{
	 unsigned char *ubuf;

     ubuf = (unsigned char*)buf;
	 puncomp_hdr->version = ubuf[0] >> 5;
	 puncomp_hdr->type = (ubuf[0] & 16) >> 4;
	 puncomp_hdr->hdr_flg = (ubuf[0] & 8) >> 3;

		// shft into the MSB and then add the LSB and mask off bits
	 puncomp_hdr->ApId = ((ubuf[0] << 8) + ubuf[1]) & 0x07ff;
	 puncomp_hdr->grp_flg = ((ubuf[2] << 8) + ubuf[3]) & 0xcfff;
	 puncomp_hdr->seq_cnt = ((ubuf[2] << 8) + ubuf[3]) & 0x3fff;

	 puncomp_hdr->pck_len = ((ubuf[4] << 8) + ubuf[5]);
 	 puncomp_hdr->secs =	(ubuf[6] << 24) + (ubuf[7] << 16) + (ubuf[8] << 8) + ubuf[9];
	 puncomp_hdr->subsecs = ubuf[10];
	 
	 if (puncomp_hdr->subsecs > 0) 
		puncomp_hdr->subsecs /= 256;
	 else
		puncomp_hdr->subsecs = 0;

	 puncomp_hdr->secs += puncomp_hdr->subsecs;   
}



void uncompress_APacket(char *buf)
{ 
	 unsigned char *ubuf;
	 int i;

     ubuf = (unsigned char*)buf;

//	 printf("\nubuf[2]=0x%x, ubuf[3]=0x%x",ubuf[2],ubuf[3]);

	 pAPacket->MajorFrame=(ubuf[4] << 8)+ubuf[3];   // not log-compressed

	 pAPacket->LivetimeX=(ubuf[6] << 8) + ubuf[5];
	 Livetime=long_rate(pAPacket->LivetimeX);

	 pAPacket->TriggerRateX=(ubuf[8] << 8) + ubuf[7];
	 TriggerRate=long_rate(pAPacket->TriggerRateX);

	 pAPacket->CoincRateX=(ubuf[10] << 8) + ubuf[9];
	 CoincRate=long_rate(pAPacket->CoincRateX);

	 pAPacket->NEventsTotalX=(ubuf[12] << 8) + ubuf[11];
	 NEventsTotal=long_rate(pAPacket->NEventsTotalX);

	 pAPacket->NSinglesQueuedX=(ubuf[14] << 8) + ubuf[13];
	 NSinglesQueued=long_rate(pAPacket->NSinglesQueuedX);

	 pAPacket->NStopQueuedX=(ubuf[16] << 8) + ubuf[15];
	 NStopQueued=long_rate(pAPacket->NStopQueuedX);

	 pAPacket->NPenQueuedX=(ubuf[18] << 8) + ubuf[17];
	 NPenQueued=long_rate(pAPacket->NPenQueuedX);

	 pAPacket->NStopHX=(ubuf[20] << 8) + ubuf[19];
	 NStopH=long_rate(pAPacket->NStopHX);

	 pAPacket->NStopHeX=(ubuf[22] << 8) + ubuf[21];
	 NStopHe=long_rate(pAPacket->NStopHeX);

	 pAPacket->NStopZgt2X=(ubuf[24] << 8) + ubuf[23];
	 NStopZgt2=long_rate(pAPacket->NStopZgt2X);

	 pAPacket->NPenHX=(ubuf[26] << 8) + ubuf[25];
	 NPenH=long_rate(pAPacket->NPenHX);

	 pAPacket->NPenHeX=(ubuf[28] << 8) + ubuf[27];
	 NPenHe=long_rate(pAPacket->NPenHeX);

	 pAPacket->NPenZgt2X=(ubuf[30] << 8) + ubuf[29];
	 NPenZgt2=long_rate(pAPacket->NPenZgt2X);

	 for(i=0;i<4;i++)
	 {
		pAPacket->NInvalidX[i]=(ubuf[32+2*i] << 8) + ubuf[31+2*i];
		NInvalid[i]=long_rate(pAPacket->NInvalidX[i]);
	 }
	 
	 pAPacket->NStimX=(ubuf[40] << 8) + ubuf[39];
	 NStim=long_rate(pAPacket->NStimX);

	 for(i=0;i<109;i++)
	 {
		 pAPacket->SWCtrX[i]=(ubuf[42+2*i] << 8) + ubuf[41+2*i];
		 SWCtr[i]=long_rate(pAPacket->SWCtrX[i]);
	 }

//	 ubuf[259] = TBD1 (single byte)

	 pAPacket->checksum=ubuf[260];


	 sec58_ts(puncomp_hdr->secs,ts);
	 fprintf(outfi2,"\nApID=%hd, MajorFrame=%hu,Livetime=%ld, ts=%s\n",puncomp_hdr->ApId,pAPacket->MajorFrame,Livetime,ts);
//	 getchar();
}