 /*
 *  ltposix.c
 *  lt
 *
 *  Description, routines for tweezers USB communication through FTDI VCP driver
 *  VCP is virtual com port, which makes the USB port read and write functions
 *  accessible from POSIX.
 *
  /*
  *  Shane Saxon has waived all copyright and related or neighboring rights to
  *  "miniTweezers GUI". Published from the United States as Public Domain.
  *  Creative Commons License CC0 1.0 Universal.
  *
  *  Created by Shane Saxon with contributions by Steven Smith, Evan Sperber,
  *  and Josep-Maria Huguet.  Code base started in 2004.
  *
  *  To view the full license with a list of additional authors and to download or
  *  contribute code goto http://tweezerslab.unipr.it/license.html
  *
  */

// Outline:
//
// 1) Initialize serial port
//
// 2) Allocate shared variables
//
// 3) Launch a p-thread, pass in pointer to buffer
//
// P-thread executes a do until gUSBMustDie
// read(24 bytes)
// if buffer-pointer + 24 > end of buffer
// buffer-pointer = start of buffer
// store 24 bytes in buffer
// increment buffer-pointer by 24
//
// Main thread is awakened by a timer every .1 millisecond
// Main thread compares local buffer-pointer to shared pointer
// if shared pointer not equal to buffer-pointer
// local buffer-pointer = shared-pointer
// read 24 bytes from buffer prior to shared pointer
//
// On quit, set gUSBMustDie true
// wait a millisecond for it to die, if it doesn't die, kill it with a stick
// tear down p-thread and deallocate shared memory
//
//=================================================

#include "ltposix.h"

#define kFileListSize 16

static int fileList[kFileListSize];


/*
 *  ioUSBSerial.c
 *  GLCarbonAGLWindow
 *
 /*
 *  Shane Saxon has waived all copyright and related or neighboring rights to
 *  "miniTweezers GUI". Published from the United States as Public Domain.
 *  Creative Commons License CC0 1.0 Universal.
 *
 *  Created by Shane Saxon with contributions by Steven Smith, Evan Sperber,
 *  and Josep-Maria Huguet.  Code base started in 2004.
 *
 *  To view the full license with a list of additional authors and to download or
 *  contribute code goto http://tweezerslab.unipr.it/license.html
 *
 */
 

#include <fcntl.h>
#include <termios.h>
#include <sys/param.h>		//get rid of this by defining MAXPATHLEN
#include <IOKit/IOKitLib.h>
#include <IOKit/serial/IOSerialKeys.h>


// Hold the original termios attributes so we can reset them
static struct termios gOriginalTTYAttrs;

