// Flamewheel.ino
// sketch for DFRobot Flamewheel robot for use with pfodApp
// To program, remove one battery, close BLE connection and connect USB
// Select UNO board in Arduino IDE to program.
//
// Using Serial and 115200 baud for send and receive
/*
   (c)2014-2017 Forward Computing and Control Pty. Ltd.
   NSW Australia, www.forward.com.au
   This code is not warranted to be fit for any purpose. You may only use it at your own risk.
   This generated code may be freely used for both private and commercial use
   provide this copyright is maintained.
*/

#include "DFRobotRomeoBLEMini.h"
DFRobotRomeoBLEMini myDFRobotRomeoBLEMini;

#include <pfodParser.h>

// ======================
// used to suppress warning
#define pfod_MAYBE_UNUSED(x) (void)(x)

pfodParser parser("V1"); // create a parser to handle the pfod messages
/* use
  pfodParser parser(""); // when making code changes for testing
*/

pfodDwgs dwgs(&parser);  // drawing support

int cols = 23; //
int rows = 25;
int zeroCol = 11;
int zeroRow = 12;
int colTouched = zeroCol;
int rowTouched = zeroRow;
int col = 0;
int row = 0;
char touchZoneCmd = 'p';
int leftWheelSpeed = 0;
int rightWheelSpeed = 0;
int absLastSpeed = 0;
int absWheelSpeed = 0;
int lastWheelSpeed = 0;

unsigned long timer;
unsigned long TIME_OUT = 100; // 100ms

bool timerRunning = false;
void startTimer() {
  timer = millis();
  timerRunning = true;
}

void checkTimer() {
  if (timerRunning && (millis() - timer) > TIME_OUT) {
    timerRunning = false;
    calculateWheelSpeed();
  }
}


void stop() {
  colTouched = zeroCol;
  rowTouched = zeroRow;
  col = 0;
  row = 0;
  absLastSpeed = 0;
  absWheelSpeed = 0;
  lastWheelSpeed = 0;
  calculateWheelSpeed();
}

// calculate Wheel speed based on col and row
// Note the Flamewheel chokes if you try and start at top speed, stall current too high?
// so if this speed - last speed > 128 ramp it up
// also if go from forward to back quickly

void calculateWheelSpeed() {
  if (row > 0) {
    row += 1;
  }
  if (row < 0) {
    row -= 1;
  }
  int wheelSpeed = row * 23;  // 255 / (10+1) ~ 23
  // limit wheelSpeed
  if (wheelSpeed > 255) {
    wheelSpeed = 255;
  }
  if (wheelSpeed < -255) {
    wheelSpeed = -255;
  }

  absWheelSpeed = (wheelSpeed < 0) ? -wheelSpeed : wheelSpeed;

  // check for reverse direction or starting
  if (((lastWheelSpeed > 0) && (wheelSpeed < 0))
      || ((lastWheelSpeed < 0) && (wheelSpeed > 0 ))) {
    // if swapping direction go through zero
    absWheelSpeed = 0;
    startTimer(); // to set real speed
  } else if ((absLastSpeed == 0) && (absWheelSpeed > 0)) {
    // if starting over come stickion and limit stall current
    absWheelSpeed =  128;
    startTimer(); // to set real speed
  }

  wheelSpeed = (wheelSpeed < 0) ? -absWheelSpeed : absWheelSpeed;

  leftWheelSpeed = wheelSpeed;
  rightWheelSpeed = wheelSpeed;

  int leftRight = col * col;
  if (leftRight > 100) {
    leftRight = 100;
  }
  if (col < 0) {
    leftRight = -leftRight;
  }

  if (leftRight < 0) {
    // reduce leftWheelSpeed
    leftWheelSpeed = (int) ((leftWheelSpeed * (100 - (-leftRight))) / 100.0f );
  } else {
    // reduce rightWheelSpeed
    rightWheelSpeed = (int) ((rightWheelSpeed * (100 - leftRight)) / 100.0f );
  }
  myDFRobotRomeoBLEMini.speed(leftWheelSpeed, rightWheelSpeed);
  absLastSpeed = absWheelSpeed;
  lastWheelSpeed = wheelSpeed;
}

// the setup routine runs once on reset:
void setup() {
  stop();
  Serial.begin(115200);
  for (int i = 3; i > 0; i--) {
    // wait a few secs
    delay(1000);
  }
  parser.connect(&Serial); // connect the parser to the i/o stream
  // <<<<<<<<< Your extra setup code goes here
}

