/****************************************************************************
*****************************************************************************
    cmdutil.c       - Bird Command Utilities Routine

    written for:    Ascension Technology Corporation
		    PO Box 527
		    Burlington, Vermont  05402
		    802-655-7879

    by:             Jeff Finkelstein
		    802-985-2535


    Modification History:
			4/29/91        jf  - created from BIRDCMDS.c
			5/20/91            jf  - fixed row/col order in printmatrix
	    9/16/91        jf  - added showfbbconfig
	    2/25/92        jf  - removed showfbbconfig
	    4/7/92         jf  - added fprintmatrix
	    4/20/92        mo  - added fprintquaternions() and
				 printquaternions().
	    10/12/92       jf  - modified to display 3 sig. digits for Matrix
	    12/23/92       jf  - updated readbirddata to be CPU independent
				 by using C types to define byte ordering
	    1/26/93        jf  - added functions to print/file for all
				 output modes
			   jf  - modified readbirddata to read an extra
				 character if in fbbgroupdata mode
	    2/26/93        jf  - added getsystemstatus
	    3/1/93         jf  - added checkerrorstatus
			       - added displayerror
			       - added getsystemstatus
	    3/22/93        jf  - added expanded error 'Note'
	    6/22/94        ssw - modified getaddmode to include setting IRQ 0
				 fix to stop lockout

	   <<<< Copyright 1990 Ascension Technology Corporation >>>>
*****************************************************************************
****************************************************************************/
#include <stdio.h>          /* general I/O */
#include <math.h>           /* trig math funcs */
#include "asctech.h"        /* general definitions */
#include "compiler.h"       /* Compiler Specific Header */
#include "menu.h"           /* for menus */
#include "serial.h"         /* serial I/O */
#include "cmdutil.h"        /* cmdutil.header */

/*
    External Definitions
*/

extern float crystalfreq;
extern short buttonvalue;
extern float posk;
extern unsigned char fbbgroupdataflg;
extern short numfbbaddrs;
extern short numfbbrcvrs;
extern unsigned char fbbsystemstatus[];
extern unsigned char rs232tofbbstartaddr;
extern unsigned char displayliststartaddr;
extern unsigned char displayliststopaddr;
extern unsigned char displaymultidataflg;

/*
    Definitions Expanded Error Decoding
*/
unsigned char fbbaddrbits;
unsigned char cmdbitshft;

/*
    readbirddata        -   Read Bird Data and convert to 2's complement

    Prototype in:       cmdutil.h

    Parameters Passed:  birddata    - pointer to integer array to store data
			numwords    - number of words (16 bits) to read
			outputmode  - POINT, CONTINUOUS, or STREAM
			buttonmode  - 0 if off, 1 if on (send button value)

    Return Value:       TRUE if successful
			FALSE otherwise

    Remarks:            routine reads a record from the bird, and adjusts
			the least and most significant Bird bytes into 2's
			complemented words.
*/
int readbirddata(birddata,numwords,outputmode,buttonmode)
short * birddata;
short numwords;
short outputmode;
short buttonmode;
{
	short i;
    short extrachar=0;

    if (fbbgroupdataflg)
	extrachar = 1;

    /*
	Read 2*numbwords characters from the bird, check for errors
	...add one character to record length if buttonmode == 1
    */
    if ((get_serial_record((unsigned char *)birddata,
			   (2 * numwords) + buttonmode + extrachar,
			   outputmode)) < 0)
    {
	if (outputmode == STREAM)
		send_serial_cmd((unsigned char *)"B",1);     /* stop streaming, via Point cmd */
	printf("\n\r** ERROR ** could not read record data from the BIRD\n\r");
	hitkeycontinue();
	return(FALSE);
    }

    /*
	go though the birddata and make into two's complemented
	16 bit integers by:
	    - lsbyte << 1
	    - word << 1
    */
    for (i=0;i<numwords;i++)
    {
	/*
	   MOTOROLA and some RISC CPUs place
	   the high order byte of a 16 bit word in the lower address of
	   memory whereas INTEL like CPUs place the high order bytes in the
	   higher addresses...therefore this operation must be CPU
		   independent
	*/
	*birddata++ = (short)((((short)(*(unsigned char *) birddata) & 0x7F) |
				(short)(*((unsigned char *) birddata+1)) << 7)) << 2;
    }
    
    /*
	Read the Button Value if Required
	...Note that birddata currently points to the button value
    */
    if (buttonmode)
		buttonvalue = (short)(*(unsigned char *)birddata);

    return(TRUE);
}

