/*
 Path Following Robotic Arm (MeArm)
 pfodDevice for Arduino
 see www.pfod.com.au for more details

 * ===== pfod for Robotic Arm ====
// Using Serial and 9600 for send and receive
// Serial D0 (RX) and D1 (TX) on Arduino Uno, Micro, ProMicro, Due, Mega, Mini, Nano, Pro and Ethernet
// This code uses Serial so remove shield when programming the board

 (c)2015 Forward Computing and Control Pty. Ltd.
 This code may be freely used for both private and commerical use.
 Provide this copyright is maintained.
 */

#include <Servo.h>
#include <EEPROM.h>
#include "pfodParser.h"

pfodParser parser; // create a parser to handle the pfod messages

// uncomment this the following line to enable debugging
//#define DEBUG
unsigned long SLEW_RATE_INTERVAL = 6; // 6mS, must be > 2.5mS which is max pulse width for servo control
const size_t MAX_STEPS = 10; // can increase this to max 26 steps.  Uses cmds 'a' to 'z'
const int MAX_MOVEMENT_IN_MANUAL = 4; // how far can we move the arm each SLEW_RATE_INTERVAL
const int MAX_MOVEMENT_IN_RUN = 1; // how far can we move the arm each SLEW_RATE_INTERVAL


Servo rotationServo;
Servo inOutServo;
Servo upDownServo;
Servo gripperServo;
const int STARTING_POINT = DEFAULT_PULSE_WIDTH;
const int GRIPPER_STARTING_POINT = 1900; // <<<<<<< set this starting position once you have calibrated your gripper

long rotationPosition = STARTING_POINT;
long rotationCmd = STARTING_POINT;  //rotation
long inOutPosition = STARTING_POINT;
long inOutCmd = STARTING_POINT;
long upDownPosition = STARTING_POINT;
long upDownCmd = STARTING_POINT;
long gripperPosition = GRIPPER_STARTING_POINT;
long gripperCmd = GRIPPER_STARTING_POINT;

bool gotoChanged = false; // set to true if user selected any position button to goto
bool repeat = false; // set to true if user selects Run to repeat cycle of steps for ever

unsigned long slewRateTimer = 0;
bool havePositionError = false;
bool isRunning = false; // set if finishIdx set to != stepIdx and only cleared when havePositionError is false AND finishIdx == stepIdx OR stopped

/**
 * This armPositionStruct is used to store the way points for the arm movement
 * -ve rotation means not used
 */
struct armPositionStruct {
  long rotation; //
  long inOut; //
  long upDown; //
  long gripper;
  int idx; //
};

/**
 * Limit the in arg to +/- limit
 */
long limit(long in, long limit) {
  if (in > limit) {
    return limit;
  }
  if (in < -limit) {
    return -limit;
  }
  //else {
  return in;
}

size_t stepIdx = 0; // what path step are we on currently
size_t finishIdx = 0; // what path step do we want to stop on
size_t endIdx = 0; // 0 path step is always filled in --  endIdx is last valid
// create array to store path steps
struct armPositionStruct path_States[MAX_STEPS];  // Holds the steps

#ifdef DEBUG
// for debugging - not normally used
void printStep(struct armPositionStruct *state ) {
  parser.println();
  parser.print(F("Idx:")); parser.print(state->idx);
  parser.print(F(" rotation:")); parser.print(state->rotation);
  parser.print(F(" inOut:")); parser.print(state->inOut);
  parser.print(F(" upDown:")); parser.print(state->upDown);
  parser.print(F(" gripper")); parser.println(state->gripper);
}

// for debugging -- not normally used
void printAllSteps() {
  parser.println();
  parser.print(F("print all steps ===== current idx:")); parser.println(stepIdx);
  for (int i = 0; i < MAX_STEPS; i++) {
    armPositionStruct *state = path_States + i;
    if (state->rotation < 0) {
      break;
    }
    printStep(state);
  }
  parser.println(F("================="));
  parser.print(F("rotationCmd:")); parser.println(rotationCmd);
  parser.print(F("inOutCmd:"));  parser.println(inOutCmd);
  parser.print(F("upDownCmd:"));  parser.println(upDownCmd);
  parser.print(F("gripperCmd:"));  parser.println(gripperCmd);
  parser.println(F("================="));
}
#endif // DEBUG

