//
//  Arttu-1 v0.3 19.6.2019
//
//  This "mover" uses 360-degree servos as motors.
//
//  Commands:
//     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)
//     T     = trigger sonic sensor, i.e. measure distance (or t)
//     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 newValue to varaiable n (or nv)
//     C     = calibrate i.e. init some values (or c)
//
#include <Servo.h>
// ***** servo definitions *****
#define RIGHT_PIN        10    // right motor
#define LEFT_PIN         11    // left motor
#define RIGHT_FORWARDS   20
#define RIGHT_BACKWARDS 120
#define RIGHT_STOP       90
#define LEFT_FORWARDS   120
#define LEFT_BACKWARDS   20
#define LEFT_STOP        90
// ***** operation modes *****
#define MANUAL_MODE 0
#define AUTO_MODE   1
#define SAVE_MODE   2
// ***** indicator leds *****
#define RED_LED     9
#define GREEN_LED   8
#define YELLOW_LED  7
// **** ultra-sonic sensors ****
#define FRONT_SENSOR       1
#define LEFT_SENSOR        2
#define RIGHT_SENSOR       3
#define FRONT_TRIGGER_PIN 13
#define FRONT_ECHO_PIN    12
#define LEFT_TRIGGER_PIN   6
#define LEFT_ECHO_PIN      5
#define RIGHT_TRIGGER_PIN  4
#define RIGHT_ECHO_PIN     3
// ***** events *****
#define MAX_EVENTS    20     // max length of event queue
#define STOP_MOTORS    1     // stop both motors
#define CHECK_DISTANCE 2     // check front distance
#define CHECK_ADJUST   3     // check side distance (left or right)
#define EXEC_COMMAND   4     // execute command
//
#define TOO_NEAR       11     //************************ should change this to 11 ??? is now 7
#define STOP_LIMIT     2
#define DISTANCE_CHECK_INTERVAL 50
#define DISTANCE_CHECK_DELAY    20
#define TURN_LEFT_90    550
#define TURN_LEFT_180  1000
#define TURN_RIGHT_90   550
#define TURN_RIGHT_180 1000
#define TURN_EXTRA      50
#define ADJUST_TO_LEFT  1     // adjust type - to left
#define ADJUST_TO_RIGHT 2     // adjust type - to right
#define ADJUST_TIME     150   // time to adjust to left/right (ms)
#define ADJUST_INTERVAL 500   // left/right distances measuring interval (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 =  30;   // ignore left/right distancies greater than this (cm) ********* index 0 ***
int     ADJUST_VAL =    15;   // 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
//
Servo rightMotor; 
Servo leftMotor; 
//
#define COMMAND_TABLE_SIZE 256
char ch;
int v;   // number value of a command
int w;   // return value from processCommand()
int inx;
int distance;
int leftDistance;
int rightDistance;
int mode;
char commandTable[COMMAND_TABLE_SIZE] = "Q";
char prog0[128] = "1000F 750W 1000B 750W 1950L 750W  1000F 750W 1000B 750W 3900R 0SQ";
char prog1[128] = "0SQ";
char *programs[3];
int save_indx = 0;
bool ff = false;
bool started = false;
int newValue = 0;
int *variables[10];
//
int eventCount = 0;
unsigned long eT;
int eP;
int eP1;
int eP2;
unsigned long eventTime[MAX_EVENTS];  // time to active this event
int eventProgram[MAX_EVENTS];         // program to excute in activation
int eventParam1[MAX_EVENTS];          // parameter for the program
int eventParam2[MAX_EVENTS];          // second parameter for the program
//
//  ********* setup **********
//
void setup() 
{
  v = 0;
  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(FRONT_TRIGGER_PIN, OUTPUT);
  pinMode(FRONT_ECHO_PIN, INPUT);
  pinMode(LEFT_TRIGGER_PIN, OUTPUT);
  pinMode(LEFT_ECHO_PIN, INPUT);
  pinMode(RIGHT_TRIGGER_PIN, OUTPUT);
  pinMode(RIGHT_ECHO_PIN, INPUT);
//
  pinMode(GREEN_LED, OUTPUT); pinMode(YELLOW_LED, OUTPUT); pinMode(RED_LED, OUTPUT);
  digitalWrite(GREEN_LED, LOW);  digitalWrite(YELLOW_LED, LOW);  digitalWrite(RED_LED, LOW);
  rightMotor.attach(RIGHT_PIN);
  rightMotor.write(RIGHT_STOP);  
  leftMotor.attach(LEFT_PIN);
  leftMotor.write(LEFT_STOP); 
  Serial.begin(9600);
  Serial.println("Arttu-1 v0.3 19.6.2019");
  digitalWrite(YELLOW_LED, HIGH);
  mode = MANUAL_MODE;
  v = 0;
  processCommand('S');    // stop motors
}
// ********** 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) processEvent();
  delay(1);
} 

