/////////////////////////////////////////////////////////////////////////////
//                           TamTam.c
//                    Version 1.0.7 "Dijkstra"
//              virtuelle Wegsuche auf dem Display
//***************************************************************************
//  der A* (A-Stern, a-star) - Algorithmus fuer den NXT
//  zur Wegsuche und Navigation an Hindernissen vorbei
//***************************************************************************
//  hier in der Variante nach Dijkstra
//  (keine geschaetzten heuristischen Kosten H, also H=0 => F=G!
//
//  1.0.4 am Schluss werden Routenanweisungen erstellt
//  1.0.1 erkennt, wenn  kein Weg zum Ziel existiert
//  0.8.9 Suchrichtung optimiert: direkte Wege werden sehr schnell gefunden
//  0.3.8 Kurse optimiert: Kurs-Aenderungen verursachen hoehere Kosten
//  0.2.9 schneiden von Hindernis-Kanten wird vermieden
// (c) Helmut W. 2008
/////////////////////////////////////////////////////////////////////////////



#include "RobotC+.h"

#define println nxtDisplayTextLine
#define printXY nxtDisplayStringAt
//********************************************************************
short   xStart, yStart, xZiel, yZiel;

const short MadlyEnormous=10000;
const short occ=1;

const short MapSize=30;            // 30x30 Felder, je 30cm plus "Rand"
const short limit=MapSize/2;

short Map[MapSize+1][MapSize+1];    // besetzt=1 / frei=0;
short xPrev[MapSize+1][MapSize+1];  // Abszisse von Vorgaenger (x)
short yPrev[MapSize+1][MapSize+1];  // Ordinate von Vorgaenger (y)
short dPrev[MapSize+1][MapSize+1];  // Richtung  zu Vorgaenger (d= 0-7)
short F[MapSize][MapSize];      // gesamte Wege-Kosten F
short List[MapSize][MapSize];   // gehoert zu Liste:  undef=0 offen=1 geschlossen=2


//********************************************************************

short GetMap(short x, short y)                     // Zustand des Feldes: 0=frei, 1= Hindernis
{   return Map[x+limit][y+limit];}

void SetMap(short x, short y, short val)
{   Map[x+limit][y+limit]=val;}

short GetxPrev(short x, short y)                 // gespeicherter Vorgaenger: x-Koord.
{   return xPrev[x+limit][y+limit];}

void SetxPrev(short x, short y, short xPtr)
{   xPrev[x+limit][y+limit]=xPtr;}

short GetyPrev(short x, short y)                 // gespeicherter Vorgaenger: y-Koord.
{   return yPrev[x+limit][y+limit];}

void SetyPrev(short x, short y, short yPtr)
{   yPrev[x+limit][y+limit]=yPtr;}

short GetdPrev(short x, short y)                 // gespeicherter Vorgaenger: Richtung
{   return dPrev[x+limit][y+limit];}

void SetdPrev(short x, short y, short d)
{   dPrev[x+limit][y+limit]=d;}

short GetF(short x, short y)                     //  Wege-Kosten F
{   return F[x+limit][y+limit];}

void SetF(short x, short y, short val)
{   F[x+limit][y+limit]=val;}

short GetList(int x, int y)                      // Listen-Zugehoerigkeit (0, 1, 2)
{   return List[x+limit][y+limit];}

void SetList(int x, int y, int val)
{   List[x+limit][y+limit]=val;}


//********************************************************************
//********************************************************************

void SetPixel(short x, short y)   // Transformation der Koordinatensysteme
{
  int xPix, yPix;

  xPix=limit+1+x;
  yPix=limit+1+y;
  nxtSetPixel(xPix, yPix);
}

//********************************************************************

void DrawRect(short x, short y, short rad) // Quadrat um den Mittelpunkt zeichnen
{
  int xPix, yPix;

  xPix=limit+x+1; // CenterX
  yPix=limit+y+1; // CenterY
  nxtDrawRect(xPix-rad, yPix+rad, xPix+rad, yPix-rad);
}

//********************************************************************

void ClearPixel(short x, short y) // Karten-Pixel loeschen
{
  int xPix, yPix;

  xPix=limit+1+x;
  yPix=limit+1+y;
  nxtClearPixel(xPix, yPix);
}

//********************************************************************
//********************************************************************

int GetNx(short x, short pos)// 3 2 1   benachbarte x-Koordinaten
{                            // 4   0
   int nx;                   // 5 6 7

   nx=MadlyEnormous;  // wenn ungueltiger Nachbar
   if (((pos==1)||(pos==0)||(pos==7))&& (x<limit-1))
     {nx=x+1; }
   else
   if ((pos==2)||(pos==6))
     { nx=x;  }
   else
   if (((pos==3)||(pos==4)||(pos==5))&&(x>(-limit)))
     {nx=x-1; }

   return nx;
}

//********************************************************************

int GetNy(short y, short pos)// 3 2 1   benachbarte y-Koordinaten
{                            // 4   0
   int ny;                   // 5 6 7

   ny=MadlyEnormous;  // wenn ungueltiger Nachbar
   if (((pos==1)||(pos==2)||(pos==3))&& (y<limit-1))
     { ny=y+1; }
   else
   if ((pos==4)||(pos==0))
     { ny=y;  }
   else
   if (((pos==5)||(pos==6)||(pos==7))&&(y>(-limit)))
     { ny=y-1; }

   return ny;
}

//********************************************************************

bool CloseToTheEdge(short x, short y, short dir)   // 3 2 1   keine Hindernis-Kanten schneiden
{                                                  // 4   0
   bool edge;                                      // 5 6 7
   short xm,xp,ym,yp;

   edge=false;  // nur bei diagonalem Schritt beruecksichtigen!)

     xm=(xm> -limit  ? x-1 : x);
     xp=(xp< limit-1 ? x+1 : x);
     ym=(ym> -limit  ? y-1 : y);
     yp=(yp< limit-1 ? y+1 : y);


     if (dir==1) {edge=((GetMap(xp,y)==occ)||(GetMap(x,yp)==occ));} // 0  2
     else
     if (dir==3) {edge=((GetMap(x,yp)==occ)||(GetMap(xm,y)==occ));} // 2  4
     else
     if (dir==5) {edge=((GetMap(xm,y)==occ)||(GetMap(x,ym)==occ));} // 4  6
     else
     if (dir==7) {edge=((GetMap(x,ym)==occ)||(GetMap(xp,y)==occ));} // 6  0

   return edge;
}