/*
    check_done          -   Check to seed if Data Output Done

    Prototype in:       cmdutil.h

    Parameters Passed:  outputmode      - output mode POINT,CONTINUOUS, STREAM

    Return Value:       TRUE if done
			FALSE otherwise

    Remarks:            POINT mode:
			    - wait for key hit
			    - if B/b return FALSE
			      else, return TRUE

			CONTINUOUS mode:
			    - if key hit return TRUE
			      else, return FALSE

			STREAM mode:
			    - if key hit
				send 'B' command to turn off stream mode
				return TRUE
			      else, return FALSE
*/
int check_done(outputmode)
short outputmode;
{
	short chr;            /* character returned from getch() */

    /*
	Do this as a function of POINT, CONTINUOUS, or STREAM outputmode
    */
    switch (outputmode)
    {
	case STREAM:
	    if (ckkbhit())
	    {
		clearkey();                 /* clear the keyboard */
		send_serial_cmd((unsigned char *)"B",1);     /* stop streaming, via Point cmd */
		if (phaseerror_count)       /* inform the user of phase errors */
					printf("*NOTE* %d Phase Errors have occured\n\r",phaseerror_count);

		break;                        /* return TRUE */
	    }
	    return(FALSE);                  /* not done */

	case CONTINUOUS:
	    if (ckkbhit())                  /* if key hit... */
	    {
		clearkey();                 /* clear the keyboard */
		break;
	    }
	    else                            /* no key hit .. continue */
	    {
		if (send_serial_cmd((unsigned char *)"B",1) != 1) /* get another set of points */
		    return(TRUE);           /* return done if errors */

		return(FALSE);              /* continue */
	    }

	case POINT:
	    while (!ckkbhit());               /* wait for a key */
	    chr = getkey();                   /* get the character */
	    if ((chr == 'B') || (chr == 'b')) /* check if a B or b */
	    {
		send_serial_cmd((unsigned char *)"B",1);     /* get another set of points */
		return(FALSE);              /* return not done */
	    }
	    break;
    }

    /*
	Wait for the user to see the data
    */
    hitkeycontinue();
    return(TRUE);               /* indicate done */
}

/*
    displaybirddata     Display and File the Bird Data

    Prototype in:       cmdutil.h

    Parameters Passed:  birddata - array of birddata
			unsigned char datamode - POS, ANGLE, POSANGLE..
			buttonmode - ON/OFF
			datafilestream - file to store data in..if any

    Return Value:       void

    Remarks:

*/
int displaybirddata(birddata, datamode, buttonmode, displayon, displayaddr, datafilestream)
short * birddata;
unsigned char datamode;
short buttonmode;
unsigned char displayon;
unsigned char displayaddr;
FILE * datafilestream;
{
    short displaysize;
    unsigned char * tempbirddata;

    /*
	Use tempbirddata to point to individual characters in
	the birddata
    */
    tempbirddata = (unsigned char *) birddata;

    /*
	Display the address at  the begginning if in RS232TOFBB mode

	Display the Address information at the end if in the
	group data mode.. since that is where the address information
	is within the data stream
    */
    if ((displaymultidataflg) && (!fbbgroupdataflg))
    {
	if (displayon)
	    printf("%d ", displayaddr);

	if (datafilestream)
	    fprintf(datafilestream,"%d ", displayaddr);
    }

    /*
       Set the for the number of WORDs (16 bits) and Pos/Orientation Mode
       command
    */
    switch(datamode)
    {
	case POS:
	    displaysize = printposition((short *) tempbirddata,buttonmode,displayon,datafilestream);
	    break;

	case ANGLE:
	    displaysize = printangles((short *) tempbirddata,buttonmode,displayon,datafilestream);
	    break;

	case MATRIX:
	    displaysize = printmatrix((short *) tempbirddata,buttonmode,displayon,datafilestream);
	    break;

	case QUATER:
	    displaysize = printquaternions((short *) tempbirddata,buttonmode,displayon,datafilestream);
	    break;

	case POSANGLE:
	    displaysize = printposangles((short *) tempbirddata,buttonmode,displayon,datafilestream);
	    break;

	case POSMATRIX:
	    displaysize = printposmatrix((short *) tempbirddata,buttonmode,displayon,datafilestream);
	    break;

	case POSQUATER:
	    displaysize = printposquaternions((short *) tempbirddata,buttonmode,displayon,datafilestream);
	    break;

	default:
	    printf("\n\r** ERROR ** illegal data mode in displaybirddata\n\r");
	    return(0);
    }

    /*
	If in Group Data Mode, print the address just received
    */
    if (fbbgroupdataflg)
    {
	/*
	    get to the addr loc
	*/
	tempbirddata += ((displaysize * 2) + buttonmode);
	if (displayon)
	    printf("    %d", *tempbirddata); /* print the address */

	if (datafilestream)
	    fprintf(datafilestream, "    %d", *tempbirddata);

	tempbirddata += 1;                   /* increment the pointer */
    }

    /*
	Set up for the next line
    */
    if (displayon)
    {
	printf("\n\r");
	if ((datamode == MATRIX) || (datamode == POSMATRIX))
	    printf("\n\r");
    }

    if (datafilestream)
    {
	fprintf(datafilestream,"\n");
	if ((datamode == MATRIX) || (datamode == POSMATRIX))
	    fprintf(datafilestream,"\n");
    }

    return(displaysize);
}