// Returns an iterator across all known modems. Caller is responsible for
// releasing the iterator when iteration is complete.
kern_return_t FindModems(io_iterator_t *matchingServices)
{
    kern_return_t					kernResult; 
    mach_port_t					masterPort;
    CFMutableDictionaryRef		classesToMatch;

/*! @function IOMasterPort
    @abstract Returns the mach port used to initiate communication with IOKit.
    @discussion Functions that don't specify an existing object require the IOKit master port to be passed. This function obtains that port.
    @param bootstrapPort Pass MACH_PORT_NULL for the default.
    @param masterPort The master port is returned.
    @result A kern_return_t error code. */

    kernResult = IOMasterPort(MACH_PORT_NULL, &masterPort);
    if (KERN_SUCCESS != kernResult)
        printf("IOMasterPort returned %d\n", kernResult);
        
/*! @function IOServiceMatching
    @abstract Create a matching dictionary that specifies an IOService class match.
    @discussion A very common matching criteria for IOService is based on its class. IOServiceMatching will create a matching dictionary that specifies any IOService of a class, or its subclasses. The class is specified by C-string name.
    @param name The class name, as a const C-string. Class matching is successful on IOService's of this class or any subclass.
    @result The matching dictionary created, is returned on success, or zero on failure. The dictionary is commonly passed to IOServiceGetMatchingServices or IOServiceAddNotification which will consume a reference, otherwise it should be released with CFRelease by the caller. */

    // Serial devices are instances of class IOSerialBSDClient
    classesToMatch = IOServiceMatching(kIOSerialBSDServiceValue);
    if (classesToMatch == NULL)
        printf("IOServiceMatching returned a NULL dictionary.\n");
    else {
/*!
	@function CFDictionarySetValue
	Sets the value of the key in the dictionary.
	@param theDict The dictionary to which the value is to be set. If this
		parameter is not a valid mutable CFDictionary, the behavior is
		undefined. If the dictionary is a fixed-capacity dictionary and
		it is full before this operation, and the key does not exist in
		the dictionary, the behavior is undefined.
	@param key The key of the value to set into the dictionary. If a key 
		which matches this key is already present in the dictionary, only
		the value is changed ("add if absent, replace if present"). If
		no key matches the given key, the key-value pair is added to the
		dictionary. If added, the key is retained by the dictionary,
		using the retain callback provided
		when the dictionary was created. If the key is not of the sort
		expected by the key retain callback, the behavior is undefined.
	@param value The value to add to or replace into the dictionary. The value
		is retained by the dictionary using the retain callback provided
		when the dictionary was created, and the previous value if any is
		released. If the value is not of the sort expected by the
		retain or release callbacks, the behavior is undefined.
*/
#if 0
        CFDictionarySetValue(classesToMatch,
                            CFSTR(kIOSerialBSDTypeKey),
                            CFSTR(kIOSerialBSDModemType));
#else
        CFDictionarySetValue(classesToMatch,
                            CFSTR(kIOSerialBSDTypeKey),
                            CFSTR(kIOSerialBSDRS232Type));
#endif
        // Each serial device object has a property with key
        // kIOSerialBSDTypeKey and a value that is one of kIOSerialBSDAllTypes,
        // kIOSerialBSDModemType, or kIOSerialBSDRS232Type. You can experiment with the
        // matching by changing the last parameter in the above call to CFDictionarySetValue.
        
        // As shipped, this sample is only interested in modems,
        // so add this property to the CFDictionary we're matching on. 
        // This will find devices that advertise themselves as modems,
        // such as built-in and USB modems. However, this match won't find serial modems.
    }
    
    /*! @function IOServiceGetMatchingServices
        @abstract Look up registered IOService objects that match a matching dictionary.
        @discussion This is the preferred method of finding IOService objects currently registered by IOKit. IOServiceAddNotification can also supply this information and install a notification of new IOServices. The matching information used in the matching dictionary may vary depending on the class of service being looked up.
        @param masterPort The master port obtained from IOMasterPort().
        @param matching A CF dictionary containing matching information, of which one reference is consumed by this function. IOKitLib can contruct matching dictionaries for common criteria with helper functions such as IOServiceMatching, IOOpenFirmwarePathMatching.
        @param existing An iterator handle is returned on success, and should be released by the caller when the iteration is finished.
        @result A kern_return_t error code. */

    kernResult = IOServiceGetMatchingServices(masterPort, classesToMatch, matchingServices);    
    if (KERN_SUCCESS != kernResult)
        printf("IOServiceGetMatchingServices returned %d\n", kernResult);
        
    return kernResult;
}
    
// Given an iterator across a set of modems, return the BSD path to the first one.
// If no modems are found the path name is set to an empty string.
kern_return_t GetModemPath (io_iterator_t serialPortIterator, char *bsdPath, 
	CFIndex maxPathSize, void *dataRef)
{
    io_object_t		modemService;
    kern_return_t	kernResult = KERN_FAILURE;
    Boolean		modemFound = false;
    
	 pData data = (pData) dataRef;
    // Initialize the returned path
    *bsdPath = '\0';
    
/*! @function IOIteratorNext
    @abstract Returns the next object in an iteration.
    @discussion This function returns the next object in an iteration, or zero if no more remain or the iterator is invalid.
    @param iterator An IOKit iterator handle.
    @result If the iterator handle is valid, the next element in the iteration is returned, otherwise zero is returned. The element should be released by the caller when it is finished. */

//    modemService = IOIteratorNext(serialPortIterator);
//    printf("Skipping internal modem\n");											//debug
    
    while ((modemService = IOIteratorNext(serialPortIterator)) && !modemFound)
    {
        CFTypeRef	modemNameAsCFString;
        CFTypeRef	bsdPathAsCFString;
        

/*! @function IORegistryEntryCreateCFProperty
    @abstract Create a CF representation of a registry entry's property.
    @discussion This function creates an instantaneous snapshot of a registry entry property, creating a CF container analogue in the caller's task. Not every object available in the kernel is represented as a CF container; currently OSDictionary, OSArray, OSSet, OSSymbol, OSString, OSData, OSNumber, OSBoolean are created as their CF counterparts. 
    @param entry The registry entry handle whose property to copy.
    @param key A CFString specifying the property name.
    @param allocator The CF allocator to use when creating the CF container.
    @param options No options are currently defined.
    @result A CF container is created and returned the caller on success. The caller should release with CFRelease. */

        modemNameAsCFString = IORegistryEntryCreateCFProperty(modemService,
                                                              CFSTR(kIOTTYDeviceKey),
                                                              kCFAllocatorDefault,
                                                              0);
        if (modemNameAsCFString)
        {
            char modemName[128];
            Boolean result;
            
            result = CFStringGetCString(modemNameAsCFString,
                                        modemName,
                                        sizeof(modemName), 
                                        kCFStringEncodingASCII);
            CFRelease(modemNameAsCFString);
            
            if (result)
            {
					//returns 0 if equal 
				if ( strcmp(modemName, "usbserial-DPCEMDQK") == 0)		//old USB245BM transciever SN# usbserial-FTNFI3JE
						printf("Found TweedleDee: %s \n", modemName);
				modemFound = true;
				kernResult = KERN_SUCCESS;
            }
        }

        bsdPathAsCFString = IORegistryEntryCreateCFProperty(modemService,
                                                            CFSTR(kIOCalloutDeviceKey),
                                                            kCFAllocatorDefault,
                                                            0);
        if (bsdPathAsCFString)
        {
            Boolean result;
            
            result = CFStringGetCString(bsdPathAsCFString,
                                        bsdPath,
                                        maxPathSize, 
                                        kCFStringEncodingASCII);
            CFRelease(bsdPathAsCFString);
            
//            if (result)																	//debug
//                printf("BSD path: %s", bsdPath);
        }

        printf("\n");

/*! @function IOObjectRelease
    @abstract Releases an object handle previously returned by IOKitLib.
    @discussion All objects returned by IOKitLib should be released with this function when access to them is no longer needed. Using the object after it has been released may or may not return an error, depending on how many references the task has to the same object in the kernel.
    @param object The IOKit object to release.
    @result A kern_return_t error code. */
    
        (void) IOObjectRelease(modemService);
        // We have sucked this service dry of information so release it now.
    }
        
    return kernResult;
}

