//
char title[] = "Arttu-3 v0.1 23.1.2020";
//
//  This "mover" uses normal motors (two, left and right) to move.
//  It has three utrasonic sensors to measure the distancees to front, left and right.
//  The goal of Arttu-3 is to move as fast as possible through a simple 'labyrinth'.
//
//  Commands (can be given in upper or lower case):
//     n..n  = number value to be saved into variable v 
//     n..nR = turn to right n..n milliseconds (or n..nr)
//     n..nL = turn to left n..n milliseconds (or n..nl)
//     n..nF = forwards n..n milliseconds (or n..nf)
//     n..nB = backwards n..n milliseconds (or n..nb)
//     S     = stop motors (or s)
//     n..nW = wait n..n milliseconds (or n..nw)
//     nM    = change mode to n (0-manual, 1-auto, 2-save) (or nm)
//     nH    = put pin n HIGH (or nh)
//     nI    = put pin n LOW (or ni)
//     xVyT  = trigger sonic sensor, i.e. measure distance x=trigger pin, y=echo pin (or xvyt)
//     nP    = run program n (or np)
//     G     = go/stop flip-flop (or g)
//     n..nN = number (int) value to be saved into variable newValue (or nn)
//     nV    = save n to variable val (or nv)
//     aaVnJ = set angle aa to servo n (or aavnj)
//     D     = debuging on/off (or d)
//     nZ    = set speed and speed depending parameters (or nz)
//     xxVyA = set value xx to nearTable[y] (or xxvya)
//

// ***** motor pins *****
#define LEFT_MOTOR1        2   
#define LEFT_MOTOR2        3   
#define RIGHT_MOTOR1       4    
#define RIGHT_MOTOR2       5   
// ***** sensor pins
#define RIGHT_TRIGGER      6
#define RIGHT_ECHO         7
#define LEFT_TRIGGER       8
#define LEFT_ECHO          9
#define FRONT_TRIGGER     10
#define FRONT_ECHO        11
//
#define GEN_DELAY         100
// ***** operation modes *****
#define MANUAL_MODE 0
#define AUTO_MODE   1
#define SAVE_MODE   2
// ***** indicator leds *****
// currently none
//
#define TOO_NEAR        8
#define STOP_DELAY     100     // time to stop (milliseconds)
#define DISTANCE_CHECK_INTERVAL 200
#define DISTANCE_CHECK_DELAY    5
#define TURN_90_RIGHT  300
#define TURN_90_LEFT   350
#define TURN_180       850     // time to turn 180 degrees (ms)
#define TURN_EXTRA      50
#define ADJUST_TIME    100     // time to turn because of adjusting
#define ADJUST_TO_LEFT  -1     // adjust type - to left
#define ADJUST_TO_RIGHT  1     // adjust type - to right
#define ADJUST_WAIT     50    // delay before measuring left/right distancies (ms)
// the following variables can be modified by command N & V
int     ADJUST_LIMIT =  10;   // ignore left/right distancies greater than this (cm) ********* index 0 ***
int     ADJUST_VAL =    10;   // use this if left/right distance greater than ADJUST_LIMIT *** index 1 ***
int     ADJUST_DIFF =   4;    // difference between left and right distances ***************** index 2 ***
//
#define COMMAND_TABLE_SIZE 256
bool go_command = false;   // true if go-command got, else false
char ch;
int inx;
int speedControlPeriod = 50;   // ms
int frontDistance;
int leftDistance;
int rightDistance;
int mode;
int v = 0;    // current number value 
int val = 0;  // command V (or v) saves current value from v to this variable
int newValue = 0;
int w;        // return value from processCommand
char commandTable[COMMAND_TABLE_SIZE] = "Q";
char prog0[] = "10000F 1000W 8000L 1000W 8000R 1000W 10000B SQ";
char prog1[] = "5000W 5V11J 2000W 150V11j 2000W 80V11J Q";
char *programs[3];
int save_indx = 0;
bool ff = false;
bool debuging = false;
bool tracking = false;
int moving_direction = 0;  // 0-stopped, 1-forwards, 2-backwards, 3-turn to right, 4-turn to left
//                                                            3
int x_sum = 0;                             //  ^ y          <---
int y_sum = 0;                             //  |         4 |      ^
int cur_dir = 1; // 1=y, 2=x, 3=-y, 4=-x   //  |           |      | 2
int next_dir_right[] = {0,4,1,2,3};        //   ----> x    v ---> |
int next_dir_left[]  = {0,2,3,4,1};        //                 1
int cur_sum = 0;
//
// event data
//
typedef void (*EventFunction) ();
#define MAX_EVENTS    20     // max length of event queue
#define STOP_MOVING    1
#define MOVE_FORWARDS  2
#define MOVE_BACKWARDS 3
#define TURN_LEFT      4
#define TURN_RIGHT     5
#define CHECK_DISTANCE 6     // check distance
#define EXEC_COMMAND   8     // execute serie of commands
#define EXEC_ONE_CMD   9     // execute one command
#define TRACK_ME      10

