/****************************************************************************
*****************************************************************************
    birdcmds        - Bird Command Routines

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

    by:             Jeff Finkelstein
		    802-985-2535


    Modification History:
	  10/18/90  jf - released
	  10/24/90  jf - modified trap for 1.00 in birdanglealign to be 0x7fff
	  11/12/90  jf - add new commands
	  11/29/90  jf - added 'Hit 'B' Key...' to checkdone
			 added the dislay of retrace count to display CRT
			 Pick Display
	  2/4/91    jf - added code to display the button value if enabled
		       - modified change/examine filter strings to reflect
			 Wide/Narrow Notch filters
		       - fixed negative angle bug in bird_referframe
	  2/20/91   jf - added initialization of factory test mode to
			 birdloadconfig
	  3/16/91   jf - added new selection for CRT Sync for Fast Vertical
			 retrace (> 70 Hz)
	  4/25/91   jf - added restoreconsole to exit() condition for UNIX
			 compatibility
	  4/29/91   jf - fixed bug in checkdone(), STREAM mode to assure
			 a TRUE return if a key is hit
	  9/17/91   jf - added new change and examine value commands
			 for the FBB
		    jf - added posk global for POSK36 and POSK72
	  10/18/91  jf - modified max measurement rate to 144 Hz
	  11/1/91   jf - added chg/exm alpha max and glitch checking
	  11/10/91  jf - changed 'glitch checking' to 'sudden output change'
	  11/18/91  jf - changed max CRT sync to 72 Hz
	  1/3/92    jf - fixed bug in shutting off button mode
	  2/25/92   jf - added showfbbconfig from cmdutil.c
	  3/23/92   jf - modified Alpha Min/Max to 7 word tables
	  4/7/92    jf - added data file streaming to all the orientation
			 modes
		    jf - modified operation of chg/exm Error Mask
		    jf - modified measurement rate const to 0.3 for
			 ROMs 3.28 and 4.11
	  4/20/92   mo - added new functions bird_quaternions(),
			 bird_posquaternions() ,fprintquaternions()
			 and printquaternions().
	  6/1/92    jf - updated examine value to reflect new status 
			 definitions
	  6/9/92    jf - removed save config from change and examine value
		    jf - added examine extended error code
	  12/22/92  jf - updated for CPU independence...high byte/low byte
			 order now handled at run time to be compatible
			 with Motorola and RISC CPUs
	  1/26/93   jf - removed individual pos/orient cmds and replaced
			 them with getsinglebirddata
	  2/23/93   jf - added ERC filter values and text strings
	  5/18/94   sw - added Normal/expanded address mode detection
			 for examine birdstatus
	  5/23/94   sw - added XYZ Reference frame change value / examine value 
		       - corrected errors in filter distance labels
	  12/20/94  sw - added routines to run error statistics.
	   5/11/95  sw - removed addressing mode check on examine value, unless
			 command will need that information.                         
	   6/6/95   sw - Placed \r into new printf statements to work with unix systems.

	   <<<< 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"        /* command utilities */
#include "birdcmds.h"

/***********************
    Globals
***********************/

/*
    External Varibles
*/
extern FILE * datafilestream;
extern unsigned char displaymultidataflg;

/*
    Position Variable
*/
float posk;
float crystalfreq = DEFAULTCRYSTALFREQ;
float clockvalue = DEFAULTCLOCKVALUE;
short buttonvalue = 0;
unsigned char fbbgroupdataflg = FALSE;
short flocksize = 1;
unsigned char fbbsystemstatus[31];
short numfbbaddrs = 30;
short numfbbrcvrs;
unsigned data_ready_char = ',';

/*
  data returned
*/
short birddata[14];    /* holds the data from the BIRD for buttondata */

/************************
    Routines
************************/

