//*!!Sensor,    S4,                    t, sensorTouch,                           !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

//
// Simple program to test the mindSensor motor MUX
//
typedef enum
{
  mmFloat = 0,
  mmForward = 1,
  mmReverse = 2,
  mmBrake = 3
} TMotorMuxDirections;

const tSensors kSensorMuxPort  = S1;    // Connect motor MUX to this port!!
const tSensors lightSensor = (tSensors) S2;
const tSensors touchSensor = (tSensors) S4;


int nSpeed;
const int kMotorOnMux = 0;	//motormux motor connection Port number 0 = port #1
const int kDelayInterval = 500; // Make this whatever you want to
int detectDominoLight = 0;





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

void mmControl(int nMotor,
               TMotorMuxDirections nDir,
               int nSpeed)
{
  int motorMuxMsg[5];
  const int kMsgSize        = 0;
  const int kMuxAddress     = 1;
  const int kMotorAddress   = 2;
  const int kMotorDirection = 3;
  const int kMotorPower     = 4;

  //
  // 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.
  //

  //
  // Build the I2C message
  //
  motorMuxMsg[kMsgSize]        = 4;
  motorMuxMsg[kMuxAddress]     = 0xB4;
  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);
  return;
}



void raiseTower()
//Raise the front tower unit
{
  while(nMotorEncoder[motorB] > -325)   //as the tower raises, the encoder goes negative. Wait till it hits -330
	{
	 motor[motorB] = -80;			//80% up
	}
  motor[motorB] = 0;			//Off
}


void lowerTower()
{
	//move domino placer back to bottom - wait for touch = 1
  while(SensorValue(touchSensor) == 0)
	{
	 motor[motorB] = 80;			//80% down
	}
   motor[motorB] = 0;			//Off

   nMotorEncoder[motorB] = 0; //motor encoder initialized back to 0


}

void moveDegree(int rotate, int speedPCT)
{
	//move robot forward or backwards based on passed in rotation value and speed

	if (rotate <= 0)
	{
    //negative rotate value passed in
	  while(nMotorEncoder[motorA] > rotate)
	  {
	    motor[motorA] = speedPCT;			//% fwd
	    motor[motorC] = speedPCT;			//% fwd
	  }
	   motor[motorA] = 0;			//Off
     motor[motorC] = 0;			//Off

  }else{

    //positive rotate value passed in
	  while(nMotorEncoder[motorA] < rotate)
	  {
	    motor[motorA] = speedPCT;			//% fwd
	    motor[motorC] = speedPCT;			//% fwd
	  }
	   motor[motorA] = 0;			//Off
     motor[motorC] = 0;			//Off
  }

	nMotorEncoder[motorA] = 0;		//motor encoder initialized back to 0
	nMotorEncoder[motorC] = 0;		//motor encoder initialized back to 0
}


void calibrateLight()
{
	int j = 0;
	int lightVal;


	while (j < 50)
	{
		eraseDisplay();
		lightVal = SensorValue(lightSensor);
		nxtDisplayTextLine(2,"Actual Light: %d",lightVal);

    detectDominoLight = detectDominoLight + lightVal;
    wait1Msec(200);	  //wait a bit between each reading
    PlaySound(soundBlip);
	  j++;
   }
	detectDominoLight = detectDominoLight / 50;

	nxtDisplayTextLine(5,"Light: %d",detectDominoLight);
	PlaySound(soundLast);

	//calibrateLight will return a light value that is the avg of the outside ambient light. Based on this
	//add x % to it to have it flag a domino as being dispense. Test readings show that a domino will bring the value
	//up by 10% more than the avg readng
	detectDominoLight = detectDominoLight + 10;
	//wait10Msec(1000);
}


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

task main()
{
  // Initialization Code
	//


  nSpeed = 100;
  SetSensorType(lightSensor, sensorLightActive);
  nMotorEncoder[motorA] = 0;		//motor encoder initialized to 0
  nMotorEncoder[motorB] = 0;		//motor encoder initialized to 0
  nMotorEncoder[motorC] = 0;		//motor encoder initialized to 0
	SensorType[kSensorMuxPort] = Lowspeed_Custom0;   // or perhaps use 'Lowspeed_9V_Custom0'??

	//
	// Just something to test it out
	//


	 calibrateLight();






 int i =0;

  while (i < 13)
  {


  while(SensorValue(lightSensor) < detectDominoLight)		//loops until light intensity is greater than or equal to xx%
	{
   mmControl(kMotorOnMux, mmForward, nSpeed);
	}
	wait10Msec(20);	  //wait a bit to ensure that domino falls
	//backup domino placer a bit
	mmControl(kMotorOnMux, mmReverse, nSpeed);
	wait1Msec(500);	  //pull dominos back a bit
	//float the motor off
	mmControl(kMotorOnMux, mmFloat, nSpeed);

  //shake();
  //backup a short bit to get domino closer to previous placed unit
  //moveDegree(rotations, speed)
 	moveDegree(35, 60);
  raiseTower();
	//move robot fwd a bit to get out of way of domino
	moveDegree(-115, -70);
  lowerTower();



	nMotorEncoder[motorA] = 0;		//motor encoder initialized back to 0
	nMotorEncoder[motorB] = 0;		//motor encoder initialized back to 0

  i++;
  }


 	mmControl(kMotorOnMux, mmFloat, nSpeed);


}