int eventCount = 0;
unsigned long eT;
int eP;     // program nbr
int eP1;    // parameter 1
int eP2;    // parameter 2
EventFunction eventHandlers[MAX_EVENTS];  // pointers to the event handling routines (programs)
unsigned long eventTime[MAX_EVENTS];      // time to active this event
int eventProgram[MAX_EVENTS];             // program nbr to excute event (index to eventRoutines-table)
int eventParam1[MAX_EVENTS];              // parameter for the program
int eventParam2[MAX_EVENTS];              // second parameter for the program

//
//  ********* setup **********
//
void setup() 
{
  // init event handlers
  eventHandlers[STOP_MOVING] = stopMoving;   
  eventHandlers[MOVE_FORWARDS] = moveForwards; 
  eventHandlers[MOVE_BACKWARDS] = moveBackwards;
  eventHandlers[TURN_LEFT] = turnLeft;
  eventHandlers[TURN_RIGHT] = turnRight;
  eventHandlers[CHECK_DISTANCE] = checkDistance;
  eventHandlers[EXEC_COMMAND] = executeCommand;
  eventHandlers[EXEC_ONE_CMD] = executeOneCommand;
  eventHandlers[TRACK_ME] = trackMovement;
  v = 0;     // current value
  val = 0;   // command V (or v) saves current value from v to this
  programs[0] = &prog0[0];
  programs[1] = &prog1[0];
  programs[2] = &commandTable[0];
  pinMode(LEFT_MOTOR1, OUTPUT); pinMode(LEFT_MOTOR2, OUTPUT);
  pinMode(RIGHT_MOTOR1, OUTPUT); pinMode(RIGHT_MOTOR2, OUTPUT);
  pinMode(RIGHT_TRIGGER, OUTPUT); pinMode(RIGHT_ECHO, INPUT);
  pinMode(LEFT_TRIGGER, OUTPUT); pinMode(LEFT_ECHO, INPUT);
  pinMode(FRONT_TRIGGER, OUTPUT); pinMode(FRONT_ECHO, INPUT);
//
  Serial.begin(9600);
  addEvent(EXEC_ONE_CMD, 1000, '!', 0);
  mode = MANUAL_MODE;
}
// ********** loop **********
void loop() 
{  
  if (mode == MANUAL_MODE)
  {
    if ( Serial.available()) 
    {
      ch = Serial.read();
      processCommand(ch);
    }
  }
  else if (mode == AUTO_MODE)
  {
    inx = 0;
    ch = commandTable[inx];
    while ( ch != 'q' && ch != 'Q')
    {
      processCommand(ch);
      inx++; ch = commandTable[inx];
    }
    mode = MANUAL_MODE;
  }
  else if (mode == SAVE_MODE)
  {
    if (Serial.available())
    {
      ch = Serial.read();
      commandTable[save_indx] = ch; save_indx++;
      if (ch == 'q' || ch == 'Q') 
      {
        Serial.print(save_indx);
        Serial.println(" - save ok");
        mode = MANUAL_MODE;
      }
      if (save_indx >= COMMAND_TABLE_SIZE-1)
      {
        commandTable[save_indx] = 'Q';
        mode = MANUAL_MODE;
        Serial.println("command table full");
      }
    }
  }
  // check if any event waiting to be processed
  while (getEvent() > 0) eventHandlers[eP]();
  delay(1);
} 