//********************************************************************

void ExpandNode(short xa, short ya)  // checkt Nachbarfelder von (xa, ya)
{
  short F, d, k, v, w, z;

  for (d=0;d<=7;d++)
  {
       v=GetNx(xa,d); //  Nachbarn durchgehen:  3 2 1
       w=GetNy(ya,d); //                        4   0
                      //                        5 6 7
       if ((d==0) ||(d==2) ||(d==4) ||(d==6))    // ((p%2)==0 arbeitet fehlerhaft!)
       {k=10; }                                  // Kosten gerader Schritt: 10
       else
       {
         k=14;                                   // Kosten diagonaler Schritt: 15
         if(CloseToTheEdge(xa,ya,d)) continue;   // diagonal ist bei Hinderniskante verboten
       }

       if ((v<MadlyEnormous)&&(w<MadlyEnormous)) // gueltiger Nachbar ?!
       {
         z=GetMap(v,w);                          // Hindernis vorhanden?
         if ( (z==0)&&(!(GetList(v,w)==2)))      // frei und nicht in geschlossener Liste
         {
           F=GetF(xa,ya)+k;                      // F: Kosten zu (v,w) ueber aktuelles Quadrat (xa,ya)

           if (!(GetList(v,w)==1))          // wenn NICHT in offener Liste
           {
              SetList (v,w,1);              // dann in die offene uebernehmen
              SetxPrev(v,w, xa);            // Vorgaenger: ^x-aktuell
              SetyPrev(v,w, ya);            // Vorgaenger: ^y-aktuell
              SetdPrev(v,w,  d);            // Richtung zum Vorgaenger (0-7)
              if(d==GetdPrev(xa,ya)) F-=2;  // Kosten geringer, wenn keine Kursaenderung
              SetF(v,w,F);                  // Kosten gesamt
           } // if  (Nicht offen)
           else
           if (GetList(v,w)==1)             // wenn aber bereits in offener Liste
            {
               if (F<=GetF(v,w))              // Wegekosten G dorthin vergleichen:
                                             // neuer Weg dahin: geringere Kosten als alte Wegekosten ?
               {                             // dann ueberschreiben + neuen Weg im Quadrat einspeichern
                 SetxPrev(v,w, xa);          // Vorgaenger: ^x-aktuell
                SetyPrev(v,w, ya);          // Vorgaenger: ^y-aktuell
                SetdPrev(v,w,  d);          // Richtung zum Vorgaenger (0-7)
                if(d==GetdPrev(xa,ya)) F-=2;// Kosten geringer, wenn keine Kursaenderung
                SetF(v,w,F);                // Kosten gesamt
               }// if (neuer Weg besser)
           } // else if (schon offen)

         }//  if (nicht besetzt, nicht geschlossen)
       } // if (gueltiger Nachbar)

  }// for

}//ExpandNode

//********************************************************************
//********************************************************************

