Diff: LabyLib.h

Revision:

0:810679c5659e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LabyLib.h	Sat Dec 29 11:37:34 2018 +0000
@@ -0,0 +1,229 @@
+#ifdef _LABYLIB_H_
+#else 
+#define _LABYLIB_H_
+#define LABY_X 16
+#define LABY_Y 16
+#define LABY_UP    8
+#define LABY_DOWN  2
+#define LABY_LEFT  4
+#define LABY_RIGHT 6
+
+
+#define CAP(x,m,M) (x=max(m,min(x,M)))
+extern Serial pc_uart;
+extern Serial bt_uart;
+
+class LabyLib {
+    bool labyrinthe[LABY_X][LABY_Y]; // false = wall, true=empty
+    char path[LABY_X][LABY_Y]; // false = wall, true=empty
+    char position_x, position_y;
+    char init_x, init_y;
+    char size_x, size_y;
+    char direction, init_direction;  // 2=down,4=left,8=up,6=right
+    char fill_line;
+    
+    int max(int a, int b) {return a>b?a:b;}
+    int min(int a, int b) {return a<b?a:b;}
+    
+   public:
+    LabyLib(char _size_x, char _size_y, char _position_x, char _position_y, char _direction) {
+        size_x     = _size_x+1;
+        size_y     = _size_y+1;
+        position_x = _position_x;
+        position_y = _position_y;
+        init_direction=direction  = _direction;
+        fill_line = 1;
+         CAP(size_x,0,LABY_X);
+        CAP(size_y,0,LABY_Y);
+        init_x=CAP(position_x,1,_size_x);
+        init_y=CAP(position_y,1,_size_y);
+ //       pc_uart.printf("LABY init %d x %d, pos = %d,%d, direction = %d\n\r",size_x,size_y,position_x,position_y, direction);
+      
+        if(direction!=LABY_UP && direction!=LABY_DOWN && direction!=LABY_LEFT && direction!=LABY_RIGHT) direction=LABY_UP;
+         
+        int i,j;
+        for(i=0;i<LABY_X;i++) {
+            labyrinthe[i][0]=false;
+            labyrinthe[i][size_x]=false;
+        }
+
+        for(j=0;j<LABY_Y;j++) { 
+            labyrinthe[0][j]=false;
+            labyrinthe[size_y][j]=false;
+        }
+
+       for(i=1;i<size_x;i++) 
+          for(j=1;j<size_y;j++) {
+            labyrinthe[i][j]=true;
+        }
+
+     }
+        
+    void FillLaby(unsigned int Line) { // Fill as binary 0x101001 means | |  | 1 being wall
+        if(fill_line>=size_y) return;
+        unsigned int caseX = 1<<((size_x-2)*4);
+        for(int i=1;i<size_x;i++) {
+             labyrinthe[i][fill_line] = !(Line & caseX);
+             caseX >>= 4;
+        }
+        fill_line++;
+        
+    }
+        
+    void printLaby() {
+        pc_uart.printf("LABY %d x %d, pos = %d,%d, direction = %d\n\r",size_x,size_y,position_x,position_y, direction);
+        for(int j=1;j<size_y;j++) {
+            char l=0;
+            for(int i=1;i<size_x;i++) {
+                if(i==position_x && j==position_y) {
+                    switch(direction) {
+                            case LABY_UP:l='^';break;
+                            case LABY_DOWN:l='v';break;
+                            case LABY_RIGHT:l='>';break;
+                            case LABY_LEFT:l='<';break;
+                            default:l='/';break;
+                    }  
+                 } else {
+                    if(labyrinthe[i][j]) l='.';
+                    else l='x';
+                 }
+                 pc_uart.putc(l);
+                bt_uart.putc(l);
+            }
+            pc_uart.printf("----\n\r");
+            bt_uart.printf("----\n\r");
+        }
+    }
+    bool atStart() {
+        if(position_x==init_x && position_y==init_y) return true;
+        return false;
+    }
+    
+    void setCell(int x, int y, bool wall) {  // 0 for cell, 1 for wall
+        if(x<0 || x>=size_x) return;
+        if(y<0 || y>=size_y) return;
+        labyrinthe[x][y]=!wall;
+    }
+        
+    bool goUP() {
+        bool value = labyrinthe[position_x][position_y-1];
+        if(value) position_y--;
+        return value;
+    }
+   
+    bool goDOWN() {
+        bool value = labyrinthe[position_x][position_y+1];
+        if(value) position_y++;
+        return value;
+    }
+   
+    bool goLEFT() {
+        bool value = labyrinthe[position_x-1][position_y];
+        if(value) position_x--;
+        return value;
+    }
+   
+    bool goRIGHT() {
+        bool value = labyrinthe[position_x+1][position_y];
+        if(value) position_x++;
+        return value;
+    }
+    
+    bool turnLEFT() {
+        static char leftT[4] = {6,2,8,4};
+        direction = leftT[direction/2-1];
+        return true;
+    }
+  
+    bool turnRIGHT() {
+        static char leftT[4]={4,8,2,6};
+        direction = leftT[direction/2-1];
+        return true;
+    }
+    
+    bool turnBACK() {
+        static char leftT[4]={8,6,4,2};
+        direction = leftT[direction/2-1];
+        return true;
+    }
+    
+    bool goForward() {
+        switch(direction) {
+           case LABY_UP:return goUP(); 
+           case LABY_DOWN:return goDOWN(); 
+           case LABY_RIGHT:return goRIGHT(); 
+           case LABY_LEFT:return goLEFT(); 
+           default:return goUP(); 
+        }
+    }
+   
+    bool goBackward() {
+        switch(direction) {
+           case LABY_UP:return goDOWN(); 
+           case LABY_DOWN:return goUP(); 
+           case LABY_RIGHT:return goLEFT(); 
+           case LABY_LEFT:return goRIGHT(); 
+           default:return goUP(); 
+        }
+    }
+        
+    unsigned int alignDirection(unsigned int target) {
+        unsigned int crossDir[4][4] = {
+            {8,6,4,2},
+            {4,8,2,6},
+            {6,2,8,4},
+            {2,4,6,8},
+            };
+        return crossDir[direction/2-1][target/2-1];
+    }
+    
+    
+    unsigned int alignStart() {
+        return alignDirection(init_direction);
+    }
+    
+    unsigned int goStart() { // assumption laby is linear
+        // path = 0 => not checked
+        // path = 1 => checked
+        // path = 2 => to be checked
+        // path = 8 => Target
+        int i,j,value;
+        bool found = false;
+        for(i=1;i<size_x;i++)
+            for(j=1;j<size_y;j++)
+                path[i][j]=0;
+        path[init_x][init_y]=2;
+        path[position_x][position_y]=8;
+        while(!found) {
+            for(j=1;j<size_y;j++) 
+            for(i=1;i<size_x;i++) {
+                if(path[i][j]==2) {
+                    // check Up, then right, then down, then left.
+                    // Check UP
+                    value = path[i][j-1]|!labyrinthe[i][j-1];
+                    if(value ==0) path[i][j-1]=2;
+                    else if(value == 8) return alignDirection(LABY_DOWN);
+                   // Check DOWN
+                    value = path[i][j+1]|!labyrinthe[i][j+1];
+                    if(value ==0) path[i][j+1]=2;
+                    else  if(value == 8) return alignDirection(LABY_UP);
+                   // Check RIGHT
+                    value = path[i+1][j]|!labyrinthe[i+1][j];
+                    if(value ==0) path[i+1][j]=2;
+                    else  if(value == 8) return alignDirection(LABY_LEFT);
+                   // Check LEFT
+                    value = path[i-1][j]|!labyrinthe[i-1][j];
+                    if(value ==0) path[i-1][j]=2;
+                    else if(value == 8)  return alignDirection(LABY_RIGHT);
+                    
+                    path[i][j]=1;
+                    
+                }
+             }
+        }
+     return LABY_UP;   
+    }
+
+};
+
+#endif
\ No newline at end of file