int processCommand(char c)
{
    int d;
    w = -1;
    switch(c) 
    {
      case '!':
        Serial.println(title);
        break;
      case '0'...'9':     // ********** numbers 0-9 **********
        v = v * 10 + ch - '0';
        break;
      case ' ':
      case '*':
        break;
      case 'r':           // ********** turn right  **********
      case 'R':
        w = v;
        moving_direction = 3;
        stopMoving();                                  // first stop possible previous movement
        addEvent(TURN_RIGHT, STOP_DELAY, 0, 0);        // then ask for the turning movement
        if (w > 0)  addEvent(STOP_MOVING, w, 0, 0);    // ask for stopping after delay if needed        
        if (debuging)
        {
          Serial.print(v);
          Serial.println(" - right");
        }
        v = 0;
        break;
      case 'l':         // ********** turn left **********
      case 'L':
        w = v;
        moving_direction = 4;
        stopMoving();                                 // first stop possible previous movement
        addEvent(TURN_LEFT, STOP_DELAY, 0, 0);        // then ask for the turning movement
        if (w > 0)  addEvent(STOP_MOVING, w, 0, 0);   // ask for stopping after delay if needed
        if (debuging)
        {
          Serial.print(v);
          Serial.println(" - left");
        }
        v = 0;
        break;     
      case 'f':        // ********** move forwards **********
      case 'F':
        w = v;
        stopMoving();                               // first stop possible previous movement
        addEvent(MOVE_FORWARDS, STOP_DELAY, 0, 0);  // then ask for the forwards movement
        if (v > 0) addEvent(STOP_MOVING, v, 0, 0);  // ask for stopping after delay if needed
        moving_direction = 1;
        if (debuging)    
        {
          Serial.print(v);
          Serial.println(" - forwards");
        }
        v = 0;
        break;
      case 'b':        // ********** move backwards **********
      case 'B':
        w = v;
        stopMoving();                                // first stop possible previous movement
        addEvent(MOVE_BACKWARDS, STOP_DELAY, 1, 0);  // then ask for the backwards movement
        if (v > 0) addEvent(STOP_MOVING, v, 0, 0);   // ask for stopping after delay if needed
        moving_direction = 2;
        if (debuging)    
        {
          Serial.print(v);
          Serial.println(" - backwards");
        }
        v = 0;
        break;
      case 's':       // ********** stop moving **********
      case 'S':
        addEvent(STOP_MOVING, 1, 0, 0);
        v = 0;
        break;
      case 'm':       // ********** set mode **********
      case 'M':
        if (v == 0) mode = MANUAL_MODE;
        else if (v == 1) mode = AUTO_MODE;
        else if (v == 2) 
        {
          mode = SAVE_MODE; save_indx = 0;
        }
        else 
        {
          Serial.print(v);
          Serial.println(" - unknown mode");
        }
        v = 0;
        break;
      case 't':       // ********** trigger sonic sensor and measure distance **********
      case 'T':
        d = getDistance(val, v);   // val=trigger pin, v=echo pin
        if (debuging) 
        {
          Serial.print("sensor-");
          Serial.print(val);
          Serial.print(" - ");
          Serial.println(d);
        }
        v = 0;
        break;
      case 'p':       // ********** start program **********
      case 'P':   
        if (v>2)     // only programs 0, 1 and 2 exist
        {
          Serial.print("undef program - ");
          Serial.println(v);
        }
        else 
        {
          Serial.print("program - ");
          Serial.println(v);
          addEvent(EXEC_COMMAND, 0, v, 0); 
        }
        v = 0;
        break;
      case 'q':       // ********** stop (quit) program **********
      case 'Q':    
        if (debuging)
        {
          Serial.print("quit program - ");
          Serial.println(v);
        } 
        v = 0;
        break;
      case 'g':    // *********** go/stop (flip-flop) ************
      case 'G':
        if (go_command)   // stop now
        {
          removeEvent(CHECK_DISTANCE);
          v = 0;
          processCommand('S');
          go_command = false;
          removeEvent(TRACK_ME);
          tracking = false;
          moving_direction = 0;
          Serial.print('#'); Serial.println(cur_sum);
          Serial.print("x_sum="); Serial.println(x_sum);
          Serial.print("y_sum="); Serial.println(y_sum);
          if (debuging) Serial.println("go stopped *****");
        }
        else          // start now
        {
          v = 0;
          processCommand('F');    // go forwards
          addEvent(CHECK_DISTANCE, DISTANCE_CHECK_INTERVAL, 1, 0);   // check distances
          go_command = true;
          addEvent(TRACK_ME, 0, 1, 0);
          tracking = true;
          moving_direction = 1;
          cur_dir = 1;
          if (debuging) Serial.println("go started *****");
        }
        v = 0;
        break;
      case 'N':
      case 'n':
        newValue = v;
        v = 0;
        break;
      case 'V':
      case 'v':
        val = v;
        v = 0;
        break;   
      case 'w':        // ********** wait **********
      case 'W':
        delay(v);
        if (debuging)
        {
          Serial.print(v);
          Serial.println(" - wait");
        }
        v = 0;
        break;
      case 'h':        // ********** put pin HIGH **********
      case 'H':
        digitalWrite(v, HIGH);
        if (debuging)
        {
          Serial.print(v);
          Serial.println(" - pin HIGH");
        }
        v = 0;
        break;
      case 'i':        // ********** put pin LOW **********
      case 'I':
        digitalWrite(v, LOW);
        if (debuging)    
        {
          Serial.print(v);
          Serial.println(" - pin LOW");
        }
        v = 0;
        break;
      case 'd':    // debug on/off
      case 'D':
        debuging = !debuging;
        Serial.print("debuging "); Serial.println(debuging);
        break;
      default:     // ********** undefined command **********
        if (debuging)
        {
          if (c == 10 || c == 12) break;
          Serial.print(c);
          Serial.println(" - unknown command");
        }
        break;
    }
    return w;    // w = time value related to the command; e.g. command = 500F, w = 500
}