void A_Stern (short xStart, short yStart, short xZiel, short yZiel)
{
  short i, j, c=0, xAkt, yAkt, xOld, yOld, xd, yd, xDa, yDa, FMin=MadlyEnormous;


  xAkt=xZiel;       // wir starten rueckwaerts und beginnen die Suche beim Ziel !
  yAkt=yZiel;       // Das Quadrat, von dem aus gesucht wird, heisst "aktuelles Quadrat"

  SetList(xAkt, yAkt, 1);                   // akt. Quadrat in Offene Liste (=1)
  SetF (xAkt,yAkt,0);                       // Kosten F=G+H; G=0, H=0;
  SetList(xAkt, yAkt, 2);                   // aktuelles Quadrat in geschlossene Liste uebernehmen
  xd=abs(xZiel-xStart);
  yd=abs(yZiel-yStart);
     // solange das andere Ende (der Start) nicht gefunden wurde:

  while (GetList(xStart,yStart)!=2)
  {
    FMin=MadlyEnormous;
     xOld=xAkt;
     yOld=yAkt;
    // suche in offener Liste (=1)das Feld mit kleinstem F

    if ((xZiel>=xStart)&&(yZiel>=yStart)) {
    for (i=-limit;i<limit;i++)
     {
        for (j=-limit;j<limit;j++)
       {
          if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) )
         {
            FMin=GetF(i,j);
           xAkt=i;    // wenn gefunden: nimm es als aktuelles Quadrat
           yAkt=j;
           xDa=abs(i-xStart);
           yDa=abs(j-yStart);
             if ((xDa<xd)||(yDa<yd))
              {
                SetList(xAkt, yAkt, 2);     // aktuelles Quadrat in geschlossene Liste
              SetPixel(xAkt,yAkt);
              ExpandNode(xAkt, yAkt);     // Nachbarfelder untersuchen
             c+=1;
           }
         } // if
       } // for j
       SetList(xAkt, yAkt, 2);     // aktuelles Quadrat in geschlossene Liste
       SetPixel(xAkt,yAkt);
       ExpandNode(xAkt, yAkt);     // Nachbarfelder untersuchen
      c+=1;
     } // for i

    } // if (Richtung)
    else
    if ((xZiel<xStart)&&(yZiel<yStart)){
    for (i=limit-1;i>=(-limit);i-=1)
     {
        for (j=limit-1;j>=(-limit);j-=1)
       {
          if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) )
         {
            FMin=GetF(i,j);
           xAkt=i;    // wenn gefunden: nimm es als aktuelles Quadrat
           yAkt=j;
           xDa=abs(i-xStart);
           yDa=abs(j-yStart);
             if ((xDa<xd)||(yDa<yd))
              {
                SetList(xAkt, yAkt, 2);     // aktuelles Quadrat in geschlossene Liste
              SetPixel(xAkt,yAkt);
              ExpandNode(xAkt, yAkt);     // Nachbarfelder untersuchen
             c+=1;
           }
         } // if
       } // for j
       SetList(xAkt, yAkt, 2);     // aktuelles Quadrat in geschlossene Liste
       SetPixel(xAkt,yAkt);
       ExpandNode(xAkt, yAkt);     // Nachbarfelder untersuchen
      c+=1;
     } // for i

    } // if (Richtung)

    else
    if ((xZiel<xStart)&&(yZiel>=yStart))
    {
    for (i=limit-1;i>=(-limit);i-=1)
    {
        for (j=-limit;j<limit;j+=1)
       {
          if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) )
         {
            FMin=GetF(i,j);
           xAkt=i;    // wenn gefunden: nimm es als aktuelles Quadrat
           yAkt=j;
           xDa=abs(i-xStart);
           yDa=abs(j-yStart);
             if ((xDa<xd)||(yDa<yd))
              {
                SetList(xAkt, yAkt, 2);     // aktuelles Quadrat in geschlossene Liste
              SetPixel(xAkt,yAkt);
              ExpandNode(xAkt, yAkt);     // Nachbarfelder untersuchen
             c+=1;
           }
         } // if
       } // for j
       SetList(xAkt, yAkt, 2);     // aktuelles Quadrat in geschlossene Liste
       SetPixel(xAkt,yAkt);
       ExpandNode(xAkt, yAkt);     // Nachbarfelder untersuchen
      c+=1;
     } // for i

    } // if (Richtung)

    else
    if ((xZiel>=xStart)&&(yZiel<yStart))
    {
    for (i=-limit;i<limit;i+=1)
     {
        for (j=limit-1;j>=(-limit);j-=1)
       {
          if ((GetList(i,j)==1)&&(GetF(i,j)<FMin) )
         {
            FMin=GetF(i,j);
           xAkt=i;    // wenn gefunden: nimm es als aktuelles Quadrat
           yAkt=j;
           xDa=abs(i-xStart);
           yDa=abs(j-yStart);
             if ((xDa<xd)||(yDa<yd))
              {
                SetList(xAkt, yAkt, 2);     // aktuelles Quadrat in geschlossene Liste
              SetPixel(xAkt,yAkt);
              ExpandNode(xAkt, yAkt);     // Nachbarfelder untersuchen
             c+=1;
           }
         } // if
       } // for j
       SetList(xAkt, yAkt, 2);     // aktuelles Quadrat in geschlossene Liste
       SetPixel(xAkt,yAkt);
       ExpandNode(xAkt, yAkt);     // Nachbarfelder untersuchen

      c+=1;
     } // for i


    } // if (Richtung)
    println(1, "Akt(X,Y)%4d%4d",xAkt,yAkt);  // Anzeige von xAkt, yAkt
    printXY(46, 20, "Wege=%4d",c);           // Anzeige der Zahl der Zyklen/Wege

    if((xAkt==xOld)&&(yAkt==yOld)&&(FMin==MadlyEnormous))
     {
        println(2,"%s","No Way!");
        PlaySound(soundBeepBeep);
        Wait10Msec(200);
        break;
     }

  } // while

  printXY(46,10, "fertig %s","");     // Anzeige, wenn fertig
  ClearPixel(xStart,yStart);
  Wait10Msec(200);

}

//********************************************************************
//********************************************************************


sub DrawMap()  //  Hindernisse und Start/Ziel zeichnen
{
   short x, y;

   nxtDrawRect(0, MapSize+1, MapSize+1, 0); // Umrandung

//==========================================================================
   x=8;                       // Hindernis setzen:
   for (y=-3;y<=5;y++)        // Hier kann man sich austoben und beliebige
   {                          // Hindernis-Punkte definieren:
    SetMap(x,y,occ);         // a) in Karte eintragen (occ=1=besetzt)
    SetPixel(x,y);           // b) auf Display malen
  }
  y=9;
  for (x=-8;x<=7;x++)        // nochmal weitere
   {                          // Hindernis-Punkte definieren:
    SetMap(x,y,occ);         // a) in Karte eintragen (occ=1=besetzt)
    SetPixel(x,y);           // b) auf Display malen
  }

  y=-5;
  for (x=0;x<=5;x++)         // nochmal weitere
   {                          // Hindernis-Punkte definieren:
    SetMap(x,y,occ);         // a) in Karte eintragen (occ=1=besetzt)
    SetPixel(x,y);           // b) auf Display malen
  }

  y=1;
  for (x=10;x<=12;x++)         // nochmal weitere
   {                          // Hindernis-Punkte definieren:
    SetMap(x,y,occ);         // a) in Karte eintragen (occ=1=besetzt)
    SetPixel(x,y);           // b) auf Display malen
  }

  y=6;
  for (x=8;x<=14;x++)         // nochmal weitere
   {                          // Hindernis-Punkte definieren:
    SetMap(x,y,occ);         // a) in Karte eintragen (occ=1=besetzt)
    SetPixel(x,y);           // b) auf Display malen
  }

//==========================================================================

  SetPixel(xStart,yStart);    // Start als Punkt malen
  DrawRect(xZiel, yZiel,1);   // Ziel als Viereck malen

}

//********************************************************************
//********************************************************************

void WalkThisWay()   // virtuelle Fahrt der berechneten Route
{
  short x, y, xn, yn, dir, olddir, turn;

  x=xStart;
  y=yStart;
  dir=0;
  olddir=0;
  turn=0;
  //printXY(46,10,"Kost=%4d",GetF(xStart,yStart));
  println(0, "Turn=%4d",turn);
  println(1, "Kurs=%4d",dir);
  println(2, "Pos XY%3d%3d",x,y);
  wait1Msec(1000);

  while (! ((x==xZiel)&&(y==yZiel)))
  {
    println(2, "Pos XY%3d%3d",x,y);
    SetPixel(x, y);
    wait1Msec(500);

    dir=180+(45*GetdPrev(x,y));
    if (dir>=360) dir-=360;

    turn=dir-olddir;
    if (turn<-180) turn=turn+360;
    if (turn>180) turn=360-turn;

    println(0, "Turn=%4d",turn);
    wait1Msec(500);

    println(1, "Kurs=%4d",dir);
      wait1Msec(500);

      olddir=dir;
      xn=GetxPrev(x,y);
      yn=GetyPrev(x,y);
      x=xn;
      y=yn;

   }
  SetPixel(x, y);
  println(2, "Pos XY%3d%3d",x,y);
  printXY(46,30, "%s","Ziel");
  printXY(46,20, "%s","erreicht!");



}