// Given the path to a serial device, open the device and configure it.
// Return the file descriptor associated with the device.
int OpenSerialPort(const char *bsdPath)
{
    int 		fileDescriptor = -1;
    struct termios	options;
    
    fileDescriptor = open(bsdPath, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fileDescriptor == -1)
    {
        printf(" Error opening serial port %s -\n",
               bsdPath);
        goto error;
    }
    
//    printf("Opened serial port %s.\n",bsdPath);

#if 1
    if (fcntl(fileDescriptor, F_SETFL, 0) == -1)
    {
        printf("Error clearing O_NDELAY %s - \n",
            bsdPath);
        goto error;
    }
#endif
    
    // Get the current options and save them for later reset
    if (tcgetattr(fileDescriptor, &gOriginalTTYAttrs) == -1)
    {
        printf("Error getting tty attributes %s - \n",
            bsdPath);
        goto error;
    }

#if 0
    // Set raw input, one second timeout
    // These options are documented in the man page for termios
    // (in Terminal enter: man termios)
    options = gOriginalTTYAttrs;
    options.c_cflag |= (CLOCAL | CREAD);
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;
    options.c_cc[ VMIN ] = 1;
    options.c_cc[ VTIME ] = 0;
    
    /* XON-XOFF */
    options.c_cflag &= ~CRTSCTS;
    options.c_iflag |= (IXON | IXOFF | IXANY);
    
    // Set the options
    if (tcsetattr(fileDescriptor, TCSANOW, &options) == -1)
    {
        printf("Error setting tty attributes %s - %s(%d).\n",
            bsdPath, strerror(errno), errno);
        goto error;
    }

    sleep(5);
#endif

#if 1															
    // Set raw input, one second timeout
    // These options are documented in the man page for termios
    // (in Terminal enter: man termios)
    options = gOriginalTTYAttrs;
    options.c_cflag |= (CLOCAL | CREAD);
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;
    options.c_cc[ VMIN ] = 1;
    options.c_cc[ VTIME ] = 0;
    
    /* RTS-CTS */
    options.c_cflag |= CRTSCTS;
    options.c_iflag &= ~(IXON | IXOFF | IXANY);
    
    cfsetospeed(&options,B115200);
    cfsetispeed(&options,B115200);
    
    
    // Set the options
    if (tcsetattr(fileDescriptor, TCSANOW, &options) == -1)
    {
        printf("Error setting tty attributes %s - \n", bsdPath);
        goto error;
    }
#endif

#if 0
    sleep(5);

    // Set raw input, one second timeout
    // These options are documented in the man page for termios
    // (in Terminal enter: man termios)
    options = gOriginalTTYAttrs;
    options.c_cflag |= (CLOCAL | CREAD);
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;
    options.c_cc[ VMIN ] = 1;
    options.c_cc[ VTIME ] = 0;
    
    /* NONE */
    options.c_cflag &= ~CRTSCTS;
    options.c_iflag &= ~(IXON | IXOFF | IXANY);
    
    // Set the options
    if (tcsetattr(fileDescriptor, TCSANOW, &options) == -1)
    {
        printf("Error setting tty attributes %s - %s(%d).\n",
            bsdPath, strerror(errno), errno);
        goto error;
    }
#endif

    // Success
    return fileDescriptor;
    
    // Failure path
error:
    if (fileDescriptor != -1)
        close(fileDescriptor);
    return -1;
}