int getDistance(int t, int e)
{
  int d, d1, d2, d3;
  
        d1 = readDistance(t, e);
        // delay(DISTANCE_CHECK_DELAY);   // make three measurements
/*        d2 = readDistance(t, e); delay(DISTANCE_CHECK_DELAY);
        d3 = readDistance(t, e); delay(DISTANCE_CHECK_DELAY);
        if (d1 <= d2 && d1 >= d3) d = d1;                         // choose the middle value
        else if (d1 >= d2 && d1 <= d3) d = d1;
        else if (d2 <= d1 && d2 >= d3) d = d2;
        else if (d2 >= d1 && d2 <= d3) d = d2;
        else if (d3 >= d1 && d3 <= d2) d = d3;
        else d = d3;
        return d;
        */
        return d1;
}

int readDistance(int t, int e)
{
  int d;
   digitalWrite(t, LOW);
   delayMicroseconds(2);
   digitalWrite(t, HIGH);
   delayMicroseconds(10);
   digitalWrite(t, LOW);
   d = pulseIn(e, HIGH);
   d = d/58;   // distance in cent meters
   return d;
}


// ********* add event *******
int addEvent(int p, int t, int p1, int p2)
// p = program to execute in activation, t = activation time (ms),
// p1 = parameter for program p, p2 = second parameter for program p
// return value, 0 = ok, 1 = event que full
{
  unsigned long l;
  int i;
  int j;

  l = t;
  l = millis() + l;
  if (eventCount >= MAX_EVENTS) return 1; // error: too many events
  if (eventCount == 0)            // if no events then just add the first one
    {
    eventTime[0] = l;
    eventProgram[0] = p;
    eventParam1[0] = p1;
    eventParam2[0] = p2;
    eventCount = 1;
    return 0;
    }
// already some events; so put the new one into the right slot
  for (i = 0; i<eventCount; i++)  // and move other events forwards in the queue
    {
    if (l < eventTime[i])
      {
      for (j = eventCount; j>i; j--)
        {
        eventTime[j] = eventTime[j-1];
        eventProgram[j] = eventProgram[j-1];
        eventParam1[j] = eventParam1[j-1];
        eventParam2[j] = eventParam2[j-1];      
        }
      eventTime[i] = l;
      eventProgram[i] = p;
      eventParam1[i] = p1;
      eventParam2[i] = p2;
      eventCount++;
      return 0; 
      }
    }
  eventTime[eventCount] = l;
  eventProgram[eventCount] = p;
  eventParam1[eventCount] = p1;
  eventParam2[eventCount] = p2;
  eventCount++;
  return 0;
}

// ******* remove event *******
int removeEvent(int p)
// p = event program to be removed
// return value = nbr of removed events
{
  int i;
  int j;
  
  if (eventCount == 0) return 0;
  for (i=0; i<eventCount; i++)
  {
    if (eventProgram[i] == p) 
    {
      j = i+1;
      while (j<eventCount)
      {
        eventTime[j-1] = eventTime[j];
        eventProgram[j-1] = eventProgram[j];
        eventParam1[j-1] = eventParam1[j];
        eventParam2[j-1] = eventParam2[j];
        j++;
      }
      eventCount--;
            return 1;
    }
  }
  return 0;
}

