//
char title[] = "Arttu-2 v0.1 20.11.2019";
//
//  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-2 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)
//     C     = calibrate i.e. init some values (or c)
//     aaVnJ = set angle aa to servo n (or aavnj)
//     D     = debuging on/off (or d)
//     nnVZ  = set zero speed period, ms (or nnvz)
//

// ***** 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         50
// ***** operation modes *****
#define MANUAL_MODE 0
#define AUTO_MODE   1
#define SAVE_MODE   2
// ***** indicator leds *****
// currently none
#define TOO_NEAR        12     // cm
#define STOP_DELAY      50     // time to stop (milliseconds)
#define DISTANCE_CHECK_INTERVAL 500
#define DISTANCE_CHECK_DELAY    5
#define TURN_90       1000     // time to turn 90 degrees (ms)
#define TURN_180      2000     // time to turn 180 degrees (ms)
#define TURN_EXTRA      50
#define ADJUST_TO_LEFT   1     // adjust type - to left
#define ADJUST_TO_RIGHT  2     // adjust type - to right
#define ADJUST_TURN    500   // time to adjust to left/right (ms)
#define ADJUST_WAIT     50    // delay before measuring left/right distancies (ms)
// the following variables can be modified by command N & V
int     ADJUST_LIMIT =  20;   // ignore left/right distancies greater than this (cm) ********* index 0 ***
int     ADJUST_VAL =    20;   // 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 NBR_VAR         3     // nbr of variables which can be modified by commands N & V
//
#define COMMAND_TABLE_SIZE 256
bool left_back = false;    // true if left legs are in back position
bool right_back = false;   // true if right legs are in back position
bool go_command = false;   // true if go-command got, else false
char ch;
int inx;
int speedControlPeriod = 50;   // ms
int zeroSpeedPeriod = 0;       // 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;
int *variables[10];
//
// 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 command 

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;
  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];
  variables[0] = &ADJUST_LIMIT;
  variables[1] = &ADJUST_VAL;
  variables[0] = &ADJUST_DIFF;
  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);
  Serial.println(title);
  mode = MANUAL_MODE;
  addEvent(STOP_MOVING, 10, 0, 0);
}
// ********** 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 '0'...'9':     // ********** numbers 0-9 **********
        v = v * 10 + ch - '0';
        break;
      case ' ':
      case '*':
        break;
      case 'r':           // ********** turn right  **********
      case 'R':
        w = v;
        stopMoving();                                  // first stop possible previous movement
        addEvent(TURN_RIGHT, STOP_DELAY, 1, zeroSpeedPeriod);        // then ask for the turning movement
        if (v > 0)  addEvent(STOP_MOVING, v, 1, 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;
        stopMoving();                                 // first stop possible previous movement
        addEvent(TURN_LEFT, STOP_DELAY, 1, zeroSpeedPeriod);        // then ask for the turning movement
        if (v > 0)  addEvent(STOP_MOVING, v, 1, 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, 1, zeroSpeedPeriod);  // then ask for the forwards movement
        if (v > 0) addEvent(STOP_MOVING, v, 0, 0);  // ask for stopping after delay if needed
        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, zeroSpeedPeriod);  // then ask for the backwards movement
        if (v > 0) addEvent(STOP_MOVING, v, 0, 0);   // ask for stopping after delay if needed
        if (debuging)    
        {
          Serial.print(v);
          Serial.println(" - backwards");
        }
        v = 0;
        break;
      case 's':       // ********** stop moving **********
      case 'S':
        addEvent(STOP_MOVING, 1, 0, 0);
        if (debuging) Serial.println("stop");
        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;
          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;
          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 'C':
      case 'c':
        Serial.println("C not yet implemented ...");
        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;
      case 'z':
      case 'Z':
        if (val < speedControlPeriod) zeroSpeedPeriod = val;
        if (debuging)
        {
          Serial.print("zero speed ratio = ");
          Serial.print(zeroSpeedPeriod);
          Serial.print("/");
          Serial.println(speedControlPeriod);
        }
        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;
  unsigned long tt;
        tt = millis();
        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;
        if (debuging) Serial.println(millis()-tt);
        return d;
}