void storePoint() {
  if (stepIdx >= MAX_STEPS) {
    return; //don't store
  }
  armPositionStruct *state = path_States + stepIdx;
  state->rotation = rotationCmd;
  state->inOut = inOutCmd;
  state->upDown = upDownCmd;
  state->gripper = gripperCmd;
  state->idx = stepIdx;
  // only forward is next location is end
  if (stepIdx == endIdx) {
    forward();
  }
  finishIdx = stepIdx;
#ifdef DEBUG
  parser.print(F("Stored:"));
  printStep(state);
#endif
}

/**
 * increase step if not at end
 *
 */
void forward() {
  if ((stepIdx + 1) < MAX_STEPS) {
    armPositionStruct *state = path_States + stepIdx + 1;
    if (state->rotation < 0) {
      // copy last location to this one
      copyState(path_States + stepIdx, path_States + stepIdx + 1);
      // now update
      stepIdx = stepIdx + 1;
      endIdx = stepIdx; // new end
    } else {
      stepIdx = stepIdx + 1;
    }
  }
  setPositions(path_States + stepIdx);
}

/**
 * Go back one step
 */
void reverse() {
  if (stepIdx > 0) {
    stepIdx--;
  }
  armPositionStruct *state = path_States + stepIdx;
  setPositions(state);
}

/**
 * copy and set current position
 */
void copyState(struct armPositionStruct *fromState, struct armPositionStruct *toState) {
  toState->rotation = fromState->rotation;
  toState->inOut = fromState->inOut;
  toState->upDown = fromState->upDown;
  toState->gripper = fromState->gripper;
}

/**
 * Set the end positions for current from this state
 * Will slew to these positions
 */
void setPositions(struct armPositionStruct *state) {
  rotationCmd = state->rotation;
  inOutCmd = state->inOut;
  upDownCmd = state->upDown;
  gripperCmd = state->gripper;
  havePositionError = true;
#ifdef DEBUG
  parser.print(F("setPositions:"));
  printStep(state);
#endif
}

/**
 * Clears the path and set idx 0 to STARTING_POINT
 * First stored path point will overried idx 0 starting point
 */
void clearPath() {
  for (int i = 0; i < MAX_STEPS; i++) {
    armPositionStruct *state = path_States + i;
    state->rotation = -1;
    state->idx = i;
  }
  stepIdx = 0; // clear idx
  // initialize the first point
  // will be overwritten by first store
  armPositionStruct *state = path_States + stepIdx;
  state->rotation = STARTING_POINT;
  state->inOut = STARTING_POINT;
  state->upDown = STARTING_POINT;
  state->gripper = GRIPPER_STARTING_POINT;
  setPositions(state); // move to the starting point
}

/**
 * Called from setup() do extra initialization
 */
void extraSetup() {
  clearPath();
  rotationServo.attach(6);
  rotationServo.writeMicroseconds(rotationCmd);
  inOutServo.attach(9);
  inOutServo.writeMicroseconds(inOutCmd);
  upDownServo.attach(10);
  upDownServo.writeMicroseconds(upDownCmd);
  gripperServo.attach(11);
  gripperServo.writeMicroseconds(gripperCmd);
  slewRateTimer = millis();
}

// the setup routine runs once on reset:
void setup() {
  Serial.begin(9600);
  for (int i = 6; i > 0; i--) {
    // wait a few secs to see if we are being programmed
    delay(500);
  }
  parser.connect(&Serial); // connect the parser to the i/o stream
  extraSetup();
}

/**
 * called each SLEW_RATE_INTERVAL to move arm a little
 * until reach commanded position
 */