//********************************************************************

sub initVar()
{
   short i,j;

   // verschiedene Start/Ziel-Kombinationen

  xStart=0; yStart=0;       // Startpunkt definieren
  xZiel=13; yZiel=4;        // Zielpunkt definieren

  //xStart=13; yStart=4;    // Startpunkt definieren
  //xZiel=0; yZiel=0;       // Zielpunkt definieren

   //xStart=0; yStart=4;       // Startpunkt definieren
  //xZiel=13; yZiel=0;        // Zielpunkt definieren

  //xStart=13; yStart=0;       // Startpunkt definieren
  //xZiel=0; yZiel=4;          // Zielpunkt definieren

   for(i=0;i<MapSize;i++)
   {
      for (j=0;j<MapSize;j++)
      {
        Map[i][j]=0;             // Felder frei
        xPrev[i][j]=0;           // Vorgaenger Nullpunkt
      yPrev[i][j]=0;
      F[i][j]=MadlyEnormous;   // Kosten gewaltig
      List[i][j]=0;            // Liste undefiniert
    }
  }
}
//********************************************************************

task main()
{
  eraseDisplay();
  initVar();
  DrawMap();

  A_Stern(xStart,yStart,xZiel, yZiel);

  while(true)
  {
     eraseDisplay();
    DrawMap();
    WalkThisWay();
    wait1Msec(2000);
  }
}
//********************************************************************
//********************************************************************

///////////////////////////////////////////////////////////////////////
// RobotC+.h - damit's ein bisserl einfacher wird...    ;-)          //
// Version 0.5.8
///////////////////////////////////////////////////////////////////////



//===================================================================
// mathematische Konstanten und Funktionen
//===================================================================

// Eulersche Zahl E:
const float E=2.718281828450945235360287471352662497757247093699959574966967627724076630353547594571;

//********************************************

int  min(int a, int b)
{
   int m;
   m=(a<b ? a:b);
   return m;
}

//********************************************

float min(float a, float b)
{
   float m;
   m=(a<b ? a:b);
   return m;
}

//********************************************

int  max(int a, int b)
{
   int m;
   m=(a>b ? a:b);
   return m;
}

//********************************************

float max(float a, float b)
{
   float m;
   m=(a>b ? a:b);
   return m;
}

//********************************************

int round(float f)
{
  if(f>0) return (int)(f + 0.5);
  else    return (int)(f - 0.5);
}

//********************************************

// ArcusTangens mit Sonderfaellen; Angabe in Grad!
// x=Ankathete y=Gegenkathete Tangens=y/x

float atan2Degrees(float x, float y)
{
  float phi, alpha; //phi=Bogenmass, alpha=Grad;

   if (x>0) {phi=atan(y/x);}
   else
   if ((x<0)&&(y>=0))  {phi=PI+atan(y/x);}
   else
   if ((x<0)&&(y<0))   {phi=-PI+atan(y/x);}
   else
   if ((x==0)&&(y>0))  {phi=PI/2;}
   else
   if ((x==0)&&(y<0))  {phi=-PI/2;}
   else
   if ((x==0)&&(y==0)) {phi=0;}

   alpha=radiansToDegrees(phi);
   return alpha;
}

//********************************************

bool IsInRange(int Wert, int Basis, int Toleranz)
{
   return((Wert>=Basis-Toleranz) && (Wert<=Basis+Toleranz));
}


//===================================================================
// Prozeduren fuer Sensoren
//===================================================================

bool SMuxPressed(int port, byte ch) // MUX-Port an NXT (S1, S2,... ) // ch: 0=direkt, 1=ch1, 2=ch2, 3=ch3

{
  int v;
   bool pressed;

   v=SensorValue(port);
   if (ch==0)
   { pressed=(v<100 ); }
   else
  if (ch==1)
   { pressed=( IsInRange(v,775,30) || IsInRange(v,523,25) || IsInRange(v,343,8)   || IsInRange(v,275,8)) ; }
  else
   if (ch==2)
   { pressed=( IsInRange(v,620,40) || IsInRange(v,523,25) || IsInRange(v,295,8)   || IsInRange(v,275,8)) ; }
  else
   if (ch==3)
   { pressed=( IsInRange(v,363,15) || IsInRange(v,343,8)  || IsInRange(v,295,8)   || IsInRange(v,275,8)) ; }
  return pressed;

// Wertetabelle fuer Sensor-Muxer (natuerlich Bauform-abhaengig)
//*************************************************************
//
//  rcx ch3 ch2 ch1     raw       // RCX-WiderstMux als sensorRawValue definiert
//   0   0   0   0     1023       // an 3 Mux-Einaenge UND direktan rcx-Eingang
//   0   0   0   1      775       // je 1 Taster angeschlossen
//   0   0   1   0      620
//   0   0   1   1      523
//   0   1   0   0      363
//   0   1   0   1      343
//   0   1   1   0      295
//   0   1   1   1      275
//   1   0   0   0       79        // ab hier die Werte, wenn der
//   1   0   0   1       64        // 4. Taster direkt am rcx-Sensor-Eingang
//   1   0   1   0       74        // gedrueckt wird.
//   1   0   1   1       70        // Eine Unterscheidung, ob dann auch
//   1   1   0   0       69        // gleichzeitig  noch andere Taster
//   1   1   0   1       55        // zusaetzlich gedrueckt sind,
//   1   1   1   0       55        // ist dann kaum mehr moeglich.
//   1   1   1   1       55        //

}

//===================================================================
// globale Prozeduren fue Motor-Steuerung
//===================================================================

