/*
Please visit my website: http://nxt.meinblock.eu
03/14/2008
14.03.2008
*/

#define MITTEY 32
#define MITTEX 50
#define RADIUS 30
#define UMDREHUNG 20000
#define REPEAT 100

int sinwerte[]=  {0,18,35,52,70,87,105,122,139,156,174,191,208,225,242,259,276,292,309,326,342,358,375,391,407,423,438,454,467,485,500,515,530,545,559,574,588,602,616,629,643,656,669,682,695,707,719,731,743,755,766,777,788,799,809,819,829,839,848,857,866,875,882,891,899,906,914,921,927,933,940,946,951,956,961,966,970,974,978,982,985,988,990,993,995,996,998,999,999,100,100};

int sin(int sinwinkel)
{
  int sinwert;
  if(sinwinkel >= 360)
    sinwinkel = sinwinkel-360;
  if(sinwinkel >= 270)
    {
      sinwert = -sinwerte[360-sinwinkel];
      return sinwert;
    }
  if(sinwinkel >= 180)
    {
      sinwert = -sinwerte[sinwinkel-180];
      return sinwert;
    }
  if(sinwinkel >= 90)
    {
      sinwert = sinwerte[180-sinwinkel];
      return sinwert;
    }
  if(sinwinkel >= 0)
    {
      sinwert = sinwerte[sinwinkel];
      return sinwert;
    }
 }

int cos(int coswinkel)
{
  int coswert;
  if(coswinkel >= 360)
    coswinkel = coswinkel-360;
  if(coswinkel >= 270)
    {
      coswert = sinwerte[coswinkel-270];
      return coswert;
    }
  if(coswinkel >= 180)
    {
      coswert = -sinwerte[270-coswinkel];
      return coswert;
    }
  if(coswinkel >= 90)
    {
      coswert = -sinwerte[coswinkel-90];
      return coswert;
    }
  if(coswinkel >= 0)
    {
      coswert = sinwerte[90-coswinkel];
      return coswert;
    }
 }

task main()
{
  int winkel = 0;
  int entfernung;
  int motorwinkelneu;
  int motorwinkelalt = 0;
  SetSensorLowspeed(IN_1);
  SetSensorTouch(IN_2);
  NumOut(0, LCD_LINE1, RADIUS);
  PointOut(MITTEX, MITTEY);
  CircleOut(MITTEX, MITTEY, RADIUS);
  until(Sensor(IN_2) == 1);
  /*
  repeat(REPEAT)
    {
      RotateMotor(OUT_C, 100, UMDREHUNG/REPEAT);
      winkel = winkel + 360/REPEAT;
      entfernung = SensorUS(IN_1);
      if(entfernung < RADIUS);
        PointOut(((cos(winkel)*entfernung)/1000)+MITTEX, ((sin(winkel)*entfernung)/1000)+MITTEY);
    }
  */
  
//  /*
  while(true)
    {
      OnFwd(OUT_C, 100);
      motorwinkelneu = MotorRotationCount(OUT_C);
      if(motorwinkelneu >= UMDREHUNG)
        {
          break;
        }
      if(motorwinkelneu >= motorwinkelalt+UMDREHUNG/360)
        {
          motorwinkelalt = motorwinkelneu;
          winkel++;
          entfernung = SensorUS(IN_1);
          if(entfernung < RADIUS);
            PointOut(((cos(winkel)*entfernung)/1000)+MITTEX, ((sin(winkel)*entfernung)/1000)+MITTEY);
        }
    }
//  */
  Off(OUT_C);
  RotateMotor(OUT_C, 100, -UMDREHUNG);
  Off(OUT_C);
  until(Sensor(IN_2) == 1);
}