// Given the file descriptor for a serial device, close that device.
void CloseSerialPort(int fileDescriptor)
{
    printf("Closing serial port %x.\n", fileDescriptor);

    close(fileDescriptor);
}


//-------------------------------------------------------------------
int InitUSBSerial(int *fileDescriptor, void *data)
{
	int err = 0;
	
	kern_return_t	kernResult; // on PowerPC this is an int (4 bytes)
/*
 *	error number layout as follows (see mach/error.h):
 *
 *	hi		 		       lo
 *	| system(6) | subsystem(12) | code(14) |
 */

    io_iterator_t	serialPortIterator;
    char		bsdPath[ MAXPATHLEN ];
 
    kernResult = FindModems (&serialPortIterator);
    kernResult = GetModemPath (serialPortIterator, bsdPath, sizeof(bsdPath), data);
    IOObjectRelease (serialPortIterator);	// Release the iterator.

    
    // Now open the modem port we found, initialize the modem then close it
    if (!bsdPath[0]) {
        printf("No modem port found.\n");
        return -1; //EX_UNAVAILABLE;		//debug
    }

    *fileDescriptor = OpenSerialPort(bsdPath);
    if (-1 == *fileDescriptor)
        return -1; //EX_IOERR;				//debug

	//	fcntl(fileDescriptor, F_SETFL, FNDELAY);	//debug

    return err;
}


///////////////////////////////////////////////
void OpenUSBSerial (int *fileDescriptor, void *dataRef)
{
	int err = 0, i, fileListIndex = 0;
	
	pData data = (pData) dataRef;
	
	err = InitUSBSerial(fileDescriptor, data);
	
	if (err != 0)
		*fileDescriptor = -1;
	
	for (i = 0; i < kFileListSize; i++)
		if (fileList[i] != -1)
			fileListIndex = i;	
			
	if (fileList[fileListIndex] != -1)		//need error handling, debug
		err = 1021;
	else
		fileList[fileListIndex] = *fileDescriptor;
}