void  motSync (int m, int s) // setzt Sync-Master & Slave
{
   if ((m==0) && (s==0))
   { nSyncedMotors = synchNone;}
   else
   if ((m==0) && (s==1))
   { nSyncedMotors = synchAB;}
   else
   if ((m==0) && (s==2))
   { nSyncedMotors = synchAC;}
   else
   if ((m==1) && (s==0))
   { nSyncedMotors = synchBA;}
   else
  if ((m==1) && (s==2))
   { nSyncedMotors = synchBC;}
   else
  if ((m==2) && (s==0))
   { nSyncedMotors = synchCA;}
   else
  if ((m==2) && (s==1))
   { nSyncedMotors = synchCB;}
   else nSyncedMotors = synchNone;
}


//===================================================================
//===================================================================

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / TamTam.c
/ / Version 1.0.7 "Dijkstra"
/ / Virtual path search on the display
/ / ************************************************ ***************************
/ / The A * (A-Star, A-star) - algorithm for the NXT
/ / For path finding and navigation past obstacles
/ / ************************************************ ***************************
/ / Here in the variant of Dijkstra
/ / (No estimated heuristic cost H, ie H = 0 => F = G!
/ /
/ / 1.0.4 at the end of route instructions are created
/ / 1.0.1 detects when no route to the destination exists
/ / 0.8.9 optimized search direction: direct paths are found very quickly
/ / 0.3.8 Courses optimized price changes lead to higher costs
/ / 0.2.9 cutting-edge obstacle is avoided
/ / (C) Helmut W. 2008
/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /



# Include "RobotC +. H"

# Define println nxtDisplayTextLine
# Define printXY nxtDisplayStringAt
/ / ************************************************ ********************
short Xstart, yStart, xZiel, yZiel;

const short MadlyEnormous = 10000;
const short occ = 1;

const short MapSize = 30, / / ​​30x30 fields, each 30cm plus "edge"
limit = const short MapSize / 2;

short Map [MapSize +1] [MapSize +1] / / 1 = busy / free = 0;
short xPrev [MapSize +1] [MapSize +1] / / abscissa of predecessor (x)
short yPrev [MapSize +1] [MapSize +1] / / ordinate of predecessors (y)
short dPrev [MapSize +1] [MapSize +1] / / direction to predecessor (d = 0-7)
short F [MapSize] [MapSize] / / total path cost F
short list [MapSize] [MapSize] / / list belongs to: undef = 0 open = closed = 1 2


/ / ************************************************ ********************

short GetMap (short x, short y) / / state of the field: 0 = free, 1 = obstacle
{Return map [x + limit] [y + limit];}

void SetMap (short x, short y, short val)
{Map [+ limit x] [y + limit] = val;}

GetxPrev short (short x, short y) / / stored predecessor: x-coord.
XPrev {return [x + limit] [y + limit];}

void SetxPrev (short x, short y, short XPTR)
XPrev {[x + limit] [y + limit] = XPTR;}

GetyPrev short (short x, short y) / / stored predecessor: y-coord.
YPrev {return [x + limit] [y + limit];}

void SetyPrev (short x, short y, short yPtr)
YPrev {[x + limit] [y + limit] = yPtr;}

GetdPrev short (short x, short y) / / stored predecessor: Direction
DPrev {return [x + limit] [y + limit];}

void SetdPrev (short x, short y, short d)
DPrev {[x + limit] [y + limit] = d;}

GETF short (short x, short y) / / path cost F
{Return F [x + limit] [y + limit];}

void SetF (short x, short y, short val)
{F [x + limit] [y + limit] = val;}

short GetList (int x, int y) / / List component parts (0, 1, 2)
{Return List [+ limit x] [y + limit];}

SetList void (int x, int y, int val)
{List [+ limit x] [y + limit] = val;}


/ / ************************************************ ********************
/ / ************************************************ ********************

void SetPixel (short x, short y) / / transformation of the coordinate systems
{
  int xPix, yPix;

  xPix = limit +1 + x;
  yPix = limit +1 + y;
  nxtSetPixel (xPix, yPix);
}

/ / ************************************************ ********************

void drawRect (short x, short y, short rad) / / Draw a square around the center
{
  int xPix, yPix;

  xPix = limit + x +1 / / CenterX
  yPix = limit + y +1; / / CenterY
  nxtDrawRect (xPix wheel, yPix + rad + rad xPix, yPix-rad);
}

/ / ************************************************ ********************

void Clear pixels (short x, short y) Delete / / map pixels
{
  int xPix, yPix;

  xPix = limit +1 + x;
  yPix = limit +1 + y;
  nxtClearPixel (xPix, yPix);
}

/ / ************************************************ ********************
/ / ************************************************ ********************

int GetNx (short x, short pos) / / 3 1 2 adjacent x-coordinate
{/ / 4 0
   int nx, / / ​​5 6 7

   nx = MadlyEnormous / / if invalid neighbor
   if (((pos == 1) | | (pos == 0) | | (pos == 7)) && (x <limit-1))
     {Nx = x +1;}
   else
   if ((pos == 2) | | (pos == 6))
     {Nx = x;}
   else
   if (((pos == 3) | | (pos == 4) | | (pos == 5)) && (x> (-limit)))
     {Nx = x-1;}

   return nx;
}

/ / ************************************************ ********************

int GetNy (short y, short pos) / / 3 1 2 adjacent y coordinates
{/ / 4 0
   int ny, / / ​​5 6 7

   ny = MadlyEnormous / / if invalid neighbor
   if (((pos == 1) | | (pos == 2) | | (pos == 3)) && (y <limit-1))
     {Ny = y +1;}
   else
   if ((pos == 4) | | (pos == 0))
     Ny = {y;}
   else
   if (((pos == 5) | | (pos == 6) | | (pos == 7)) && (y> (-limit)))
     {Ny = y-1;}

   return ny;
}

/ / ************************************************ ********************

CloseToTheEdge bool (short x, short y, short you) / / 3 1 2 cutting-edge no obstacle
{/ / 4 0
   bool edge; / / 5 6 7
   short xm, xp, ym, yp;

   edge = false, / / ​​only take into account diagonal step)!

     xm = (xm>-limit x-1: x?);
     xp = (xp <limit-1 x +1: x?);
     ym = (ym>-limit y-1: y?);
     yp = (yp <1 limit-y +1, y?);


     if (dir == 1) {edge = ((GetMap (xp, y) == occ) | | (GetMap (x, y p) == occ));} / / 0 2
     else
     if (dir == 3) {edge = ((GetMap (x, y p) == occ) | | (GetMap (xm, y) == occ));} / / 2 4
     else
     if (dir == 5) {edge = ((GetMap (xm, y) == occ) | | (GetMap (x, ym) == occ));} / / 4 6
     else
     if (dir == 7) {edge = ((GetMap (x, ym) == occ) | | (GetMap (xp, y) == occ));} / / 6 0

   return edge;
}



