///////////////////////////////////////////////////////////////////////////////
//
//                    Three US sensor Ping Driver
//
//
// This device driver is a modified version of the sample from RobotC 
//  Written by Dick Swan and Modified (for 3 sensors) by Steve Hassenplug
//
//  It may not be the best method, but it works...
//
//
///////////////////////////////////////////////////////////////////////////////



///////////////////////////////////////////////////////////////////////////////
// 
// The following line should be included in the declaration section of the program
// it tells the program to include the device driver
//
// #include "NXT 3x Ultrasonic Driver.c"  
//
//
// The following line should be near the start of the main program.  It starts the driver.
//
//	StartTask(UltraSonic3xDriver);
//
// This driver will return two values (that are the same).
// SensorValue[S1], where S1 is the sensor port
// USDistance[n], where n is the number of the of the port (ex 1,2,3)
//
//
///////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////
// this section defines which sensors are used.  It can be changed to any 
// combination of ports
///////////////////////////////////////////////////////////////////////////////


const int TotalUsSensors = 3;
tSensors SensorPort[TotalUsSensors] = {S1,S2,S3};

bool ValidSensor[TotalUsSensors];
int USDistance[TotalUsSensors];

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


task UltraSonic3xDriver()
{
  //
  // Message constants for Lego Sonar (Ultra Sonic) sensor
  //
  const ubyte kSonarID               = 2;
  const ubyte kContinuousMeasurement = 2;
  const ubyte kSingleShot			 = 1;
  const ubyte kInitializeSonar       = 0x41;
  const ubyte kReadSonarResult       = 0x42;

  //
  // I2C Message definition for SONAR sensor
  //
//  static const byte kSonarInitialize[] = {3, kSonarID, kInitializeSonar, kContinuousMeasurement};
  static const byte kSonarInitializeSingle[] = {3, kSonarID, kInitializeSonar, kSingleShot};

  static const byte kSonarRead[]       = {2, kSonarID, kReadSonarResult};
	const int kSonarReplySize     = 1;
	const int kOutOfRangeSonar    = 255; // Value returned when SONAR sensor was out of range
	static int nCurrentSensor = 0;

  //
  //  state machine for SONAR "device driver"
  //
  typedef enum
  {
  	stateInitializing,
  	stateWaitForInitDone,
  	stateSendPollMessage,
  	stateWaitingForReply,
  	stateError,
  } TSonarState;

  static TSonarState nUSState[TotalUsSensors];

  //
  // Set up port 'kTestPort' as user defined I2C sensor
  //

  for(nCurrentSensor = 0; nCurrentSensor < TotalUsSensors; ++nCurrentSensor)
  {
	nUSState[nCurrentSensor] = stateInitializing;
	SensorType[SensorPort[nCurrentSensor]] = sensorI2CCustomStd9V;
	ValidSensor[nCurrentSensor] = false;
	USDistance[nCurrentSensor] = 255;

  }

  nCurrentSensor = 0;


  while(true)
  {
  	//
  	// Loop forever, polling the SONAR sensor
  	//
  	switch (nUSState[nCurrentSensor])
	  {
	  case stateInitializing:
	  	//
	  	// Send initialize message to Sonar sensor
	  	//

	  	sendI2CMsg(SensorPort[nCurrentSensor], kSonarInitializeSingle[0], 0);
	  	nUSState[nCurrentSensor] = stateWaitForInitDone;
	  	break;

	  case stateWaitForInitDone:
	  	//
	  	// Wait for Sonar sensor initialization to complete
	  	//
	  	switch (nI2CStatus[SensorPort[nCurrentSensor]])
	  	{
  		case NO_ERR:
		  	nUSState[nCurrentSensor] = stateSendPollMessage;
		  	break;

  		case STAT_COMM_PENDING:
  			// Keep waiting for reply. I2C messaging is not complete
  			wait1Msec(1);
  			break;

  		default:
  		case ERR_COMM_BUS_ERR:
		  	// re-initialize sensor. An I2C messaging error occurred.
		  	nUSState[nCurrentSensor] = stateError;
		  	break;
			}
			break;

	  case stateSendPollMessage:
	  	//
	  	// Ready to send next polling message to Sensor
	  	//
	  	nI2CBytesReady[SensorPort[nCurrentSensor]] = 0; // Clear any pending bytes
	  	sendI2CMsg(SensorPort[nCurrentSensor], kSonarRead[0], kSonarReplySize);
	  	nUSState[nCurrentSensor] = stateWaitingForReply;
	  	break;


	  case stateWaitingForReply:
	  	//
	  	// Wait for reply from SONAR sensor
	  	//
	  	switch (nI2CStatus[SensorPort[nCurrentSensor]])
	  	{
  		case NO_ERR:
		  	// Reply from SONAR sensor is ready to be read.
		  	//    - Update sensor value
		  	//    - Setup to send next polling message.

		  	byte replyMsg[1];

		  	//
		  	// Read the SONAR value and store it in "SensorValue" array so that it can be used
		  	// by other tasks as a standard sensor.
		  	//
		  	readI2CReply(SensorPort[nCurrentSensor], replyMsg[0], kSonarReplySize);
//		  	if (replyMsg[0] != kOutOfRangeSonar)
//		  	{
				USDistance[nCurrentSensor] = replyMsg[0] & 0xff;
		  	  SensorValue[SensorPort[nCurrentSensor]] = USDistance[nCurrentSensor];
//		    }



		  	nUSState[nCurrentSensor] = stateSendPollMessage;
			ValidSensor[nCurrentSensor] = true;

		  	// look at next sensor
		  	if (++nCurrentSensor>=TotalUsSensors)
		  		nCurrentSensor = 0;

		  	nUSState[nCurrentSensor] = stateInitializing;
		  	break;

  		case STAT_COMM_PENDING:
  			// Insert short wait for reply. I2C messaging xmit/rcv is not complete
  			wait1Msec(1);
  			break;

  		default:
  		case ERR_COMM_BUS_ERR:
		  	// re-initialize sensor. An I2C messaging error occurred.
		  	nUSState[nCurrentSensor] = stateError;
		  	break;
			}
		break;
	  case stateError:
	  	// reading is not currently valid...
//		ValidSensor[nCurrentSensor] = false;
	  	nUSState[nCurrentSensor] = stateInitializing;
		// re initalize
		  	// look at next sensor
	  	if (++nCurrentSensor>=TotalUsSensors)
	  		nCurrentSensor = 0;
		break;
		}
	}
	return;
}