void updateServoPosition() {
  int movement_limit = MAX_MOVEMENT_IN_MANUAL;
  if (isRunning) {
    // make the limit smaller for running mode
    movement_limit = MAX_MOVEMENT_IN_RUN;
  }
  havePositionError = false;
  long diff = (rotationCmd - rotationPosition);
  diff = limit(diff, movement_limit);
  havePositionError = havePositionError || (diff != 0);
  rotationPosition += diff;
  rotationServo.writeMicroseconds(rotationPosition);

  diff = (inOutCmd - inOutPosition);
  diff = limit(diff, movement_limit);
  havePositionError = havePositionError || (diff != 0);
  inOutPosition += diff;
  inOutServo.writeMicroseconds(inOutPosition);

  diff = (upDownCmd - upDownPosition);
  diff = limit(diff, movement_limit);
  havePositionError = havePositionError || (diff != 0);
  upDownPosition += diff;
  upDownServo.writeMicroseconds(upDownPosition);

  diff = (gripperCmd - gripperPosition);
  diff = limit(diff, movement_limit);
  havePositionError = havePositionError || (diff != 0);
  gripperPosition += diff;
  gripperServo.writeMicroseconds(gripperPosition);
}

/**
 * if not at finish index and have reached the commanded position for the current step
 * then move on to the next step
 * if at the finish idx and user has asked for movement (gotoChanged == true)
 * then re-asert the final state to move there.
 * This for the case where the user stops the run at a step before the step completes and then
 * selects that step as the end point.  In this case slew to stored postion for that step
 */
void updateArmPosition() {
  if (finishIdx != stepIdx) {
    if (!havePositionError) {
      //  moveOneStep();
      if (finishIdx > stepIdx) {
        stepIdx++;
      } else {
        stepIdx--;
      }
      if (stepIdx < 0) {
        stepIdx = 0;
      }
      if (stepIdx >= MAX_STEPS) {
        stepIdx = MAX_STEPS - 1;
      }
      if (stepIdx > endIdx) {
        stepIdx = endIdx;
      }
      armPositionStruct *state = path_States + stepIdx;
      setPositions(state);
      havePositionError = true;
    }
  }
  if ((finishIdx == stepIdx) && gotoChanged) {
    gotoChanged = false;
    armPositionStruct *state = path_States + stepIdx;
    setPositions(state);
  }
}

/**
 *  Each loop update arm commanded postion if needed
 *  if we are at the finish idx and there is no position error, the finished running, i.e. set isRunning = false
 *  Then each SLEW_RATE_INTERVAL move arm if current postion != commanded position.  This movement is limited
 */