/ / ************************************************ ********************

void expandNode (short xa, ya short) / / check the neighboring fields of (xa, ya)
{
  short F, d, k, v, w, z;

  for (d = 0, d <= 7, d + +)
  {
       GetNx v = (xa, d); go through / / neighbors: 3 2 1
       w = GetNy (ya, d) / / 4 0
                      / / 5 6 7
       if ((d == 0) | | (d == 2) | | (d == 4) | | (d == 6)) / / ((p% 2) == 0 is faulty!)
       {K = 10} / / costs straight step: 10
       else
       {
         k = 14, / / ​​costs diagonal step: 15
         if (CloseToTheEdge (xa, ya, d)) continue; / / diagonal is prohibited on obstacle edge
       }

       if ((v <MadlyEnormous) && (w <MadlyEnormous)) / / Valid national neighbor?
       {
         z = GetMap (v, w) / / obstacle exists?
         if ((z == 0) && (! (GetList (v, w) == 2))) / / free and not in a closed list
         {
           GETF F = (xa, ya) + k / / F: cost (v, w) over current square (xa, ya)

           if ((GetList (v, w) == 1)) / / if NOT open list
           {
              SetList (v, w, 1) / / then take over the open
              SetxPrev (v, w, xa), / / ​​Predecessor: ^ x-date
              SetyPrev (v, w, ya), / / ​​Predecessor: ^ y-current
              SetdPrev (v, w, d) / / direction to the predecessor (0-7)
              if (d == GetdPrev (xa, ya)) F = 2, / / ​​cost less if no Last change
              Set Button F (v, w, F) / / Total cost of
           } / / If (not open)
           else
           if (GetList (v, w) == 1) / / but if already in the open list
            {
               if (F <= GETF (v, w)) / / infrastructure costs compare G there:
                                             / / New way there: lower costs than old road costs?
               {/ / Then storing overwrite + new path in the square
                 SetxPrev (v, w, xa), / / ​​Predecessor: ^ x-date
                SetyPrev (v, w, ya), / / ​​Predecessor: ^ y-current
                SetdPrev (v, w, d) / / direction to the predecessor (0-7)
                if (d == GetdPrev (xa, ya)) F = 2 ;/ / cost less if no Last change
                Set Button F (v, w, F) / / Total cost of
               } / / If (new way better)
           } / / Else if (already open)

         } / / If (not open, not closed)
       } / / If (Valid national neighbor)

  } / / For

} / / ExpandNode

/ / ************************************************ ********************
/ / ************************************************ ********************

void A_Stern (Xstart short, short yStart, xZiel short, short yZiel)
{
  short i, j, c = 0, xAkt, YAKT, xOld, yold, xd, yd, XDA, YDA, FMin = MadlyEnormous;


  xAkt xZiel = / / we start backwards and start the search at the goal!
  YAKT yZiel = / / The square, from which is sought is called "current square"

  SetList (xAkt, YAKT, 1) / / act. Square in Open list (= 1)
  SetF (xAkt, YAKT, 0) / / costs F = G+ H, G = 0, H = 0;
  Take over / / current square in closed list; SetList (xAkt, YAKT, 2)
  xd = abs (xZiel-Xstart);
  yd = abs (yZiel-yStart);
     / / While the other end (the start) was not found:

  while (GetList (Xstart, yStart)! = 2)
  {
    FMin = MadlyEnormous;
     xOld = xAkt;
     yold = YAKT;
    / / Search in the open list (= 1) is the smallest field with F

    if ((xZiel> = Xstart) && (yZiel> = yStart)) {
    for (i =-limit, i <limit; i + +)
     {
        for (j =-limit j <limit j + +)
       {
          if ((GetList (i, j) == 1) && (GETF (i, j) <FMin))
         {
            GETF FMin = (i, j);
           xAkt = i / / if found: take it as the current square
           YAKT = j;
           XDA = abs (i-Xstart);
           YDA = abs (j-yStart);
             if ((XDA <xd) | | (YDA <yd))
              {
                SetList (xAkt, YAKT, 2) / / current list enclosed in square
              SetPixel (xAkt, YAKT);
              Examine / / neighboring fields; expandNode (xAkt, YAKT)
             + c = 1;
           }
         } / / If
       } / / For j
       SetList (xAkt, YAKT, 2) / / current list enclosed in square
       SetPixel (xAkt, YAKT);
       Examine / / neighboring fields; expandNode (xAkt, YAKT)
      + c = 1;
     } / / For i

    } / / If (direction)
    else
    if ((xZiel <Xstart) && (yZiel <yStart)) {
    for (i = limit-1; i> = (-limit), i = 1)
     {
        for (j = limit-1; j> = (-limit), j = 1)
       {
          if ((GetList (i, j) == 1) && (GETF (i, j) <FMin))
         {
            GETF FMin = (i, j);
           xAkt = i / / if found: take it as the current square
           YAKT = j;
           XDA = abs (i-Xstart);
           YDA = abs (j-yStart);
             if ((XDA <xd) | | (YDA <yd))
              {
                SetList (xAkt, YAKT, 2) / / current list enclosed in square
              SetPixel (xAkt, YAKT);
              Examine / / neighboring fields; expandNode (xAkt, YAKT)
             + c = 1;
           }
         } / / If
       } / / For j
       SetList (xAkt, YAKT, 2) / / current list enclosed in square
       SetPixel (xAkt, YAKT);
       Examine / / neighboring fields; expandNode (xAkt, YAKT)
      + c = 1;
     } / / For i

    } / / If (direction)

    else
    if ((xZiel <xStart)&&(yZiel> yStart =))
    {
    for (i = limit-1; i> = (-limit), i = 1)
    {
        for (j =-limit j <limit j + = 1)
       {
          if ((GetList (i, j) == 1) && (GETF (i, j) <FMin))
         {
            GETF FMin = (i, j);
           xAkt = i / / if found: take it as the current square
           YAKT = j;
           XDA = abs (i-Xstart);
           YDA = abs (j-yStart);
             if ((XDA <xd) | | (YDA <yd))
              {
                SetList (xAkt, YAKT, 2) / / current list enclosed in square
              SetPixel (xAkt, YAKT);
              Examine / / neighboring fields; expandNode (xAkt, YAKT)
             + c = 1;
           }
         } / / If
       } / / For j
       SetList (xAkt, YAKT, 2) / / current list enclosed in square
       SetPixel (xAkt, YAKT);
       Examine / / neighboring fields; expandNode (xAkt, YAKT)
      + c = 1;
     } / / For i

    } / / If (direction)

    else
    if ((xZiel> = Xstart) && (yZiel <yStart))
    {
    for (i =-limit, i <limit; i + = 1)
     {
        for (j = limit-1; j> = (-limit), j = 1)
       {
          if ((GetList (i, j) == 1) && (GETF (i, j) <FMin))
         {
            GETF FMin = (i, j);
           xAkt = i / / if found: take it as the current square
           YAKT = j;
           XDA = abs (i-Xstart);
           YDA = abs (j-yStart);
             if ((XDA <xd) | | (YDA <yd))
              {
                SetList (xAkt, YAKT, 2) / / current list enclosed in square
              SetPixel (xAkt, YAKT);
              Examine / / neighboring fields; expandNode (xAkt, YAKT)
             + c = 1;
           }
         } / / If
       } / / For j
       SetList (xAkt, YAKT, 2) / / current list enclosed in square
       SetPixel (xAkt, YAKT);
       Examine / / neighboring fields; expandNode (xAkt, YAKT)

      + c = 1;
     } / / For i


    } / / If (direction)
    println (1, "act (X, Y)% 4d% 4d", xAkt, YAKT) / / display of xAkt, YAKT
    printXY (46, 20, "paths =% 4d", c); / / display the number of cycles / paths

    if ((== xAkt xOld) && (YAKT == yold) && (FMin == MadlyEnormous))
     {
        println (2, "% s", "No Way!");
        PlaySound (soundBeepBeep);
        Wait10Msec (200);
        break;
     }

  } / / While

  printXY (46,10, "% s finished", "") / / display when finished
  Clear pixels (Xstart, yStart);
  Wait10Msec (200);

}

