#define vscan  40 // 40
#define vsbremse  14 // 14
#define sbremseabstand 30//40

#define vorlauf 25  // 16 vom NXT wegdrehen
#define wascan 5
#define wescan 175

#define abstand 60   // entfernung zum hinderniss

#define Radardif 90  // radar anfangsausgabe 270 ab 9 uhr
#define Radarcenter 0 // wo der radar mittelpunkt ist
#define messungen 200     // bei langsamer geschwindigkei werden mehr messungen gemacht
#define maxgeschwindigkeit 60
bool screchtslauf;
int  Winkel[];
byte  Entfernung[];
int richtung;
int erfasst;
int maxentfernung;
int loch;
int tacho;
int mingeschwindigkeit;
bool crash;
int abgrundwo;
bool abgrundrl;
int xalt, yalt;
int kannrichtung[];
int Bit[] = {128,64,32,16,8,4,2,2,1,0};

 //-------------------------------------------------------

void Richtung(int winkel,int xs,int ys,int l,int l2)
  { int x,y;

   x = (Sin(winkel)*l)/100;
   y = (Cos(winkel)*l)/100;
   LineOut(xs,ys,xs+x,ys+y );
  }

void ausweichen(int wo)
{
  if (!crash) richtung=0;
  crash=true;

  TextOut(0, LCD_LINE2,"      Crash" );
  //Richtung(Radardif-Winkel[wo],50,Radarcenter,Entfernung[wo],10);   // 270 damit ab 9 Uhr anzeigt
  //NumOut(45, LCD_LINE2 , Winkel[wo]);
  // NumOut(45, LCD_LINE3, Entfernung[wo]);

  if (mingeschwindigkeit< 20)  mingeschwindigkeit=20;
  NumOut(0, LCD_LINE3, mingeschwindigkeit);

  if ( Winkel[wo]>85 ) richtung=1;
  if (richtung == 0 )
      {
      OnFwdSync (OUT_AB, mingeschwindigkeit, -60);
      TextOut(0, LCD_LINE1 , "wende-links  ");
      }
   else
      {
      OnFwdSync (OUT_AB, mingeschwindigkeit, 60);
    //  TextOut(0, LCD_LINE1 , "wende-rechts   ");
      }
}
void auswerten()
{
int max, maxdumy;
max=0;
maxdumy=0;

loch = 255;
if (mingeschwindigkeit < maxgeschwindigkeit)
   {
    mingeschwindigkeit = mingeschwindigkeit + 10;
    }

crash= false;


for(int e=0 ; e<erfasst ; e++)
     {


        if (Entfernung[e]< mingeschwindigkeit) mingeschwindigkeit=Entfernung[e];

        if(Winkel[e]>0 && Winkel[e] <= 30 && Entfernung[e]< abstand ){
                loch= loch & 254;
                kannrichtung[1]++;
                if(Entfernung[e] < 20)
                  {
                   ausweichen(e);
                  }
                }
        if(Winkel[e]>30 && Winkel[e] <= 60 && Entfernung[e]< abstand ){
                loch= loch & 253;
                kannrichtung[2]++;
                if   (Entfernung[e] < 20)
                  {
                   ausweichen(e);
                  }
                }
        if(Winkel[e]>60 && Winkel[e] <= 80 && Entfernung[e]< abstand ){
                loch= loch & 251;
                kannrichtung[3]++;
                if (Entfernung[e] < 20)
                  {
                  ausweichen(e);
                  }
                }
        if(Winkel[e]>80 && Winkel[e] <= 100 && Entfernung[e]< abstand ){
                loch= loch & 247;
                kannrichtung[4]++;

                if (Entfernung[e] < 23)
                  {
                   ausweichen(e);
                  }
            }
       if(Winkel[e]>100 && Winkel[e] <= 120 && Entfernung[e]< abstand ){
                loch= loch & 239;
                kannrichtung[5]++;
                if   (Entfernung[e] < 20)
                  {
                   ausweichen(e);
                  }
                }
       if(Winkel[e]>120 && Winkel[e] <= 150&& Entfernung[e]< abstand ){
                loch= loch & 223;
                kannrichtung[6]++;
                if   (Entfernung[e] < 20)
                  {
                   ausweichen(e);
                  }
                }
        if(Winkel[e]>150 && Winkel[e] <= 180&& Entfernung[e]< abstand ){
                loch= loch & 191 ;
                kannrichtung[7]++;
                if   (Entfernung[e] < 20)
                  {
                   ausweichen(e);
                  }
                }
    } // for

for(int i=1;i<=7;i++)
   {
     if (kannrichtung[i] > maxdumy)
       {
        maxdumy = kannrichtung[i] ;
        max = i;
       // NumOut(45, LCD_LINE2 ,  maxdumy);
       // NumOut(45, LCD_LINE3 , max);
        }
        kannrichtung[i]=0;
     }
     
    //  loch =  Bit[max];
   //  NumOut(45, LCD_LINE4 ,  loch);
}  //  auswerten