int processCommand(char c)
{
    w = -1;
    switch(c) 
    {
      case '0'...'9':     // ********** numbers 0-9 **********
        v = v * 10 + ch - '0';
        break;
      case ' ':
        break;
      case 'r':           // ********** turn right  **********
      case 'R':
        w = v;
        leftMotor.write(LEFT_FORWARDS);
        rightMotor.write(RIGHT_BACKWARDS);
        if (v > 0) addEvent(STOP_MOTORS, v, 0, 0);
        if (mode == MANUAL_MODE)
        {
          Serial.print(v);
          Serial.println(" - right");
        }
        v = 0;
        break;
      case 'l':         // ********** turn left **********
      case 'L':
        w = v;
        leftMotor.write(LEFT_BACKWARDS);
        rightMotor.write(RIGHT_FORWARDS);
        if (v > 0) addEvent(STOP_MOTORS, v, 0, 0);
        if (mode == MANUAL_MODE)
        {
          Serial.print(v);
          Serial.println(" - left");
        }
        v = 0;
        break;
      case 'w':        // ********** wait **********
      case 'W':
        delay(v);
        if (mode == MANUAL_MODE)
        {
          Serial.print(v);
          Serial.println(" - wait");
        }
        v = 0;
        break;
      case 'd':        // ********** detach motors (servos) **********
      case 'D':
        rightMotor.detach();
        leftMotor.detach();
        if (mode == MANUAL_MODE)
        {
          Serial.println("detach ok");
        }
        break;
      case 'h':        // ********** put pin HIGH **********
      case 'H':
        digitalWrite(v, HIGH);
        if (mode == MANUAL_MODE)
        {
          Serial.print(v);
          Serial.println(" - pin HIGH");
        }
        v = 0;
        break;
      case 'i':        // ********** put pin LOW **********
      case 'I':
        digitalWrite(v, LOW);
        if (mode == MANUAL_MODE)    
        {
          Serial.print(v);
          Serial.println(" - pin LOW");
        }
        v = 0;
        break;
      case 'f':        // ********** go forwards **********
      case 'F':
        w = v;
        leftMotor.write(LEFT_FORWARDS);
        rightMotor.write(RIGHT_FORWARDS);
        if (v > 0) addEvent(STOP_MOTORS, v, 0, 0);
        if (mode == MANUAL_MODE)    
        {
          Serial.print(v);
          Serial.println(" - forwards");
        }
        v = 0;
        break;
      case 'b':        // ********** go backwards **********
      case 'B':
        w = v;
        leftMotor.write(LEFT_BACKWARDS);
        rightMotor.write(RIGHT_BACKWARDS);
        if (v > 0) addEvent(STOP_MOTORS, v, 0, 0);
        if (mode == MANUAL_MODE)    
        {
          Serial.print(v);
          Serial.println(" - backwards");
        }
        v = 0;
        break;
      case 's':       // ********** stop motors **********
      case 'S':
        stopMotors();
        if (mode == MANUAL_MODE)    
        {
          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 front sonic sensor and measure distance **********
      case 'T':
        if (v == FRONT_SENSOR) distance = getDistance(FRONT_SENSOR);
        else if (v == LEFT_SENSOR) distance = getDistance(LEFT_SENSOR);
        else if (v == RIGHT_SENSOR) distance = getDistance(RIGHT_SENSOR);
        else distance = 0;
        if (mode == MANUAL_MODE) 
        {
          Serial.print("sensor-");
          Serial.print(v);
          Serial.print(" - ");
          Serial.println(distance);
        }
        v = 0;
        break;
      case 'p':       // ********** start program **********
      case 'P':   
        if (started) break;
        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 (mode == MANUAL_MODE)
        {
          Serial.print("quit program - ");
          Serial.println(v);
        } 
        v = 0;
        break;
      case 'g':    // go/stop (flip-flop)
      case 'G':
        if (started)   // stop now
        {
          removeEvent(CHECK_DISTANCE);
          removeEvent(CHECK_ADJUST);
          v = 0;
          processCommand('S');
          digitalWrite(GREEN_LED, LOW);  digitalWrite(YELLOW_LED, LOW);  digitalWrite(RED_LED, HIGH);
          started = false;
          Serial.println("stopped");
        }
        else          // start now
        {
          v = 0;
          processCommand('F');    // go forwards
          addEvent(CHECK_DISTANCE, DISTANCE_CHECK_INTERVAL, 0, 0);   // check front distance
          addEvent(CHECK_ADJUST, ADJUST_INTERVAL, 0, 0);   // check side distances
          digitalWrite(GREEN_LED, HIGH);  digitalWrite(YELLOW_LED, LOW);  digitalWrite(RED_LED, LOW);
          started = true;
          Serial.println("started");
        }
        v = 0;
        break;
      case 'N':
      case 'n':
        newValue = v;
        v = 0;
        break;
      case 'V':
      case 'v':
        if (v <0 || v >= NBR_VAR) 
        {
          Serial.print(v);
          Serial.println(" - illegal index");
        }
        else
        {
          *variables[v] = newValue;
          Serial.print(newValue);
          Serial.print(" saved to ");
          Serial.println(v);
        }
        v = 0;
        break;   
      case 'C':
      case 'c':
        Serial.println("C not yet implemented ...");
        break;  
      default:     // ********** undefined command **********
        if (mode == MANUAL_MODE)
        {
          if (c == 10 || c == 12) break;
          Serial.print(c);
          Serial.println(" - unknown command");
        }
        break;
    }
    return w;
}

int getDistance(int sensor)
{
  int d, d1, d2, d3;
        d1 = readDistance(sensor); delay(DISTANCE_CHECK_DELAY);
        d2 = readDistance(sensor); delay(DISTANCE_CHECK_DELAY);
        d3 = readDistance(sensor); delay(DISTANCE_CHECK_DELAY);
        if (d1 <= d2 && d1 >= d3) d = d1;
        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;
}

int readDistance(int sensor)
{
  int d;
  int t, e;

  switch (sensor)
  {
    case FRONT_SENSOR:
      t = FRONT_TRIGGER_PIN;
      e = FRONT_ECHO_PIN;
      break;
    case LEFT_SENSOR:
      t = LEFT_TRIGGER_PIN;
      e = LEFT_ECHO_PIN;
      break;
    case RIGHT_SENSOR:
      t = RIGHT_TRIGGER_PIN;
      e = RIGHT_ECHO_PIN;
      break;
    default:
      t = FRONT_TRIGGER_PIN;
      e = FRONT_ECHO_PIN;
      break;
 }     
   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;
}

void stopMotors()
{
   leftMotor.write(RIGHT_STOP);
   rightMotor.write(LEFT_STOP);
}

// ********* 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;
}