/*
    getsinglebirddata   Get Data from a Single Bird

    Prototype in:       birdcmds.h

    Parameters Passed:  outputmode  - POINT,CONTINUOUS or STREAM
			datamode    - POS, ANGLE, POSANGLE..
			displayon   - 
			buttonmode  - current button mode, 0 if button
				      character is not appended to data,
				      1 if button character is appended

    Return Value:       TRUE if successful
			FALSE otherwise

    Remarks:            Displays current Bird Data in POINT, CONTINUOUS
			or STREAM mode
*/
int getsinglebirddata(outputmode, datamode, displayon, buttonmode)
short outputmode;
unsigned char datamode;
unsigned char displayon;
short buttonmode;
{
    unsigned char datasize;
    unsigned char posorientcmd;

    /*
       Set the for the number of WORDs (16 bits) and the
       Pos/Orientation Mode command
    */
    switch(datamode)
    {
	case POS:
	    datasize = 3;
	    posorientcmd = 'V';
	    break;

	case ANGLE:
	    datasize = 3;
	    posorientcmd = 'W';
	    break;

	case MATRIX:
	    datasize = 9;
	    posorientcmd = 'X';
	    break;

	case QUATER:
	    datasize = 4;
	    posorientcmd = 92;
	    break;

	case POSANGLE:
	    datasize = 6;
	    posorientcmd = 'Y';
	    break;

	case POSMATRIX:
	    datasize = 12;
	    posorientcmd = 'Z';
	    break;

	case POSQUATER:
	    datasize = 7;
	    posorientcmd = ']';
	    break;

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


    /*
	Send the Mode Command (ie. POS, ANGLES...
    */
    if (send_serial_cmd(&posorientcmd,1) != 1)
	return(FALSE);

    /*
	Send the Stream command if in STREAM mode
    */
    if (outputmode == STREAM)
	if (send_serial_cmd((unsigned char *)"@",1) != 1)
	    return(FALSE);

    /*
	 Send the User a Prompt if in Point mode
    */
    if (outputmode == POINT)
	 printf ("\n\rHit the 'B' Key to read Bird data or any other key to Quit\n\r");

    /*
	Now read the data from the Bird
	storing the data in birddata[]
	check for done condition, keyboard hit while displaying the data
    */

    while (!check_done(outputmode))
    {
	/*
	    Get the data NOW
	*/
	if (!readbirddata(birddata,datasize,outputmode,buttonmode))
	    return(FALSE);

	/*
	    Display and File (if required) the Data Now
	*/
	if (!displaybirddata(birddata, datamode, buttonmode,
			displayon,0,datafilestream))
	    return(FALSE);
    }
    return(TRUE);
}

/*
    bird_anglealign     - Align the Bird User Specified Direction

    Prototype in:       birdcmds.h

    Parameters Passed:  void

    Return Value:       TRUE if successful
			FALSE otherwise

    Remarks:            prompts the user for Azimuth, Elevation and Roll of
			the User Direction and sends these values to the Bird
			to reorient the Birds direction
*/
int bird_anglealign()
{

    short i;                /* matrix pointer */
    unsigned char * cdataptr;        /* pointer to cdata */
    float tempfloat;        /* temporary float value */
    float angle[3];         /* angles input byte the user */
    static unsigned char cdata[] =   {'J',0,0,0,0,0,0,0,0,0,0,0,0}; /* cmd + 12 bytes */

    /*
	Get the 3 angles from the User
    */
    if (getangles("\n\rInput Azimuth, Elevation, and Roll angles from receiver\n\rto reference direction in degrees: ",&angle[0]) ==
ESC_SEL)
	return(FALSE);

    /*
	convert sines and cosines to configuration bytes in cdata[],
	constructing the command string as we go.
    */
    cdataptr = &cdata[1];           /* assign pointer to the first character
				       after the command character
				    */
    for (i = 0; i < 3; i++)
    {
	/*
	    calculate the sine of the angle and
	*/
	tempfloat = sin((double)(angle[i] * DTR));

	/*
	    convert to a word and store in cdata
	    NOTE: trap for sin(90)...since the bird
	    can only accept data from -1.000 to 0.99996 (0x8000 to 0x7fff)
	*/
	if (tempfloat < 0.99998)
	{
	    *cdataptr++ = (unsigned char) ((short) (tempfloat * FTW) & 0x0ff);
	    *cdataptr++ = (unsigned char) (((short) (tempfloat * FTW) & 0x0ff00) >> 8);
	}
	else
	{
	    *cdataptr++ = 0x0ff;
	    *cdataptr++ = 0x07f;
	}

	/*
	    calculate the cosine of the angle and
	*/
	tempfloat = cos((double)(angle[i] * DTR));

	/*
	    convert to a word and store in cdata
	    NOTE: trap for cos(0)...since the bird
	    can only accept data from -1.000 to 0.99996 (0x8000 to 0x7fff)
	*/
	if (tempfloat < 0.99998)
	{
	    *cdataptr++ = (unsigned char) ((short) (tempfloat * FTW) & 0x0ff);
	    *cdataptr++ = (unsigned char) (((short) (tempfloat * FTW) & 0x0ff00) >> 8);
	}
	else
	{
	    *cdataptr++ = 0x0ff;
	    *cdataptr++ = 0x07f;
	}
    }

    /*
	Send the Command
    */
    if (send_serial_cmd(cdata,13) != 13)
	return(FALSE);

    printf("Angle Alignment Data sent to the Bird\n\r");
    hitkeycontinue();

    return(TRUE);
}

/*
    bird_hemisphere     -   Set the Birds Hemisphere

    Prototype in:       birdcmds.h

    Parameters Passed:  void

    Return Value:       TRUE if successful
			FALSE if unsuccessful
			ESC_SEL if ESC selected

    Remarks:            prompt the user for the Bird hemisphere and send
			down coresponding hemisphere command to the Bird
*/
int bird_hemisphere()
{
    static char * hemisphere_menuptr[] = 
				  {"Hemisphere Options:",  /* menu options */
				   "Forward",
				   "Aft",
				   "Upper",
				   "Lower",
				   "Left",
				   "Right"};
    static unsigned char hemisphere_cdata[] = {'L',0,0};  /* command string 
							     to BIRD */

    /*
	Send the Menu to the User
    */
    switch (sendmenu(hemisphere_menuptr,6))
    {
	/*
	    Setup the Command string to the Bird as a function of the
	    User menu selection.....
	    .....2 data bytes must be set for HEMI_AXIS and HEMI_SIGN
	*/
	case 0: /* Forward */
	    hemisphere_cdata[1] = 0;       /* set XYZ character */
	    hemisphere_cdata[2] = 0;       /* set Sine character */
	    break;

	case 1: /* Aft */
	    hemisphere_cdata[1] = 0;       /* set XYZ character */
	    hemisphere_cdata[2] = 1;       /* set Sine character */
	    break;

	case 2: /* Upper */
	    hemisphere_cdata[1] = 0xc;     /* set XYZ character */
	    hemisphere_cdata[2] = 1;       /* set Sine character */
	    break;

	case 3: /* Lower */
	    hemisphere_cdata[1] = 0xc;     /* set XYZ character */
	    hemisphere_cdata[2] = 0;       /* set Sine character */
	    break;

	case 4: /* Left */
	    hemisphere_cdata[1] = 6;       /* set XYZ character */
	    hemisphere_cdata[2] = 1;       /* set Sine character */
	    break;

	case 5: /* Right */
	    hemisphere_cdata[1] = 6;       /* set XYZ character */
	    hemisphere_cdata[2] = 0;       /* set Sine character */
	    break;

	case ESC_SEL:
	    return(ESC_SEL);
    }
    /*
	Send the Command
    */
    if (send_serial_cmd(hemisphere_cdata,3) != 3)
	return(FALSE);

    printf("Hemisphere Data Sent to the Bird\n\r");
    hitkeycontinue();

    return(TRUE);
}

/*
    bird_referframe -   Define a new Bird Reference Frame

    Prototype in:       birdcmds.h

    Parameters Passed:  void

    Return Value:       TRUE if successful
			FALSE otherwise

    Remarks:
*/
int bird_referframe()
{
    static unsigned char referframe_cdata[] =
			{'H',0,0,0,0,0,0,0,0,0,0,0,0};  /* the cmd */
    unsigned char * cdataptr = &referframe_cdata[1];
    short i;
    float tempfloat;
    float angle[3];     /* holds the floating point angles */


    /*
	Get the user to input Azimuth, Elevation, and Roll
    */
    if (getangles("\n\rInput Azimuth, Elevation, and Roll angles\n\rof new reference frame: ",&angle[0]) == ESC_SEL)
	return(FALSE);

    /*
	Convert all angles and store in command string
    */
    for(i=0;i<3;i++)
    {
	/*
	    calculate the sine of the angle and
	*/
	tempfloat = sin((double)(angle[i] * DTR));

	/*
	    convert to a word and store in cdata
	    NOTE: trap for sin(90)...since the bird
	    can only accept data from -1.000 to 0.99996 (0x8000 to 0x7fff)
	*/
	if (tempfloat < 0.99998)
	{
	    *cdataptr++ = (unsigned char) ((short) (tempfloat * FTW) & 0x0ff);
	    *cdataptr++ = (unsigned char) (((short) (tempfloat * FTW) & 0x0ff00) >> 8);
	}
	else
	{
	    *cdataptr++ = 0x0ff;
	    *cdataptr++ = 0x07f;
	}

	/*
	    calculate the cosine of the angle and
	*/
	tempfloat = cos((double)(angle[i] * DTR));

	/*
	    convert to a word and store in cdata
	    NOTE: trap for cos(0)...since the bird
	    can only accept data from -1.000 to 0.99996 (0x8000 to 0x7fff)
	*/
	if (tempfloat < 0.99998)
	{
	    *cdataptr++ = (unsigned char) ((short) (tempfloat * FTW) & 0x0ff);
	    *cdataptr++ = (unsigned char) (((short) (tempfloat * FTW) & 0x0ff00) >> 8);
	}
	else
	{
	    *cdataptr++ = 0x0ff;
	    *cdataptr++ = 0x07f;
	}
    }

    /*
	Send the Command
    */
    if (send_serial_cmd(referframe_cdata,13) != 13)
	return(FALSE);

    printf("Reference Frame Sent to the Bird\n\r");
    hitkeycontinue();

    return(TRUE);
}

/*
    bird_reportrate -   Select the Report Rate for Stream Mode

    Prototype in:       birdcmds.h

    Parameters Passed:  void

    Return Value:       TRUE if successful
			FALSE if unsuccessful
			ESC_SEL if user selected ESC

    Remarks:            Queries the User for Bird Report Rate for STREAM
			mode...can be MAX, MAX/2, MAX/8, or MAX/32
*/
int bird_reportrate()
{
    unsigned char rate_cdata;
    static char * reportrate_menuptr[] =
				  {"Select the Report Rate (for STREAM mode):",  /* menu options */
				   "MAX",
				   "MAX/2",
				   "MAX/8",
				   "MAX/32"};
    /*
	Send the Menu to the User
    */
    switch (sendmenu(reportrate_menuptr,4))