int readDistance(int t, int e)
{
  int d;
// this must be modified for three sensors !!! *******************************************************
   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()
{
  // 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);
}

void turnLeft()
{
  switch (eP1)
  {
    case 0:
      break;
    case 1:
      digitalWrite(LEFT_MOTOR1, HIGH);
      digitalWrite(LEFT_MOTOR2, LOW);
      digitalWrite(RIGHT_MOTOR1, LOW);
      digitalWrite(RIGHT_MOTOR2, HIGH);
      if (eP2 == 0) break;
      addEvent(TURN_LEFT, speedControlPeriod-eP2, 2, eP2);  // stop after delay (ms)
      break;
    case 2:
      digitalWrite(LEFT_MOTOR1, LOW);
      digitalWrite(LEFT_MOTOR2, LOW);
      digitalWrite(RIGHT_MOTOR1, LOW);
      digitalWrite(RIGHT_MOTOR2, LOW);
      addEvent(TURN_LEFT, eP2, 1, eP2);      // start again after delay eP2 (ms)
      break;
    default:
      break;
  }
}

void turnRight()
{
  switch (eP1)
  {
  case 0:
    break;
  case 1:
    digitalWrite(LEFT_MOTOR1, LOW);
    digitalWrite(LEFT_MOTOR2, HIGH);
    digitalWrite(RIGHT_MOTOR1, HIGH);
    digitalWrite(RIGHT_MOTOR2, LOW);
    if (eP2 == 0) break;
    addEvent(TURN_RIGHT, speedControlPeriod-eP2, 2, eP2);  // stop after delay (ms)
    break;
  case 2:
    digitalWrite(LEFT_MOTOR1, LOW);
    digitalWrite(LEFT_MOTOR2, LOW);
    digitalWrite(RIGHT_MOTOR1, LOW);
    digitalWrite(RIGHT_MOTOR2, LOW);
    addEvent(TURN_RIGHT, eP2, 1, eP2);      // start again after delay eP2 (ms)
    break;
  default:
    break;
  }
}

void moveForwards()
{
  switch (eP1)
  {
    case 0:
      break;
    case 1:
      digitalWrite(LEFT_MOTOR1, HIGH);
      digitalWrite(LEFT_MOTOR2, LOW);
      digitalWrite(RIGHT_MOTOR1, HIGH);
      digitalWrite(RIGHT_MOTOR2, LOW);
      if (eP2 == 0) break;
      addEvent(MOVE_FORWARDS, speedControlPeriod-eP2, 2, eP2);  // stop after delay (ms)
      break;
    case 2:
      digitalWrite(LEFT_MOTOR1, LOW);
      digitalWrite(LEFT_MOTOR2, LOW);
      digitalWrite(RIGHT_MOTOR1, LOW);
      digitalWrite(RIGHT_MOTOR2, LOW);
      addEvent(MOVE_FORWARDS, eP2, 1, eP2);      // start again after delay eP2 (ms)
      break;
    default:
      break;
  } 
}

void moveBackwards()
{
  switch(eP1)
  {
   case 0:
      break;
    case 1:
      digitalWrite(LEFT_MOTOR1, LOW);
      digitalWrite(LEFT_MOTOR2, HIGH);
      digitalWrite(RIGHT_MOTOR1, LOW);
      digitalWrite(RIGHT_MOTOR2, HIGH);
      if (eP2 == 0) break;
      addEvent(MOVE_BACKWARDS, speedControlPeriod-eP2, 2, eP2);  // stop after delay (ms)
      break;
    case 2:
      digitalWrite(LEFT_MOTOR1, LOW);
      digitalWrite(LEFT_MOTOR2, LOW);
      digitalWrite(RIGHT_MOTOR1, LOW);
      digitalWrite(RIGHT_MOTOR2, LOW);
      addEvent(MOVE_BACKWARDS, eP2, 1, eP2);      // start again after delay eP2 (ms)
      break;
    default:
      break;
  } 
}

void checkDistance()    // ********************************* must be modified ***************************
{
  if (!go_command) return;
  switch (eP1)
  {
    case 0:
      break;
    case 1:
      frontDistance = getDistance(FRONT_TRIGGER, FRONT_ECHO);
      if (debuging) { Serial.print("front - "); Serial.println(frontDistance); }
      if (frontDistance <= TOO_NEAR)
      {
        stopMoving();
        // should we turn to left or to right ?
        leftDistance = getDistance(LEFT_TRIGGER, LEFT_ECHO);
        rightDistance = getDistance(RIGHT_TRIGGER, RIGHT_ECHO);
        if (leftDistance > rightDistance) addEvent(TURN_LEFT, GEN_DELAY, 1, 0);   // to left
        else addEvent(TURN_RIGHT, GEN_DELAY, 1, 0);                               // to right
        addEvent(CHECK_DISTANCE, TURN_90, 2, 0);
        break;
      }      
      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
      {
        stopMoving();
        addEvent(TURN_LEFT, GEN_DELAY, 1, 0);
        addEvent(CHECK_DISTANCE, ADJUST_TURN, 2, 0);
      }
      else if ((rightDistance-leftDistance)>ADJUST_DIFF)  // adjust to right
      {
        stopMoving();
        addEvent(TURN_RIGHT, GEN_DELAY, 1, 0);
        addEvent(CHECK_DISTANCE, ADJUST_TURN, 2, 0);
      }
      else addEvent(CHECK_DISTANCE, DISTANCE_CHECK_INTERVAL, 1, 0);  // no adjusting needed
      break;
    case 2:  // actions after turning or adjusting
      v = 0; processCommand('F');
      addEvent(CHECK_DISTANCE, DISTANCE_CHECK_INTERVAL, 1, 0);
      break;
    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
}