void loop() {
  unsigned long ms = millis();
  updateArmPosition();
  if ((finishIdx == stepIdx) && (!havePositionError)) {
    if (repeat) {
      havePositionError = true; // force run again if only one point 0
      // cycle again to either start or end
      if (finishIdx == endIdx) {
        finishIdx = 0;
      } else {
        finishIdx = endIdx;
      }
    } else {
      isRunning = false;
    }
  }
  if ((ms - slewRateTimer) > SLEW_RATE_INTERVAL) {
    slewRateTimer = ms;
    updateServoPosition();
  }

  byte cmd = parser.parse(); // pass it to the parser
  // parser returns non-zero when a pfod command is fully parsed
  if (cmd != 0) { // have parsed a complete msg { to }
    byte* pfodFirstArg = parser.getFirstArg(); // may point to \0 if no arguments in this msg.
    long pfodLongRtn; // used for parsing long return arguments, if any
    if (('.' != cmd) && (isRunning)) {
      // was running and user pressed any (active) button then stop
      stop();
    } else {
      if ('.' == cmd) {
        // pfodApp has connected and sent {.} , it is asking for the main menu
        // send back the menu designed
        if (parser.isRefresh()) {
          sendShortMainMenuUpdate();
        } else {
          sendMainMenu();
        }

        // now handle commands returned from button/sliders
      } else if ('A' == cmd) { // user moved slider -- 'A '
        parser.parseLong(pfodFirstArg, &pfodLongRtn); // parse first arg as a long
        rotationCmd = pfodLongRtn;
        updateServoPosition();
        parser.print(F("{:"));
        send_updateA(true);
        parser.print('}');


      } else if ('B' == cmd) { // user moved slider -- 'B '
        parser.parseLong(pfodFirstArg, &pfodLongRtn); // parse first arg as a long
        inOutCmd = pfodLongRtn;
        updateServoPosition();
        parser.print(F("{:"));
        send_updateB(true);
        parser.print('}');


      } else if ('C' == cmd) { // user moved slider -- 'C '
        parser.parseLong(pfodFirstArg, &pfodLongRtn); // parse first arg as a long
        upDownCmd = pfodLongRtn;
        updateServoPosition();
        parser.print(F("{:"));
        send_updateC(true);
        parser.print('}');


      } else if ('D' == cmd) { // user moved slider -- 'D '
        parser.parseLong(pfodFirstArg, &pfodLongRtn); // parse first arg as a long
        gripperCmd = pfodLongRtn;
        updateServoPosition();
        parser.print(F("{:"));
        send_updateD(true);
        parser.print('}');

      } else if ('S' == cmd) { // user store point -- 'S '
        storePoint();
        sendMainMenuUpdate(); // full update

      } else if ('R' == cmd) { // user stop -- 'R '
        if (isRunning) {
          stop();
        } else {
          run();
        }

      } else if (('a' <= cmd) && ('z' >= cmd)) { // max 26 steps 'a' to 'z' cmds for the Position buttons
        // move to this position
        gotoPosition(cmd - 'a');
        sendShortMainMenuUpdate();

      } else if ('!' == cmd) {
        // CloseConnection command
        closeConnection(parser.getPfodAppStream());
      } else {
        // unknown command
        parser.print(F("{}")); // always send back a pfod msg otherwise pfodApp will disconnect.
      }
    }
  }
}

/**
 * run from current position to last position and back to start continually until stopped
 */
void run() {
  repeat = true;
  gotoPosition(endIdx); // goto end
  sendMainMenuUpdate();
}

/**
 * Stop immediately at current location by making the commanded position equal to the current.
 */
void stop() {
  repeat = false;
  rotationCmd = rotationPosition;  //rotation
  inOutCmd = inOutPosition;
  upDownCmd = upDownPosition;
  gripperCmd = gripperPosition;
  finishIdx = stepIdx;
  havePositionError = false;
  sendMainMenuUpdate();
}

// go from current idx to new i via positions
void gotoPosition(int newIdx) {
  if (newIdx < 0) {
    newIdx = 0;
  }
  if (newIdx >= MAX_STEPS) {
    newIdx = (MAX_STEPS - 1);
  }
  if (newIdx > endIdx) {
    newIdx = endIdx;
  }
  gotoChanged = true;
  finishIdx = newIdx;
  havePositionError = true;
  isRunning = true;
}

void closeConnection(Stream *io) {
  // add any special code here to force connection to be dropped
}

void sendMainMenu() {
  parser.print(F("{."));  // start a Menu screen pfod message
  send_menuContents(); // send the menu contents
  parser.print(F("}"));  // close pfod message
}

void sendMainMenuUpdate() {
  parser.print(F("{:"));  // start an Update Menu pfod message
  send_menuContents(); // send the menu contents
  parser.print(F("}"));  // close pfod message
}

void sendShortMainMenuUpdate() {
  if (!isRunning) {
    sendMainMenuUpdate(); // do full update at end of run
    return;
  }
  parser.print(F("{:"));  // start an Update Menu pfod message
  parser.print(F("<w><b>Robotic Arm Control\n"));
  parser.print(F("Position ")); parser.print(stepIdx + 1); parser.print(F(" of ")); parser.print(MAX_STEPS);
  parser.print(F("`-1000"));
  send_updateA(false);
  send_updateB(false);
  send_updateC(false);
  send_updateD(false);
  parser.print(F("}"));  // close pfod message
}