void bitdarstellen (int wert)
{
int bit;
bit =1;
for (int i = 6; i>=0; i--)
   {
   if ((bit & wert) >0)
      {
       NumOut((i+1)*10, LCD_LINE6 ,1 );
       }
   else
      {
      NumOut((i+1)*10, LCD_LINE6 ,0 ) ;
      }
   bit=bit << 1;
   }
}
void fahren()
{
  int richtung;
  if( !crash){
    if(loch & 8 ) richtung=8   ;
    else
    if (loch & 16) richtung=16   ;
    else
   if (loch & 32) richtung=32   ;
   else
   if (loch & 64) richtung=64   ;
   else
   if (loch & 4) richtung=4   ;
   else
   if (loch & 2) richtung=2   ;
   else
   if (loch & 1) richtung=1   ;
   
   //NumOut(45, LCD_LINE4 ,richtung);
   
   switch(richtung)
   {
   case 1:
   OnFwdSync (OUT_AB, mingeschwindigkeit, 82);
   TextOut(0, LCD_LINE1 , "wende-recht  ");
   Richtung(Radardif-20,50,Radarcenter,80,10);

   break;
   case 2:
   OnFwdSync (OUT_AB, mingeschwindigkeit, 40);
   //TextOut(0, LCD_LINE1 , "recht        ");
   Richtung(Radardif-50,50,Radarcenter,80,10);

   break;
   case 4:
   OnFwdSync (OUT_AB, mingeschwindigkeit, 15);
   //TextOut(0, LCD_LINE1 , "leicht-recht ");
   Richtung(Radardif-70,50,Radarcenter,80,10);

   break;
   case 8:
   OnFwdReg(OUT_AB, mingeschwindigkeit, OUT_REGMODE_SPEED	);
   //TextOut(0, LCD_LINE1 , "Gerade      ");
   Richtung(Radardif-90,50,Radarcenter,80,10);

   break;
   case 16:
   OnFwdSync (OUT_AB, mingeschwindigkeit, -15);
   //TextOut(0, LCD_LINE1 , "leicht-links ");
   Richtung(Radardif-110,50,Radarcenter,80,10);

   break;
   case 32:
   OnFwdSync (OUT_AB, mingeschwindigkeit, -40);
   //TextOut(0, LCD_LINE1 , "links        ");
   Richtung(Radardif-130,50,Radarcenter,80,10);

   break;
   case 64:
   OnFwdSync (OUT_AB, mingeschwindigkeit, -82);
   TextOut(0, LCD_LINE1 , "wende-links  ");
   Richtung(Radardif-160,50,Radarcenter,80,10);
   break;
    }
  }
}

void Radar(int winkel,int xs,int ys,int l,int l2)
  { int x,y;

   x = (Sin(winkel)*l)/100;
   y = (Cos(winkel)*l)/100;
    xs += x ;
    ys += y;
  //  x = (Sin(winkel)*l2)/100
  //  y = (Cos(winkel)*l2)/100;
   RectOut(xs, ys, l2, l2,false);

  }
  void Radarkontur(int winkel,int xs,int ys,int l,int l2)
  { int x,y;

   x = (Sin(winkel)*l)/100;
   y = (Cos(winkel)*l)/100;
    xs += x ;
    ys += y;
  //  x = (Sin(winkel)*l2)/100
  //  y = (Cos(winkel)*l2)/100;
  LineOut(xalt,yalt,xs,ys );

  //  RectOut(xs, ys, l2, l2,false);
  xalt = xs;
  yalt = ys;
  }



void Skala()
{
  CircleOut(50, Radarcenter, 12, true);
  CircleOut(50, Radarcenter, 30 );
  CircleOut(50, Radarcenter, 60 ) ;
  }