//////////////////////////////////////
//updates the data in custom channels such as Net Force and others
void MakeCustomChannels (void* dataRef)
{
	pData data = (pData) dataRef;
	static XYZfloat delayedMotorVelocity;	//JMH
	static int dither = 999;		//JMH
	static int oldMotorPositionX;	//JMH
	static int oldMotorPositionY;	//JMH
	static int oldMotorPositionZ;	//JMH
	static XYZinteger oldMotorTime;
	static int previousCycleCount;	//SBS
	static int cycleCount = 0;		//SBS
	static int rolloverCount = 0;	//SBS
	int recentIndex = data->quicktime.recentIndex;

	cycleCount = data->quicktime.recent[recentIndex][kChannelCycleCount];
		
	//AD 590 temperature sensor outputs signal proportional to absolute Kelvin
	data->quicktime.recent[recentIndex][kChannelTrapATemp] =
		(float)(data->quicktime.wordFromTrapA[temperature] - 32768) 
		* kTempSensitivity - 273.0 + data->instrument.trapATempFudge;
		
	data->quicktime.recent[recentIndex][kChannelTrapBTemp] =
		(float)(data->quicktime.wordFromTrapB[temperature] - 32768) 
		* kTempSensitivity - 273.0 + data->instrument.trapBTempFudge;
	
	data->quicktime.recent[recentIndex][kChannelNetForceX] =
		data->quicktime.recent[recentIndex][kChannelTrapAPsdX]
		* data->instrument.trapAForceSensitivity.x
		+ data->quicktime.recent[recentIndex][kChannelTrapBPsdX]
		* data->instrument.trapBForceSensitivity.x
		+ data->instrument.forceOffset.x;
	
	data->quicktime.recent[recentIndex][kChannelNetForceY] =
		data->quicktime.recent[recentIndex][kChannelTrapAPsdY]
		* data->instrument.trapAForceSensitivity.y
		+ data->quicktime.recent[recentIndex][kChannelTrapBPsdY]
		* data->instrument.trapBForceSensitivity.y
		+ data->instrument.forceOffset.y;
		
	data->quicktime.recent[recentIndex][kChannelNetForceZ] =
		( data->quicktime.recent[recentIndex][kChannelTrapBIris] / data->quicktime.recent[recentIndex][kChannelTrapBPsdSum]
			* data->instrument.psdSumFullB * data->instrument.trapBForceSensitivity.z )
	-	( data->quicktime.recent[recentIndex][kChannelTrapAIris] / data->quicktime.recent[recentIndex][kChannelTrapAPsdSum]
			* data->instrument.psdSumFullA * data->instrument.trapAForceSensitivity.z )
	+	data->instrument.forceOffset.z;
	
/*	data->quicktime.recent[recentIndex][kChannelNetForceZ] =
		( data->quicktime.recent[recentIndex][kChannelTrapBIris] * data->instrument.trapBForceSensitivity.z )
	-	( data->quicktime.recent[recentIndex][kChannelTrapAIris] * data->instrument.trapAForceSensitivity.z )
	+	data->instrument.forceOffset.z;
*/		
	data->quicktime.recent[recentIndex][kChannelTension] = sqrtf (
		data->quicktime.recent[recentIndex][kChannelNetForceX] 
		* data->quicktime.recent[recentIndex][kChannelNetForceX]
		+ data->quicktime.recent[recentIndex][kChannelNetForceY]
		* data->quicktime.recent[recentIndex][kChannelNetForceY]
		+ data->quicktime.recent[recentIndex][kChannelNetForceZ]
		* data->quicktime.recent[recentIndex][kChannelNetForceZ]	);
		
	data->quicktime.recent[recentIndex][kChannelTrapANormLeverX] =
		( data->quicktime.recent[recentIndex][kChannelTrapALeverX]
			/ data->quicktime.recent[recentIndex][kChannelTrapALeverSum] )
		* data->instrument.trapALeverSensitivity.x
		+ data->instrument.trapALeverOffset.x;
		
	data->quicktime.recent[recentIndex][kChannelTrapANormLeverY] =
		( data->quicktime.recent[recentIndex][kChannelTrapALeverY]
			/ data->quicktime.recent[recentIndex][kChannelTrapALeverSum] )
		* data->instrument.trapALeverSensitivity.y
		+ data->instrument.trapALeverOffset.y;
	
	data->quicktime.recent[recentIndex][kChannelTrapBNormLeverX] =
		( data->quicktime.recent[recentIndex][kChannelTrapBLeverX]
			/ data->quicktime.recent[recentIndex][kChannelTrapBLeverSum] )
		* data->instrument.trapBLeverSensitivity.x
		+ data->instrument.trapBLeverOffset.x;
		
	data->quicktime.recent[recentIndex][kChannelTrapBNormLeverY] =
		( data->quicktime.recent[recentIndex][kChannelTrapBLeverY]
			/ data->quicktime.recent[recentIndex][kChannelTrapBLeverSum] )
		* data->instrument.trapBLeverSensitivity.y
		+ data->instrument.trapBLeverOffset.y;
		
	//offset (middle of 24bit number is 8388608) and convert to um
	//converts quadrature count to nm for the Thorlabs Z606 motor
	//negative flips the axis
	data->quicktime.recent[recentIndex][kChannelMotorX] = 
		(float)(data->quicktime.motorPosition.x - 8388608) * -0.04069;
	data->quicktime.recent[recentIndex][kChannelMotorY] = 
		(float)(data->quicktime.motorPosition.y - 8388608) * -0.04069;
	data->quicktime.recent[recentIndex][kChannelMotorZ] = 
		(float)(data->quicktime.motorPosition.z - 8388608) * -0.04069;
		
	
	dither--;
	if(dither == 500)			// trying to make zero velocity return some current readings
		{								// despite fact that motorPosition never changes
		oldMotorPositionX += 1;
		oldMotorPositionY += 1;
		oldMotorPositionZ += 1;
		}
	else if(dither == 0)
		{
		oldMotorPositionX -= 1;
		oldMotorPositionY -= 1;
		oldMotorPositionZ -= 1;
		dither = 999;
		}

	if(data->quicktime.motorPosition.x != oldMotorPositionX)
		{
		delayedMotorVelocity.x = data->quicktime.recent[recentIndex][kChannelMotorVelocityX] =
			(float)(data->quicktime.motorPosition.x - oldMotorPositionX) * 0.04069 /
			((float)(cycleCount - oldMotorTime.x)) * data->quicktime.mainFrequency;
		oldMotorPositionX = data->quicktime.motorPosition.x;
		oldMotorTime.x = cycleCount;
		}
	else
		data->quicktime.recent[recentIndex][kChannelMotorVelocityX] = delayedMotorVelocity.x;

	if(data->quicktime.motorPosition.y != oldMotorPositionY)
		{
		delayedMotorVelocity.y = data->quicktime.recent[recentIndex][kChannelMotorVelocityY] =
			(float)(data->quicktime.motorPosition.y - oldMotorPositionY) * 0.04069 /
			((float)(cycleCount - oldMotorTime.y)) * data->quicktime.mainFrequency;
		oldMotorPositionY = data->quicktime.motorPosition.y;
		oldMotorTime.y = cycleCount;
		}
	else	
		data->quicktime.recent[recentIndex][kChannelMotorVelocityY] = delayedMotorVelocity.y;


	if(data->quicktime.motorPosition.z != oldMotorPositionZ)
		{
		delayedMotorVelocity.z = data->quicktime.recent[recentIndex][kChannelMotorVelocityZ] =
			(float)(data->quicktime.motorPosition.z - oldMotorPositionZ) * 0.04069 /
			((float)(cycleCount - oldMotorTime.z)) * data->quicktime.mainFrequency;
			oldMotorPositionZ = data->quicktime.motorPosition.z;
			oldMotorTime.z = cycleCount;
		}
	else	
		data->quicktime.recent[recentIndex][kChannelMotorVelocityZ] = delayedMotorVelocity.z;
		
	if (cycleCount < previousCycleCount) 	
		rolloverCount++;		// the 16-bit main PIC cycle count has rolled over
	previousCycleCount = cycleCount;

	data->quicktime.recent[recentIndex][kChannelPICtime] = 
				(float)	( 65366 * rolloverCount + cycleCount ) / data->quicktime.mainFrequency;
	
	data->quicktime.recent[recentIndex][kChannelTweezStatus] =	
		(float) (data->instrument.runMode     * 1000 
		+ data->quicktime.byteFromTrapA[mode] * 100		// middle 2 digits encode feedback modes for trapA and trapB
		+ data->quicktime.byteFromTrapB[mode] * 10
		+ data->instrument.pausing		      * 4
		+ data->instrument.stiffTestHighA     * 2 
		+ data->instrument.movingUpward       * 1  );

	
/*	data->quicktime.recent[recentIndex][kChannelTweezStatus] =		// use this version when recording power of heating laser
		(float) (data->instrument.runMode		* 1000 
			 + (data->quicktime.switches / 2)   * 10	// middle 2 digits encode switch positions connected to PORTB of Motor_Control card
			 + data->instrument.pausing		    * 4
			 + data->instrument.stiffTestHighA  * 2 
			 + data->instrument.movingUpward    * 1  );
 */
}	

