// Definitions and variables for the Card reader part

#define Center 4

#define Light SENSOR_3
#define CardPresent SENSOR_2

int State;
int Value;
int Cpt;
int Aff;

// Threshold between black and grey
int BG;
// Threshold between grey and yellow
int GJ;

// Hysteresis
int Hyst=5;

int Yellow;
int Black;



// Definitions and variables for the mixer part

#define RestoreTimer 5
#define LP 2
#define HP 7
#define RevTime 1
#define TakeTime 300
#define DropTime 30
#define ShakeTime 12
#define AdvanceTime 100
#define GoOver 10
#define StopTime 10


int nbstep;

task main()
{
  int i, NbSteps;

  SetSensorType(Light,SENSOR_TYPE_LIGHT);
  SetSensorMode(Light,SENSOR_MODE_RAW);
  SetSensor(CardPresent,SENSOR_TOUCH);
  SetSensor(SENSOR_1,SENSOR_TOUCH);
  
  Restore(); // Position the picker to the left, in up position
  Off(OUT_A);
  gotopin (1); // Position over the conveyor belt
  Wait(50);

  do
  {
    PlaySound(1);
    PlaySound(1); // prompt for the card
    until (CardPresent==1); // and wait for it
    GetValue(); // Reads the card
	
// The 14 lower bits are sliced two bits at a time, encoding the color
// of the brick :
//    - 00 = Red brick (chute nearest of the conveyor)
//    - 01 = Gray brick
//    - 10 = Black brick
//    - 11 = Yellow brick (chute furthest of the conveyor)
// A card thus defines 7 bricks

    for(i=0; i<7; i++)
    {
      NbSteps=(Value&3)+1;
      Value/=4;
      gotopin (NbSteps);
      TakeBrick();
      gotopin (-NbSteps);
      DropBrickAndAdvance();
    }
  }

// The most significant bit read on the card is a follow-on indicator:
// when 0, the brick mixer stops
// when 1, it asks for a new card
  while ((Value&1) == 1); 
  
  PlaySound(2);
}


// Restore initialize the brick mixer:
// - lifts the picker arm
// - moves the picker slowly to the left (towards card reader)
//   till it bumps on the stop pin. Motor power is set to low so that
//   it can stall without problem (but each time the touch sensor hits a
//   position pin, the power is set high for a small time to 
//   overcome friction).

void Restore()
{
  OnFwd(OUT_C);
  Wait(TakeTime);
  Off(OUT_C);
  SetPower(OUT_A,LP);
  OnRev(OUT_A);
  ClearTimer(1);
  while (Timer(1) < RestoreTimer)
  {
    if (SENSOR_1==1)
    {
      ClearTimer(1);
      SetPower(OUT_A,HP);
      Wait(10);
      SetPower(OUT_A,LP);
    }
  }
}  


// TakeBrick lowers the picker, then lifts the brick
// The micromotor has very little torque, hence the rapid
// shaking action in the up or down position, to try (?)
// to unlock the arm - it may remain locked sometimes though...

sub TakeBrick ()
{  
  OnRev(OUT_C);
  Wait(ShakeTime);
  OnFwd(OUT_C);
  Wait(ShakeTime);
  OnRev(OUT_C);
  Wait(ShakeTime);
  OnFwd(OUT_C);
  Wait(ShakeTime);
  OnRev(OUT_C);
  Wait(TakeTime);
  OnFwd(OUT_C);
  Wait(ShakeTime);
  OnRev(OUT_C);
  Wait(ShakeTime);
  OnFwd(OUT_C);
  Wait(TakeTime);
  Off(OUT_C);
}  
  
// DropBrickAndAdvance releases the brick, then advances the
// conveyor belt. Same shaking action as for TakeBrick ;<(