void Radarausgabe()
{
  int i;
   i=0;
  //NumOut(45, LCD_LINE2 , erfasst);
   for(i=0;i<erfasst;i++)  // geht ab 0
      {
       Radarkontur(Radardif-Winkel[i],50,Radarcenter,Entfernung[i],1);   // 270 damit ab 9 Uhr anzeigt
       }
     // Richtung(Radardif-Winkel[maxentfernung],50,Radarcenter,Entfernung[maxentfernung],10);   // 270 damit ab 9 Uhr anzeigt

}  // rausgabe

void Bildschirm()
{

Skala();
auswerten();
bitdarstellen(loch);
Radarausgabe();
fahren();
erfasst=0;
}

void Trigger(int t)
{
 if(t >=wescan-sbremseabstand && screchtslauf)      // geschwindikeit runterbremsen
   {
   OnRevEx(OUT_C,vsbremse,RESET_NONE);
   // TextOut(0, 60 , "bremsr");
   }
 if(t >=wescan && screchtslauf )                         // scanner am ende
   {
   OffEx(OUT_C,RESET_NONE);
   Bildschirm();
   OnFwdEx(OUT_C,vscan,RESET_NONE);
   screchtslauf = false;

   //TextOut(0, 60 , "uml");
   }
if(t <=wascan+sbremseabstand && !screchtslauf)     // geschwindikeit runterbremsen
  {
  OnFwdEx(OUT_C,vsbremse,RESET_NONE);
  //TextOut(0, 60 , "bremsl");
  }

 if(t <=wascan && !screchtslauf)      // am anfang des scans
   {
   OffEx(OUT_C,RESET_NONE);
   Bildschirm();
   OnRevEx(OUT_C,vscan,RESET_NONE);
   screchtslauf=true;

  // TextOut(0, 60 , "umr");
   }
}


void Nullpunkt()
{
int t,talt;
OnFwd(OUT_C, 10);
do{                          // rücklauf auf 0
   talt=t ;
   Wait(60);
   t= -MotorTachoCount(OUT_C);
   } while( talt <>t )
 OnRev(OUT_C, 10); // Scanner Start
 do{                          // rücklauf auf 0
     Wait(60);
   t= -MotorTachoCount(OUT_C);
   } while( t<vorlauf  )
   Off (OUT_C);
}

task scanner()
{
byte d;
//byte abgrund, abgrundalt;
//OnRev(OUT_C, vscan); // Scanner Start
screchtslauf=false;

do{
   tacho = -MotorTachoCount(OUT_C) ;  - vorlauf;      // -moto wegen motoreinbau
   if (screchtslauf) tacho = tacho - 10;   // differens des senaors rechts links

   d = SensorUS(IN_4);
   //NumOut(45, LCD_LINE3 , tacho);
   if (d >99) d=99;    // damit keine fantome kommen
   Winkel[erfasst]=tacho;
   Entfernung[erfasst] = d ;
   erfasst++;
   Radar(Radardif-(tacho),50,Radarcenter,d,2) ;     // 270 damit ab 9 Uhr anzeigt
   //NumOut(45, LCD_LINE1, erfasst);
   //   NumOut(45, LCD_LINE7, Entfernung[maxentfernung]);
   Trigger(tacho);

 }  while(true)    // endlos
}

task main ()
{
PlayTone (200, 10);
Wait(120);
PlayTone (1000, 10);
    Wait(2000);
mingeschwindigkeit = 20;
erfasst=0;    // wieviele entfernungengemessen
ArrayInit(Winkel,0,messungen) ;     // weniger messungen als winkel
ArrayInit(Entfernung,0,messungen) ;
ArrayInit(kannrichtung,0,8);


SetSensorLowspeed(IN_4);
//SetSensorLight (S3);
Nullpunkt();

start scanner;


 //OnFwd(OUT_AB, mingeschwindigkeit );
 
//---------------------------------------------------------------------------
 do
 {

 //NumOut(45, LCD_LINE2 , erfasst);

 } while(true)

 
}
/*
 TextOut(0, LCD_LINE2 , "Abstand            ");
 TextOut(0, LCD_LINE4 , "Tacho              ");
 NumOut(45, LCD_LINE2 , d);
 NumOut(45, LCD_LINE4 , t);
 */