///////////////////////////////////////////////
int InstrumentUpdate(void* dataRef)
{
 	static int readIndex = 0, tempDebug;
	static int lastCount = 0, tempCount = 0, checkCount = 0;
	int err = 0, packetSize = 62;
	int i, j, numBytes, readIndexEnd, loByte;
	unsigned char checkSum;
	pData data = (pData) dataRef;
	
	
	if (readIndex >= data->instrument.readBufferEndIndex)
		readIndex -= data->instrument.readBufferEndIndex;  //this is for readBuffer looping
	
	if((readIndex > (data->instrument.readBufferIndex - kHostPacketID)) &
			  readIndex < data->instrument.readBufferIndex)
		return err;		//nothing to transfer, exit
	
	if (readIndex > data->instrument.readBufferIndex)
	{
		readIndexEnd = data->instrument.readBufferEndIndex;
		for ( i = 0; i < packetSize; i++)
		  data->instrument.readBuffer[data->instrument.readBufferEndIndex + i + 1] =
				data->instrument.readBuffer[i];  //copy a packet worth to end reading the rollover
	}
	else
		readIndexEnd = data->instrument.readBufferIndex;	//read up to current
 
	while (readIndex < readIndexEnd)
	{
		if(data->instrument.readBuffer[readIndex++] == kHostPacketID)
		{
			if(data->instrument.readBuffer[readIndex++] == kHostPacketID)	//we have two packetID's, check to see if packet header
			{
				checkSum = 84;
				j = packetSize - 3;  //the three is for the header and checksum
				for (i = 0; i < j; i++)
					checkSum += data->instrument.readBuffer[readIndex + i];
				
				//verify checksum and both cycleCounts, if true we have a good packet
				// j-4, j-3 are the first two bytes of the cyclecount at end of packet
				if ((  checkSum == data->instrument.readBuffer[readIndex + j]	
						&&	data->instrument.readBuffer[readIndex + j - 4] 
						== data->instrument.readBuffer[readIndex]	
						&& data->instrument.readBuffer[readIndex + j - 3] 
						== data->instrument.readBuffer[readIndex + 1]		)
						&& (	( ( data->instrument.readBuffer[readIndex + 1] * 256
						+ data->instrument.readBuffer[readIndex] ) 
						> lastCount )
						|| ( ( data->instrument.readBuffer[readIndex + 1] * 256 
						+ data->instrument.readBuffer[readIndex] ) < 25 ) 
					))
				{
							
					lastCount = data->instrument.readBuffer[readIndex + 1] * 256 +
									data->instrument.readBuffer[readIndex];
										
					data->quicktime.recentIndex++;			//multithreading conflict consider MakeCustomChannels, debug
						
					if(data->quicktime.recentIndex >= kRecentSize)		//this roll overs our current buffer
						data->quicktime.recentIndex = 0;

					data->quicktime.recent[data->quicktime.recentIndex][kChannelCycleCount] =	
						data->instrument.readBuffer[readIndex + 55]
						+ data->instrument.readBuffer[readIndex + 56] * 256;
						+ data->instrument.readBuffer[readIndex + 57] * 256 * 256;					//debug tarzan
						+ data->instrument.readBuffer[readIndex + 58] * 256 * 256 * 256;
					
					readIndex += 2;
							
					// copy trapA channels
					// 7 AD channels per trap (channels 1-7)
					for (i = 1; i < 8; i++)
						{
							loByte = data->instrument.readBuffer[readIndex++];
							data->quicktime.recent[data->quicktime.recentIndex][i] =	
							(loByte + (data->instrument.readBuffer[readIndex++] * 256)) - 32768;  //256 offsets the high byte
						}

					//err, cyclecount, byte and word parameters from trapA			
					data->quicktime.byteFromTrapA[data->instrument.readBuffer[readIndex]] = 
							data->instrument.readBuffer[readIndex + 1];
					readIndex += 2;
							
					data->quicktime.wordFromTrapA[data->instrument.readBuffer[readIndex]] = 
							data->instrument.readBuffer[readIndex + 1] +
							data->instrument.readBuffer[readIndex + 2] * 256;
					readIndex += 3;
					
					data->quicktime.recent[data->quicktime.recentIndex][kChannelErrorTrapA] =
							data->instrument.readBuffer[readIndex++];
					
					data->quicktime.recent[data->quicktime.recentIndex][kChannelTrapACycleCount] =
							data->instrument.readBuffer[readIndex++];

					// copy trapB channels
					// 7 AD channels per trap
					for (i = 8; i < 15; i++)
						{
							loByte = data->instrument.readBuffer[readIndex++];
							data->quicktime.recent[data->quicktime.recentIndex][i] =	
							(loByte + (data->instrument.readBuffer[readIndex++] * 256)) - 32768;  //256 offsets the high byte
						}
						
					//err, cyclecount, byte and word parameters from trapB
					data->quicktime.byteFromTrapB[data->instrument.readBuffer[readIndex]] = 
							data->instrument.readBuffer[readIndex + 1];
					readIndex += 2;
							
					data->quicktime.wordFromTrapB[data->instrument.readBuffer[readIndex]] = 
							data->instrument.readBuffer[readIndex + 1] +
							data->instrument.readBuffer[readIndex + 2] * 256;
					readIndex += 3;	
					
					data->quicktime.recent[data->quicktime.recentIndex][kChannelErrorTrapB] =
							data->instrument.readBuffer[readIndex++];
					
					data->quicktime.recent[data->quicktime.recentIndex][kChannelTrapBCycleCount] =
							data->instrument.readBuffer[readIndex++];
					
					//process motor data
					switch (data->instrument.readBuffer[readIndex++])	//transmitID
					{
						case kMotorX:
							data->quicktime.motorPosition.x = 
								data->instrument.readBuffer[readIndex] 
								+ data->instrument.readBuffer[readIndex + 1] * 256 
								+ data->instrument.readBuffer[readIndex + 2] * 65536;
							readIndex += 3;
							break;
						case kMotorY:
							data->quicktime.motorPosition.y = 
								data->instrument.readBuffer[readIndex] 
								+ data->instrument.readBuffer[readIndex + 1] * 256 
								+ data->instrument.readBuffer[readIndex + 2] * 65536;
							readIndex += 3;
							break;
						case kMotorZ:
							data->quicktime.motorPosition.z = 
								data->instrument.readBuffer[readIndex] 
								+ data->instrument.readBuffer[readIndex + 1] * 256 
								+ data->instrument.readBuffer[readIndex + 2] * 65536;
							readIndex += 3;
							break;
						case kMotorLimits:
							data->quicktime.motorLimit = 
								data->instrument.readBuffer[readIndex];
							data->quicktime.switches =
								data->instrument.readBuffer[readIndex + 1];	
							// reports bits 0 thru 4 of PORTB where bit-0 is blueLED and bits 1-4 are switches on heating-laser
							// printf ("switches = %d.\n",data->quicktime.switches);
							data->quicktime.motorsBusy =
								data->instrument.readBuffer[readIndex + 2];
							readIndex += 3;
							break;					}
		
					data->quicktime.recent[data->quicktime.recentIndex][kChannelMotorCycleCount] =
							data->instrument.readBuffer[readIndex++];

					//comPIC data
					readIndex += 5;								//get comPIC settings here, debug
					data->quicktime.recent[data->quicktime.recentIndex][kChannelErrorComPIC] =
							data->instrument.readBuffer[readIndex++];
											
					//used for Net Force and others		
					MakeCustomChannels (data);	
	
				}
				else
				{
					if ( tempCount + 1 ==  ( data->instrument.readBuffer[readIndex + 1] * 256
										  + data->instrument.readBuffer[readIndex] ) )
					{
						if ( checkCount++ >= 5)
							lastCount = ( data->instrument.readBuffer[readIndex + 1] * 256
										  + data->instrument.readBuffer[readIndex] );
					}
					else
						checkCount = 0;
						
					tempCount = ( data->instrument.readBuffer[readIndex + 1] * 256
										+ data->instrument.readBuffer[readIndex] );
					//printf( " bad checkSum %d ", readIndex); //need error handling here, debug
					readIndex--;
				}
			}
		}
	}

	for (i = 0; i < kChannelCount; i++)
	{
		data->quicktime.latest[i] = 
						data->quicktime.recent[data->quicktime.recentIndex][i];
	
		data->quicktime.smoothed[i] =
		( data->instrument.smoothingNum * data->quicktime.smoothed[i]
		  + data->quicktime.latest[i] )
		/ (data->instrument.smoothingNum + 1);
		if (data->quicktime.smoothed[i] >  1e10) data->quicktime.smoothed[i] = data->quicktime.latest[i];
		if (data->quicktime.smoothed[i] < -1e10) data->quicktime.smoothed[i] = data->quicktime.latest[i];
	}

	return err;
}