sub DropBrickAndAdvance ()
{
  OnRev(OUT_C);
  Wait(ShakeTime);
  OnFwd(OUT_C);
  Wait(ShakeTime);
  OnRev(OUT_C);
  Wait(ShakeTime);
  OnFwd(OUT_C);
  Wait(ShakeTime);
  OnRev(OUT_C);
  Wait(DropTime);
  OnFwd(OUT_C);
  Wait(DropTime+20);
  Off(OUT_C);
  OnFwd(OUT_B);
  Wait(AdvanceTime);
  Off(OUT_B);
  
  
}  

// gotopin(n) moves the picker by n pins towards the feeder (n>0)
// or towards the conveyor (n<0)
// gotopin beeing a function, it is complied in-line
// Thus the use of a sub to minimize code size

void gotopin (int step)
{
  nbstep=step;
  gotosub();
}

// the sub performing the actual code of gotopin
sub gotosub ()
{
  int num=0;
  
  while (true)
  {
    if (nbstep > 0)
    {
      SetPower(OUT_A,HP); // set to high power to overcome position pin friction
      OnFwd(OUT_A);
      Wait(GoOver); 
      SetPower(OUT_A,LP); // then to low power to be slowed down by the next pin
      until (SENSOR_1);   // wait for next pin
      num++;
      if (num == nbstep)
      {
        while (SENSOR_1); // go past the pin
        Off(OUT_A);       // stop
        Wait(StopTime);
        OnRev(OUT_A);     // and come back slowly, without momentum
        until (SENSOR_1);
        Off(OUT_A);
        return;
      }     
    }
    else
    {
      if (nbstep == 0) return;
      SetPower(OUT_A,HP);
      OnRev(OUT_A);
      Wait(GoOver);
      SetPower(OUT_A,LP);
      until (SENSOR_1);
      num--;
      if (num == nbstep)
      {
        while (SENSOR_1);
        Off(OUT_A);
        Wait(StopTime);
        OnFwd(OUT_A);
        until (SENSOR_1);
        Off(OUT_A); 
        return;
      }    
    }
  }
}

// GetValue reads the card (15 bits)
// Card Reader, auto-synchronized
// With light sensor auto-calibration / raw mode (variation is inverted, less light -> higher readings)
// Next bit encoding depends on previous bit color
// State encoding :
// Present state     Encoding       Next State
//           0      Yellow = 0      2
//           0      Grey   = 1      1
//           1      Black  = 0      0
//           1      Yellow = 1      2
//           2      Black  = 0      0
//           2      Grey   = 1      1

void GetValue()
{
  Yellow = Light-10; // use -10 because of diffusing tape : background is farther thus darker
  BG = Yellow+30; // First approximation of Black/Grey threshold
  Value = 0;
  State = 0;
  Cpt=0;
  OnRev(OUT_B);
  until (Light > BG + Hyst); // First plate is black
  Wait(Center);
  Black = Light; // Get Black value
  // Define Thresholds
  BG=Black-(Black-Yellow)*3/14;
  GJ=Yellow+(Black-Yellow)*2/7;

  do
  {
    Cpt++;
    Value *= 2;
    switch (State)
    {
    case 0:
    	until (Light < BG - Hyst);
	Wait(Center);
	if (Light < GJ)
	{
	  State = 2;
	}
	else
	{
	  Value += 1;
	  State = 1;
      }
      break;	

    case 1:
	until ((Light > BG + Hyst)|| (Light < GJ - Hyst));
	Wait(Center);
	if (Light > BG)
	{
	  State = 0;
	}
	else
	{
	  Value += 1;
	  State = 2;
      }
      break;	

    case 2:
	until (Light > GJ + Hyst);
	Wait(Center);
	if (Light > BG)
	{
	  State = 0;
	}
	else
	{
	  Value += 1;
	  State = 1;
      }
      break;	
    }
  } while ( Cpt < 15 );
  Wait(50);
  Float(OUT_B);
  Wait(50);
  Off(OUT_B);
}