/*
    printposition       Print and File the Position Data

    Prototype in:       cmdutil.h

    Parameters Passed:  birddata - array of birddata
			buttonmode - ON/OFF
			displayon - DISPLAYON/OFF
			datafilestream - file to store data in..if any

    Return Value:       int 3

    Remarks:
*/
int printposition(birddata,buttonmode,displayon,datafilestream)
short * birddata;
short buttonmode;
unsigned char displayon;
FILE * datafilestream;
{
    short i;
    float floatdata[3];
    char * printdataformat = "\t%7.2f\t%7.2f\t%7.2f";
    char * printbuttonformat = "\t%3d";

    /*
	Only compute if display or file is enabled
    */
    if ((displayon) || (datafilestream))
    {
	for (i=0;i<3;i++)
	    floatdata[i] = (float)(birddata[i] * posk);
    }

    /*
	Display the Data and Button Value (if required)
    */
    if (displayon)
    {
	printf(printdataformat,floatdata[0],floatdata[1],floatdata[2]);
	if (buttonmode != 0)
	    printf(printbuttonformat, buttonvalue);
    }

    /*
	Save the Data to a File...only if one exists!
    */
    if (datafilestream)
    {
	/*
	    print the data to the file
	*/
	fprintf(datafilestream,printdataformat,
		floatdata[0],floatdata[1],floatdata[2]);

	/*
	    save the Button Value if required
	*/
	if (buttonmode != 0)
	    fprintf(datafilestream, printbuttonformat, buttonvalue);
    }
    return(3);
}

/*
    printangles         Print and File the Angle Data

    Prototype in:       cmdutil.h

    Parameters Passed:  birddata - array of birddata
			buttonmode - ON/OFF
			displayon - DISPLAYON/OFF
			datafilestream - file to store data in..if any

    Return Value:       int 3

    Remarks:
*/
int printangles(birddata,buttonmode,displayon,datafilestream)
short * birddata;
short buttonmode;
unsigned char displayon;
FILE * datafilestream;
{
    short i;
    float floatdata[3];
    char * printdataformat = "\t%7.2f\t%7.2f\t%7.2f";
    char * printbuttonformat = "\t%3d";

    /*
	Only compute if display or file is enabled
    */
    if ((displayon) || (datafilestream))
    {
	for (i=0;i<3;i++)
	    floatdata[i] = (float)(birddata[i] * ANGK);
    }

    /*
	Display the Data and Button Value (if required)
    */
    if (displayon)
    {
	printf(printdataformat,floatdata[0],floatdata[1],floatdata[2]);
	if (buttonmode != 0)
	    printf(printbuttonformat, buttonvalue);
    }

    /*
	Save the Data to a File...only if one exists!
    */
    if (datafilestream)
    {
	/*
	    print the data to the file
	*/
	fprintf(datafilestream,printdataformat,
		floatdata[0],floatdata[1],floatdata[2]);

	/*
	    save the Button Value if required
	*/
	if (buttonmode != 0)
	    fprintf(datafilestream, printbuttonformat, buttonvalue);

    }
    return(3);
}

/*
    printmatrix         Print and File the Matrix Data

    Prototype in:       cmdutil.h

    Parameters Passed:  birddata - array of birddata
			buttonmode - ON/OFF
			displayon - DISPLAYON/OFF
			datafilestream - file to store data in..if any

    Return Value:       int 9