// ******* process event *******
void processEvent()
{
   int d;
   int f;
   switch (eP)
   {
    case STOP_MOTORS:
      stopMotors();
      break;
    case CHECK_DISTANCE:
      //  if distance > TOO_NEAR continue forwards
      //  else read distances to left and to right (in this order)
      //  if right distnce > left distance go forwards (my nose points already to right)
      //  else turn 180 degree to left and go forwards
      if (ff) { digitalWrite(YELLOW_LED, HIGH); ff = !ff; }
      else { digitalWrite(YELLOW_LED, LOW); ff = !ff; }
      d = getDistance(FRONT_SENSOR);
      if (d > TOO_NEAR) addEvent(CHECK_DISTANCE, DISTANCE_CHECK_INTERVAL, 0, 0);
      else 
      {
        stopMotors();
        if (d < STOP_LIMIT)
        {
          removeEvent(CHECK_ADJUST);
          break;
        }
        delay(TURN_EXTRA);
        leftDistance = getDistance(LEFT_SENSOR);
        rightDistance = getDistance(RIGHT_SENSOR);
        if (leftDistance > rightDistance) 
        {
          digitalWrite(RED_LED, HIGH);
          digitalWrite(GREEN_LED, LOW);
          turnLeft(TURN_LEFT_90);
        }
        else
        {
          digitalWrite(RED_LED, LOW);
          digitalWrite(GREEN_LED, HIGH);
          turnRight(TURN_RIGHT_90);
        }
        v = 0; processCommand('F');
        addEvent(CHECK_DISTANCE, DISTANCE_CHECK_INTERVAL, 0, 0);
        removeEvent(CHECK_ADJUST);
        addEvent(CHECK_ADJUST, ADJUST_INTERVAL, 0, 0);
      }
      break;
    case(CHECK_ADJUST):
      leftDistance = getDistance(LEFT_SENSOR);
      if (leftDistance > ADJUST_LIMIT) leftDistance = ADJUST_VAL;
      rightDistance = getDistance(RIGHT_SENSOR);
      if (rightDistance > ADJUST_LIMIT) rightDistance = ADJUST_VAL;
      if ((leftDistance-rightDistance)>ADJUST_DIFF) adjustDirection(ADJUST_TO_LEFT);
      else 
      {
      if ((rightDistance-leftDistance)>ADJUST_DIFF) adjustDirection(ADJUST_TO_RIGHT);
      }
      addEvent(CHECK_ADJUST, ADJUST_INTERVAL, 0, 0);
      break;  
    case(EXEC_COMMAND):
      ch = *(programs[eP1]+eP2);
      if (ch == 'Q' || ch == 'q') break;
      w = processCommand(ch);
      if (w>=0) eT = w+1;
      else eT=1;
      addEvent(EXEC_COMMAND, eT, eP1, eP2+1);  // next command
      break;
    default:
       break;
   }
}

void turnLeft(int d)
{
  v = 0; processCommand('L');
  delay(d);
  stopMotors();
  delay(TURN_EXTRA);
  return;
}

void turnRight(int d)
{
  v = 0; processCommand('R');
  delay(d);
  stopMotors();
  delay(TURN_EXTRA);
  return;
}

void adjustDirection(int side)
{
  switch (side)
  {
    case ADJUST_TO_LEFT:
      stopMotors();
      v = 0; processCommand('L');
      delay(ADJUST_TIME);
      v = 0; processCommand('F');
      break;
    case ADJUST_TO_RIGHT:
      stopMotors();
      v = 0; processCommand('R');
      delay(ADJUST_TIME);
      v = 0; processCommand('F');
      break;
    default:
      break;
  }
}