/ / ************************************************ ********************
/ / ************************************************ ********************


draw drawmap sub () / / obstacles and start / finish
{
   short x, y;

   nxtDrawRect (0, +1 MapSize, MapSize +1, 0), / / ​​Border

/ / ================================================ ==========================
   x = 8, / / ​​Set obstacle:
   for (y = -3, y <= 5, y + +) / / Here you can have fun and any
   Set {/ / obstacle points:
    SetMap (x, y, occ); / / a) enter in card (occ = 1 = occupied)
    Paint / / b) on display; SetPixel (x, y)
  }
  y = 9;
  for (x = -8, x <= 7, x + +) / / again more
   Set {/ / obstacle points:
    SetMap (x, y, occ); / / a) enter in card (occ = 1 = occupied)
    Paint / / b) on display; SetPixel (x, y)
  }

  y = -5;
  for (x = 0, x <= 5, x + +) / / again more
   Set {/ / obstacle points:
    SetMap (x, y, occ); / / a) enter in card (occ = 1 = occupied)
    Paint / / b) on display; SetPixel (x, y)
  }

  y = 1;
  for (x = 10, x <= 12, x + +) / / again more
   Set {/ / obstacle points:
    SetMap (x, y, occ); / / a) enter in card (occ = 1 = occupied)
    Paint / / b) on display; SetPixel (x, y)
  }

  y = 6;
  for (x = 8, x <= 14, x + +) / / again more
   Set {/ / obstacle points:
    SetMap (x, y, occ); / / a) enter in card (occ = 1 = occupied)
    Paint / / b) on display; SetPixel (x, y)
  }

/ / ================================================ ==========================

  Paint / / start as a point; SetPixel (Xstart, yStart)
  Paint / / target as a square; drawRect (xZiel, yZiel, 1)

}

/ / ************************************************ ********************
/ / ************************************************ ********************

void WalkThisWay () / / virtual ride the calculated route
{
  short x, y, xn, yn, you olddir, turn;

  x = Xstart;
  y = yStart;
  dir = 0;
  olddir = 0;
  turn = 0;
  / / PrintXY (46.10, "food =% 4d", GETF (Xstart, yStart));
  println (0, "Turn =% 4d", turn);
  println (1, "price =% 4d", dir);
  println (2, "Pos XY% 3d% 3d", x, y);
  wait1Msec (1000);

  while (! ((x == xZiel) && (y == yZiel)))
  {
    println (2, "Pos XY% 3d% 3d", x, y);
    SetPixel (x, y);
    wait1Msec (500);

    dir = 180 + (45 * GetdPrev (x, y));
    if (dir> = 360) dir = 360;

    turn = dir olddir;
    if (turn <-180) turn = turn +360;
    if (turn> 180) turn = 360 turn;

    println (0, "Turn =% 4d", turn);
    wait1Msec (500);

    println (1, "price =% 4d", dir);
      wait1Msec (500);

      olddir = dir;
      GetxPrev = xn (x, y);
      GetyPrev yn = (x, y);
      x = x;
      y = yn;

   }
  SetPixel (x, y);
  println (2, "Pos XY% 3d% 3d", x, y);
  printXY (46,30, "% s", "target");
  printXY (46,20, "% s", "reach");



}

/ / ************************************************ ********************