// ******* get event *******
int getEvent()
//  get first event from event stack if there is any and its timer is <= than current time
//  return corresponding event nbr (eventProgram) or zero if no event
//  save program nbr and parameters to corresponding global variables 
{
  unsigned long t;
  int i;
  
  if (eventCount == 0) return 0;    // no event at all
  t = millis();
  // because events are in ascending order (by time) we need check only the first one
  if (t >= eventTime[0])        // some events but is it the time to activate it ?
    {
      eT = eventTime[0];      // yes, setup variables
      eP = eventProgram[0];
      eP1 = eventParam1[0];
      eP2 = eventParam2[0];
      i = 1;
      while(i<eventCount)   // and remove this event from the stack
        {
          eventTime[i-1] = eventTime[i];
          eventProgram[i-1] = eventProgram[i];
          eventParam1[i-1] = eventParam1[i];
          eventParam2[i-1] = eventParam2[i];
          i++;
        }
      eventCount--;   // update event count
      return eP;      // return event program of this event
    }
  else return 0;
}

void stopMoving()
{
  if (debuging) {
    Serial.println("stop moving");
    Serial.print("dir="); Serial.println(moving_direction);
  }
  // first remove other events
  removeEvent(MOVE_FORWARDS);
  removeEvent(MOVE_BACKWARDS);
  removeEvent(TURN_LEFT);
  removeEvent(TURN_RIGHT);
  digitalWrite(LEFT_MOTOR1, LOW);
  digitalWrite(LEFT_MOTOR2, LOW);
  digitalWrite(RIGHT_MOTOR1, LOW);
  digitalWrite(RIGHT_MOTOR2, LOW);
  if (moving_direction == 1)
  {
    addEvent(MOVE_BACKWARDS, 0, 0, 0);
    addEvent(STOP_MOVING, 100, 0, 0);
  }
  else if (moving_direction == 2)
  {
    addEvent(MOVE_FORWARDS, 0, 0, 0);
    addEvent(STOP_MOVING, 100, 0, 0);
  }
  moving_direction = 0;
}

void turnLeft()
{
  if (debuging) Serial.print("turn left");
//  if (tracking) Serial.print("#L");
   digitalWrite(LEFT_MOTOR1, HIGH);
   digitalWrite(LEFT_MOTOR2, LOW);
   digitalWrite(RIGHT_MOTOR1, LOW);
   digitalWrite(RIGHT_MOTOR2, HIGH);
}

void turnRight()
{
   if (debuging) Serial.print("turn right");
//   if (tracking) Serial.print("#R");
   digitalWrite(LEFT_MOTOR1, LOW);
   digitalWrite(LEFT_MOTOR2, HIGH);
   digitalWrite(RIGHT_MOTOR1, HIGH);
   digitalWrite(RIGHT_MOTOR2, LOW);
}

void moveForwards()
{
    if (debuging) Serial.println("move forwards");
    digitalWrite(LEFT_MOTOR1, HIGH);
    digitalWrite(LEFT_MOTOR2, LOW);
    digitalWrite(RIGHT_MOTOR1, HIGH);
    digitalWrite(RIGHT_MOTOR2, LOW);
}

void moveBackwards()
{
   if (debuging) Serial.println("move backwards");
   digitalWrite(LEFT_MOTOR1, LOW);
   digitalWrite(LEFT_MOTOR2, HIGH);
   digitalWrite(RIGHT_MOTOR1, LOW);
   digitalWrite(RIGHT_MOTOR2, HIGH);
}