// the loop routine runs over and over again forever:
void loop() {
  uint8_t cmd = parser.parse(); // parse incoming data from connection
  // parser returns non-zero when a pfod command is fully parsed
  if (cmd != 0) { // have parsed a complete msg { to }
    uint8_t* pfodFirstArg = parser.getFirstArg(); // may point to \0 if no arguments in this msg.
    pfod_MAYBE_UNUSED(pfodFirstArg); // may not be used, just suppress warning
    long pfodLongRtn; // used for parsing long return arguments, if any
    pfod_MAYBE_UNUSED(pfodLongRtn); // may not be used, just suppress warning
    if ('.' == cmd) {
      // pfodApp has connected and sent {.} , it is asking for the main menu
      if (!parser.isRefresh()) {
        sendMainMenu(); // send back the menu designed
      } else {
        sendMainMenuUpdate(); // menu is cached just send update
      }
      stop(); // always start stopped

      // now handle commands returned from dwgs/button/sliders
    } else if ('A' == cmd) { // user touched menu item 'A'
      // in the main Menu of Flamewheel
      char dwgCmd = parser.parseDwgCmd(); // parse rest of dwgCmd, return first char of active cmd
      if (dwgCmd == (touchZoneCmd)) {
        colTouched = parser.getTouchedCol();
        rowTouched = parser.getTouchedRow();
        col = colTouched - zeroCol; // + = right
        row = -(rowTouched - zeroRow); // + == forward
        calculateWheelSpeed();
      }
      if (parser.getTouchType() == dwgs.UP) {
        // stop when user lifts finger off dwg
        stop();
      }
      sendDrawingUpdates_a();   // always send back a response or pfodApp will timeout

    } else if ('a' == cmd) { // pfodApp is asking to load dwg 'a'
      if (!parser.isRefresh()) { // not refresh send whole dwg
        sendDrawing_a();
      } else { // refresh just update drawing state
        sendDrawingUpdates_a();
      }

    } else if ('!' == cmd) {
      // CloseConnection command
      closeConnection(parser.getPfodAppStream());
      stop(); // stop when connection closed

    } else {
      // unknown command
      parser.print(F("{}")); // always send back a pfod msg otherwise pfodApp will disconnect.
    }
  }
  checkTimer();
  myDFRobotRomeoBLEMini.speed(leftWheelSpeed, rightWheelSpeed);
}

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

void sendDrawing_a() {
  dwgs.start(cols, rows, dwgs.WHITE); // background defaults to WHITE if omitted i.e. dwgs.start(50,30);
  parser.sendVersion(); // send the parser version to cache this image
  // draw the axies, leave space for the text
  dwgs.line().size(0, rows - 4).offset((cols) / 2.0, 2).send();
  dwgs.line().size(cols / 5.0f, 0).offset(cols / 2.0f - cols / 10.0f, (rows) / 2.0f).send();
  dwgs.line().size(cols / 5.0f, 0).offset(0, (rows) / 2.0f).send();
  dwgs.line().size(cols / 5.0f, 0).offset(cols - cols / 5.0f, (rows) / 2.0f).send();
  // add the labels
  dwgs.label().fontSize(-3).offset(cols / 2.0f, 1).text(F("Forward")).send();
  dwgs.label().fontSize(-3).offset(cols / 2.0f, rows - 1).text(F("Back")).send();
  dwgs.label().fontSize(-5).offset(cols / 2.0f - cols / 5.0f, (rows) / 2.0f).text(F("Stop")).send();
  dwgs.label().fontSize(-5).offset(cols / 2.0f + cols / 5.0f, (rows) / 2.0f).text(F("Stop")).send();
  // define the active touch zone and its cmd
  dwgs.touchZone().cmd(touchZoneCmd).size(cols, rows).filter(dwgs.DOWN + dwgs.DRAG + dwgs.UP).send();
  // close dwg
  dwgs.end();
}

void sendDrawingUpdates_a() {
  dwgs.startUpdate();
  // nothing to update
  dwgs.end();
}

void sendMainMenu() {
  // !! Remember to change the parser version string
  // every time you edit this method
  parser.print(F("{,")); // start a Menu screen pfod message
  // send menu background, format, prompt, refresh and version
  parser.print(F("~Flamewheel"));
  parser.print("`0"); // no auto refresh
  parser.sendVersion(); // send the menu version
  // send menu items
  parser.print(F("|+A~a")); // one menu item 'A' a dwg '+' with dwg load cmd 'a'
  parser.print(F("}")); // close pfod message
}

void sendMainMenuUpdate() {
  parser.print(F("{;"));
  // nothing to update here
  parser.print(F("}")); // close pfod message
  // ============ end of menu ===========
}