initVar sub ()
{
   short i, j;

   / / Different start / finish combinations

  Xstart = 0; yStart = 0; / / Define start point
  xZiel = 13; yZiel = 4, / / ​​define the target point

  / / Xstart = 13; yStart = 4 / / define start point
  / / XZiel = 0; yZiel = 0 / / Define target point

   / / Xstart = 0; yStart = 4 / / define start point
  / / XZiel = 13; yZiel = 0 / / Define target point

  / / Xstart = 13; yStart = 0 / / Define start point
  / / XZiel = 0; yZiel = 4, / / ​​define the target point

   for (i = 0; i <MapSize, i + +)
   {
      for (j = 0; j <MapSize j + +)
      {
        Map [i] [j] = 0 / / clear fields
        xPrev [i] [j] = 0, / / ​​Predecessor zero
      yPrev [i] [j] = 0;
      F [i] [j] = MadlyEnormous / / huge costs
      List [i] [j] = 0, / / ​​undefined list
    }
  }
}
/ / ************************************************ ********************

task main ()
{
  eraseDisplay ();
  initVar ();
  Drawmap ();

  A_Stern (Xstart, yStart, xZiel, yZiel);

  while (true)
  {
     eraseDisplay ();
    Drawmap ();
    WalkThisWay ();
    wait1Msec (2000);
  }
}
/ / ************************************************ ********************
/ / ************************************************ ********************

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / RobotC + h -.'s A little something that will be easier ... ;-) / /
/ / Version 0.5.8
/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /



/ / ================================================ ===================
/ / Mathematical constants and functions
/ / ================================================ ===================

/ / Euler number e:
const float e = 2.718281828450945235360287471352662497757247093699959574966967627724076630353547594571;

/ / ********************************************

int min (int a, int b)
{
   int m;
   m = (a <b, a: b?);
   return m;
}

/ / ********************************************

float min (float a, float b)
{
   float m;
   m = (a <b, a: b?);
   return m;
}

/ / ********************************************

int max (int a, int b)
{
   int m;
   m = (a> b a: b?)
   return m;
}

/ / ********************************************

float max (float a, float b)
{
   float m;
   m = (a> b a: b?)
   return m;
}

/ / ********************************************

int round (float f)
{
  if (f> 0) return (int) (f + 0.5);
  else return (int) (f - 0.5);
}

/ / ********************************************

/ / Arc tangent with Sonderfaellen, specified in degrees!
/ / X = y = opposite side to the adjacent side tan = y / x

atan2Degrees float (float x, float y)
{
  float phi, alpha / / phi = radians, alpha = degree;

   if (x> 0) {phi = atan (y / x);}
   else
   if ((x <0) && (y> = 0)) {phi = PI + atan (y / x);}
   else
   if ((x <0) && (y <0)) {phi = PI + atan (y / x);}
   else
   if ((x == 0) && (y> 0)) {phi = PI / 2;}
   else
   if ((x == 0) && (y <0)) {= phi-PI / 2;}
   else
   if ((x == 0) && (y == 0)) {phi = 0;}

   alpha = radiansToDegrees (phi);
   return alpha;
}

/ / ********************************************

IsInRange bool (int value, int base, int tolerance)
{
   return ((value> = base-tolerance) && (value <= base + tolerance));
}


/ / ================================================ ===================
/ / Procedures for sensors
/ / ================================================ ===================

SMuxPressed bool (int port, byte ch) / / MUX port on NXT (S1, S2, ...) / / ch: 0 = right, 1 = ch1, ch2 = 2, 3 = ch3

{
  int v;
   bool pressed;

   v = sensor value (port);
   if (ch == 0)
   Pressed = {(v <100);}
   else
  if (ch == 1)
   {Pressed = (IsInRange (v 775.30) | | IsInRange (v 523.25) | | IsInRange (v, 343.8) | | IsInRange (v, 275.8));}
  else
   if (ch == 2)
   {Pressed = (IsInRange (v, 620,40) | | IsInRange (v 523.25) | | IsInRange (v, 295.8) | | IsInRange (v, 275.8));}
  else
   if (ch == 3)
   {Pressed = (IsInRange (v, 363,15) | | IsInRange (v, 343.8) | | IsInRange (v, 295.8) | | IsInRange (v, 275.8));}
  return pressed;

/ / Table of values ​​for sensor-muxer (of course design-dependent)
/ / ************************************************ *************
/ /
/ / Rcx ch1 ch2 ch3 raw / / RCX WiderstMux defined as sensorRawValue
/ / 0 0 0 0 1023 / / 3 at Mux Einaenge AND rcx other via input
/ / 0 0 0 1 775 / / 1 each probe connected
/ / 0 0 1 0 620
/ / 0 0 1 1 523
/ / 0 1 0 0 363
/ / 0 1 0 1 343
/ / 0 1 1 0 295
/ / 0 1 1 1 275
/ / 1 0 0 0 79 / / from here the values ​​when the
/ / 1 0 0 1 64 / / 4 Button located on the rcx sensor input
/ / 1 0 1 0 74 / / pressured.
/ / 1 0 1 1 70 / / A distinction, whether then
/ / 1 1 0 0 69 / / at the same time, other key
/ / 1 1 0 1 55 / / are additionally pressured,
/ / 1 1 1 0 55 / / is then hardly possible.
/ / 1 1 1 1 55 / /

}

/ / ================================================ ===================
/ / Global procedures fue motor control
/ / ================================================ ===================

motSync void (int m, int s) / / sets sync master & slave
{
   if ((m == 0) && (s == 0))
   {NSyncedMotors = synchNone;}
   else
   if ((m == 0) && (s == 1))
   {NSyncedMotors = synchAB;}
   else
   if ((m == 0) && (s == 2))
   {NSyncedMotors = synchAC;}
   else
   if ((m == 1) && (s == 0))
   {NSyncedMotors = synchBA;}
   else
  if ((m == 1) && (s == 2))
   {NSyncedMotors = synchBC;}
   else
  if ((m == 2) && (s == 0))
   {NSyncedMotors = synchCA;}
   else
  if ((m == 2) && (s == 1))
   {NSyncedMotors = synchCB;}
   else nSyncedMotors = synchNone;
}


/ / ================================================ ===================
/ / ================================================ ===================