////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Simple program to test the mindSensor motor MUX
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#pragma platform(NXT)

typedef enum
{
  mmFloat = 0,
  mmForward = 1,
  mmReverse = 2,
  mmBrake = 3
} TMotorMuxDirections;

const tSensors kSensorMuxPort  = S1;    // Connect motor MUX to this port!!

//////////////////////////////////////////////////////////////////////////////
//
//      Send a control message for a single motor to the motor MUX
//
/////////////////////////////////////////////////////////////////////////////

void mmControl(int nMotor,
               TMotorMuxDirections nDir,
               int nSpeed)
{
	const int kMuxAddress = 0xB4;

	//
  // NOTE: structs are not fully implemented in RobotC, so I'll have to use an
  //       array to hold the message. But we'll define constants indicating the
  //       meaning of the array indices.
  //

  if (false)
  {
	  //
	  // Using an array
	  //
	  byte motorMuxMsg[5];
	  const int kMsgSize        = 0;
	  const int kMuxAddress     = 1;
	  const int kMotorAddress   = 2;
	  const int kMotorDirection = 3;
	  const int kMotorPower     = 4;
	  //
	  // Build the I2C message - send to one motor
	  //
	  motorMuxMsg[kMsgSize]        = 4;
	  motorMuxMsg[kMuxAddress]     = kMuxAddress;
	  motorMuxMsg[kMotorAddress]   = 0x40 + nMotor * 2;
	  motorMuxMsg[kMotorDirection] = nDir;
	  motorMuxMsg[kMotorPower]     = nSpeed;
	  //
	  // Wait for I2C bus to be ready
	  //
	  while (nI2CStatus[kSensorMuxPort] == STAT_COMM_PENDING)
	  {
	    // Not ready!!
	    // We could do more comprehensive error checking here. But this is a simple
	    // test program!
	  }
	  //
	  // Send the message
	  //
	  sendI2CMsg(kSensorMuxPort, motorMuxMsg[0], 0);
	}
	else
	{
		typedef struct
	  {
	  	byte nMsgSize;
	  	byte nAddress;
	  	byte nMotorIndex;
	  	TMotorMuxDirections nMotorDirection;
	  	byte nMotorPower;
	  } TMotorMuxCtrl;

	  TMotorMuxCtrl sStruct;

	  //
	  // Build the I2C message - send to one motor
	  //
	  sStruct.nMsgSize 				= 4;
	  sStruct.nAddress 				= kMuxAddress;
	  sStruct.nMotorIndex 		= 0x40 + nMotor * 2;
	  sStruct.nMotorDirection = nDir;
	  sStruct.nMotorPower 		= nSpeed;
	  //
	  // Wait for I2C bus to be ready
	  //
	  while (nI2CStatus[kSensorMuxPort] == STAT_COMM_PENDING)
	  {
	    // Not ready!!
	    // We could do more comprehensive error checking here. But this is a simple
	    // test program!
	  }
	  //
	  // Send the message
	  //
	  sendI2CMsg(kSensorMuxPort, (byte *) &sStruct, 0);
	}
  return;
}



//////////////////////////////////////////////////////////////////////////////
//
//      Send a control message for a single motor to the motor MUX
//
/////////////////////////////////////////////////////////////////////////////

task main()
{
	int nSpeed;
	const int kMotorOnMux    = 0;
	const int kDelayInterval = 500; // Make this whatever you want to

	//
	// Initialization Code
	//
	SensorType[kSensorMuxPort] = sensorI2CCustomStd;   // or perhaps use 'Lowspeed_9V_Custom0'??

	//
	// Just something to test it out
	//
	while (true)
	{
		//
	  // Ramp up speed - forward
	  //
	  for (nSpeed = 16; nSpeed += 16; nSpeed <= 255)
		{
			mmControl(kMotorOnMux, mmForward, nSpeed);
			wait1Msec(kDelayInterval);
		}
	  //
	  // Ramp down speed - forward
	  //
	  for (nSpeed = 255; nSpeed -= 16; nSpeed > 0)
		{
			mmControl(kMotorOnMux, mmForward, nSpeed);
			wait1Msec(kDelayInterval);
		}
		//
	  // Ramp up speed - reverse
	  //
	  for (nSpeed = 16; nSpeed += 16; nSpeed <= 255)
		{
			mmControl(kMotorOnMux, mmReverse, nSpeed);
			wait1Msec(kDelayInterval);
		}
	  //
	  // Ramp down speed - reverse
	  //
	  for (nSpeed = 255; nSpeed -= 16; nSpeed > 0)
		{
			mmControl(kMotorOnMux, mmReverse, nSpeed);
			wait1Msec(kDelayInterval);
		}
  }
  return;
}