// modify this method if you need to update the menu to reflect state changes
void send_menuContents() {
  // send menu prompt
  parser.print(F("<w><b>Robotic Arm Control\n"));
  parser.print(F("Position ")); parser.print(stepIdx + 1); parser.print(F(" of ")); parser.print(MAX_STEPS);
  // send menu items
  // add refresh
  if (isRunning) {
    parser.print(F("`-1000"));
  } else {
    parser.print(F("`0"));
  }
  parser.print(F("|A~Rotation "));
  addDisable();
  parser.print(F("`"));
  parser.print(rotationCmd);
  parser.print(F("`2400`544~\302\260~0.09698276~-544"));
  parser.print(F("|B~Out "));
  addDisable();
  parser.print(F("`"));
  parser.print(inOutCmd);
  parser.print(F("`2400`544~%~0.05388~-544"));
  parser.print(F("|C~Up "));
  addDisable();
  parser.print(F("`"));
  parser.print(upDownCmd);
  parser.print(F("`2400`544~%~0.05388~-544"));
  parser.print(F("|D~Gripper "));
  addDisable();
  parser.print(F("`"));
  parser.print(gripperCmd);  // 2080 == closed , 1884 == open
  parser.print(F("`2080`1884~% Grip ~0.510204~-1884")); // adjust this for our gripper
  parser.print(F("|R~"));
  if (isRunning) {
    parser.print(F("Stop"));
  } else {
    parser.print(F("Run"));
  }
  addStoreButton();
  addLocationsMenus();
  // ============ end of menu item ===========
}

// modify this method if you need to update the menu to reflect state changes
void send_updateMenuContents() {
  // send menu prompt
  parser.print(F("<w><b>Robotic Arm Control\n"));
  parser.print(F("Position ")); parser.print(stepIdx + 1); parser.print(F(" of ")); parser.print(MAX_STEPS);
  // send menu items
  // add refresh
  parser.print(F("`1000"));
  parser.print(F("|A~Rotation "));
  addDisable();
  parser.print(F("`"));
  parser.print(rotationCmd);
  parser.print(F("|B~Out "));
  addDisable();
  parser.print(F("`"));
  parser.print(inOutCmd);
  parser.print(F("|C~Up "));
  addDisable();
  parser.print(F("`"));
  parser.print(upDownCmd);
  parser.print(F("|D~Gripper "));
  addDisable();
  parser.print(F("`"));
  parser.print(gripperCmd);  // 2080 == closed , 1884 == open
  addStoreButton();
  addLocationsMenus();
  // ============ end of menu item ===========
}

void addDisable() {
  if (isRunning) {
    parser.print(F("<ex i>"));
  }
}

void addStoreButton() {
  parser.print(F("|S~Store Point"));
  if (isRunning) {
    parser.print(F("<ex i>"));
  }
}

void addLocationsMenus() {
  for (int i = 0; i < MAX_STEPS; i++) {
    parser.print(F("|")); parser.print(char('a' + i)); parser.print(F("~<-3>"));
    if (i <= endIdx) {
      parser.print(F("<bg g>"));
    } else {
      parser.print(F("<bg bk><ex i>"));
    }
    parser.print(F("Position ")); parser.print(i + 1);
  }
}

void send_updateA(bool active) {
  parser.print(F("|A~Rotation "));
  if (!active) {
    parser.print(F("<ex i>"));
  }
  parser.print(F("`"));
  parser.print(rotationCmd);
}

void send_updateB(bool active) {
  parser.print(F("|B~Out "));
  if (!active) {
    parser.print(F("<ex i>"));
  }
  parser.print(F("`"));
  parser.print(inOutCmd);
}

void send_updateC(bool active) {
  parser.print(F("|C~Up "));
  if (!active) {
    parser.print(F("<ex i>"));
  }
  parser.print(F("`"));
  parser.print(upDownCmd);
}

void send_updateD(bool active) {
  parser.print(F("|D~Gripper "));
  if (!active) {
    parser.print(F("<ex i>"));
  }
  parser.print(F("`"));
  parser.print(gripperCmd);
}