//////////////////////////////////////////////
//called from the POSIX InstrumentTask function, should it be its own thread?  debug
void UpdateCommandBuffer (void* dataRef)
{
	int i, j;
	
	pData data = (pData) dataRef;
	
	//exit if still sending the last command
	if	(data->instrument.sendCommand)
		return;
	
	//exit if no commands to send
	if (data->instrument.commandBufferIndex == 
		 data->instrument.commandBufferSentIndex)
		return;
	
	//increment the index and check for buffer roll over
	data->instrument.commandBufferSentIndex++;
	
	if (data->instrument.commandBufferSentIndex >= kCommandBufferSize)
		data->instrument.commandBufferSentIndex = 0;
	
	i = data->instrument.commandBufferSentIndex;
	
	for (j = 0; j < kWritePacketSize; j++)
		data->instrument.writeBuffer[j] = 
			data->instrument.commandBuffer[i][j];
	
	data->instrument.sendCommand = 4;
}


///////////////////////////////////////////////
int InstrumentTask(void* dataRef)
{
	int err = 0;
	int i, checkSum = 0;
	int threadID;
	int fileDescriptor;
	int numBytes;
	pData data = (pData) dataRef;
	unsigned char blankPacket[kWritePacketSize];		//sends start streaming
	
	for (i = 0; i < kWritePacketSize; i++)
		blankPacket[i] = 0;
	
	blankPacket[0] = kHostPacketID;
	blankPacket[1] = kHostPacketID;

	for ( i = 0; i < kWritePacketSize - 1; i++)			//calculates checksum
		checkSum += blankPacket[i];

	blankPacket[kWritePacketSize - 1] = checkSum;
	
	threadID = ControlThreadCurrentID();
	ControlThreadLockCurrentID ( 0 );								//unlock currentID

	OpenUSBSerial(&fileDescriptor, data);

	if (fileDescriptor == -1)
		return -1;										//error, OpenUSBSerial()
	
	err = ControlTimerUpdate(InstrumentUpdate, data);
	if( err != 0 ) 
		return err;	

	while (!ControlThreadKillThread(threadID)) 
	{
		UpdateCommandBuffer (data);
		
		if (data->instrument.sendCommand)			//could send torn packet, debug
		{	
			data->instrument.sendCommand--;
			numBytes = write (fileDescriptor, 
								   data->instrument.writeBuffer, 
								   kWritePacketSize);
		}
		else												//write zeros when all commands sent
			numBytes = write (fileDescriptor, 
								   blankPacket, 
								   kWritePacketSize);

//		if (numBytes == -1)
//			printf("Error writing to USB \n");
				
		//tweezers USB communication using POSIX read and write
		numBytes = read (fileDescriptor, 
			(data->instrument.readBuffer + data->instrument.readBufferIndex),
			 3096);
			 
		//if (numBytes <= 0) 
		//	printf("Error reading from USB \n");
		//else
		if (numBytes > 0)
		{
			data->instrument.readBufferIndex += numBytes; //update buffer index

			if (data->instrument.readBufferIndex >= (kInstrumentReadBufferSize - 3096))
			{
				data->instrument.readBufferEndIndex = data->instrument.readBufferIndex;
				data->instrument.readBufferIndex = 0;	//roll over the index to zero to loop
			}
		}
	}

	return 0;
}


////////////////////////////////////////////////
void InitPosix (void* data)
{
	int i;
	
	for (i = 0; i < kFileListSize; i++)
		fileList[i] = -1;								//needs to be -1 for fileDescriptor compatibility 
}


//////////////////////////////////////////////
void ClosePosix(void* data)
{
	int i;
	
	//closes all serial ports including our tweezers USB, VCP driver
	for (i = 0; i < kFileListSize; i++)
	{
		if (fileList[i] != -1)
		{
			CloseSerialPort(fileList[i]);			//closes all ports in list
			fileList[i] = -1;
		}
	}
}