// ************************ check distances *****************
void checkDistance()
{
  int d;
  
  if (!go_command) return;
  if (debuging) Serial.println("check distance");
  switch (eP1)
  {
    case 0:
      break;
    case 1:   // normal distance checkings
      if (debuging) {Serial.println("check distances"); }
      d = getDistance(FRONT_TRIGGER, FRONT_ECHO);
      if (debuging) {Serial.print("front="); Serial.println(d);}
      if (d < TOO_NEAR)
      {
        v = 0;
        stopMoving();
        addEvent(CHECK_DISTANCE, 2*STOP_DELAY, 2, 0);
        break;
      }
      // not too near, check need for adjusting
      d = checkSideDistances();      
      if (d == ADJUST_TO_LEFT)
      {
        stopMoving();
        if (debuging) Serial.println("adjust to left");
        addEvent(TURN_LEFT, 10, 0, 0);
        moving_direction = 4;
        addEvent(CHECK_DISTANCE, ADJUST_TIME, 3, 0);
      }
      else if (d == ADJUST_TO_RIGHT)
      {
        stopMoving();
        if (debuging) Serial.println("adjust to right");
        addEvent(TURN_RIGHT, 10, 0, 0);
        moving_direction = 3;
        addEvent(CHECK_DISTANCE, ADJUST_TIME, 3, 0);
      }
      else addEvent(CHECK_DISTANCE, DISTANCE_CHECK_INTERVAL, 1, 0);  // no adjusting needed
      break;
    case 2:  // stopped because of too near, now must turn
      leftDistance = getDistance(LEFT_TRIGGER, LEFT_ECHO);
      rightDistance = getDistance(RIGHT_TRIGGER, RIGHT_ECHO);
      if (debuging) {Serial.print("right="); Serial.println(rightDistance);}
      if (debuging) {Serial.print("left="); Serial.println(leftDistance);}
      if (leftDistance > rightDistance)
        {
          if (debuging) Serial.println("turn to left");
          turnLeft();
          if (tracking) Serial.print("#L");
          moving_direction = 4;
          cur_dir = next_dir_left[cur_dir];
          addEvent(STOP_MOVING, TURN_90_LEFT, 0, 0);
          addEvent(CHECK_DISTANCE, TURN_90_LEFT+GEN_DELAY, 3, 0);
        }
      else
        {
          if (debuging) Serial.println("turn to right");
          turnRight();
          if (tracking) Serial.print("#R");
          moving_direction = 3;
          cur_dir = next_dir_right[cur_dir];
          addEvent(STOP_MOVING, TURN_90_RIGHT, 0, 0);
          addEvent(CHECK_DISTANCE, TURN_90_RIGHT+GEN_DELAY, 3, 0);
        }
      break;
    case 3:   // actions after turning or adjusting
      v = 0; processCommand('F');
      addEvent(CHECK_DISTANCE, DISTANCE_CHECK_INTERVAL, 1, 0);
    default:
      break;    
  }
}

void executeCommand()
{
      ch = *(programs[eP1]+eP2);
      if (ch == 'Q' || ch == 'q') return;
      w = processCommand(ch);    // w = time value related to the command, e.g. command = 500F, w = 500
      if (w>=0) eT = w+1;
      else eT=1;
      addEvent(EXEC_COMMAND, eT, eP1, eP2+1);  // next command
}

void executeOneCommand()
{
  ch = eP1;
  processCommand(ch);
}

int checkSideDistances()
{
  leftDistance = getDistance(LEFT_TRIGGER, LEFT_ECHO);
  if (debuging) { Serial.print("left - "); Serial.println(leftDistance); }
  rightDistance = getDistance(RIGHT_TRIGGER, RIGHT_ECHO);
  if (debuging) { Serial.print("right - "); Serial.println(rightDistance); }
  if (leftDistance > ADJUST_LIMIT) leftDistance = ADJUST_VAL;
  if (rightDistance > ADJUST_LIMIT) rightDistance = ADJUST_VAL;
  if ((leftDistance-rightDistance)>ADJUST_DIFF)       // adjust to left
  {
    return ADJUST_TO_LEFT;
  }
  else if ((rightDistance-leftDistance)>ADJUST_DIFF)  // adjust to right
  {
    return ADJUST_TO_RIGHT;
  }
  else return 0;
}

void trackMovement()
{
  if (!tracking) return;
  if (moving_direction != 1) 
  {
    addEvent(TRACK_ME, 100, 0, 0);
    return;
  }
  switch (eP1)
  {
    case 0:
      if (cur_dir == 1) x_sum++;
      else if (cur_dir == 2) y_sum++;
      else if (cur_dir ==3) x_sum--;
      else if (cur_dir ==4) y_sum--;
      cur_sum++;
      if (cur_sum >= 9)
      {
        Serial.print('#'); Serial.println(cur_sum);
        cur_sum = 0;
      }
      addEvent(TRACK_ME, 100, 0, 0);
      break;
    case 1:    // init values
      cur_dir = 1;
      x_sum = 0;
      y_sum = 0;
      cur_sum = 0;
      addEvent(TRACK_ME, 100, 0, 0);
      break;
    default:
      break;
  }
}