/*
Visit my website: http://nxt.meinblock.eu
16.02.2008
02/16/2008
*/

int in = 0;
int dir = 0;
mutex MoveMutex;
mutex DirectionMutex;

sub BTCheck(int conn)
{
  if (!BluetoothStatus(conn)==NO_ERR)
    {
      TextOut(5,LCD_LINE2,"Error");
      Wait(1000);
      Stop(true);
    }
}

sub forwards(int pwr)
{
  Acquire(MoveMutex);
  OnFwd(OUT_C, -pwr);
  Release(MoveMutex);
}

sub backwards(int pwr)
{
  Acquire(MoveMutex);
  OnFwd(OUT_C, pwr);
  Release(MoveMutex);
}

sub left(int pwr)
{
  Acquire(DirectionMutex);
  if(dir >= -80)
    {
      RotateMotor(OUT_B, pwr, -20);
      dir = (dir - 20);
    }
  Release(DirectionMutex);
}

sub right(int pwr)
{
  Acquire(DirectionMutex);
  if(dir <= 80)
    {
      RotateMotor(OUT_B, pwr, 20);
      dir = (dir + 20);
    }
  Release(DirectionMutex);
}

sub streight(int pwr)
{
  Acquire(DirectionMutex);
  if(dir < 0)
    {
      RotateMotor(OUT_B, pwr, -dir);
      dir = 0;
    }
  if(dir > 0)
    {
      RotateMotor(OUT_B, pwr, -dir);
      dir = 0;
    }
  Release(DirectionMutex);
}

task receive()
{
  BTCheck(0);
  while(true)
    {
      if (ReceiveRemoteNumber(1, true, in) == NO_ERR)
        {
          TextOut(0, LCD_LINE3, "Received:");
          NumOut(70, LCD_LINE3, in);
        }
    }
}

task dirshow()
{
  while(true)
  {
    TextOut(0, LCD_LINE1, "Direction:");
    NumOut(70, LCD_LINE1, dir);
  }
}

task ultrasound()
{
  SetSensorLowspeed(IN_1);
  while(true)
  {
    if(SensorUS(IN_1) <= 10)
      {
        Acquire(MoveMutex);
        Off(OUT_C);
        streight(20);
        Release(MoveMutex);
        Stop(true);
      }
  }
}

task action()
{
  while(true)
  {
    if(in == 1)
      Off(OUT_C);
    if(in == 4)
      left(20);
    if(in == 5)
      streight(20);
    if(in == 6)
      right(20);
    while(in == 2)
      forwards(40);
    while(in == 8)
      backwards(40);
    while(in == 3)
      forwards(80);
    while(in == 9)
      backwards(80);
  }
}

task main()
{
  Precedes(receive, action, dirshow, ultrasound);
}