// ------ Libraries and Definitions ------
#include "simpletools.h"
#include "arlodrive.h"
#define MY_CENTER_PULSE_1485 	1485
#define MY_PING_CAUTION_60 	60
#define MY_PING_DANGER_10 	10
#define MY_X_INDEX_0 	0
#define MY_Y_INDEX_1 	1
#define MY_PORT_INDEX_0 	0
#define MY_STARBOARD_INDEX_1 	1
#define MY_FRONT_INDEX_0 	0
#define MY_REAR_INDEX_1 	1
#define MY_RC_LOOP_INDEX_0 	0
#define MY_PING_LOOP_INDEX_1 	1
#define MY_STOPPED_LOOPS_THRESHOLD_2 	2
#define MY_MAX_ROBOT_SPEED_200 	200
#define MY_MAX_JOYSTICK_MOVEMENT_410 	410
#define MY_LOOP_PER_DEBUG_50 	50
#define MY_VERY_SLOW 	5
#define MY_HALF_DEADBAND_2 	2
#define MY_MAX_PING_SPEED_60 	60
#define MY_TURN_FROM_PING_75 	75
#include "ping.h"

// ------ Global Variables and Objects ------
int inputCommand[2];
int speedFromRc[2];
int speedFromRcRaw[2];
int finalSpeed[2];
int pingDistance[2];
int frontRearPing[2];
int cogLoops[2];
int oldLoops[2];
int loopDifference[2];
int frozenLoops[2];
int loopCountStopped[2];
int otherIndex[2];
int mainIndex;
int fastestSide;
int closestPing;
int aileron;
int scaledPingSpeed;
int elevator;
int PING_CONCERN_RANGE_50;
int rcIndex;

// ------ Function Declarations ------
void MonitorRcInput();
void CheckDebug();
void MonitorPings();
void PingForwardSpeed();
void printIndexSide();
void printIndexXy();
void printIndexFrontRear();
void FindFastestSide();
void printIndexLoop();
void FindClosestPing();
void WarningText();
void ReversePingSpeed();
void LoopText();

// ------ Main Program ------
int main() {

  // I use the NED coordinate system.
  // AKA North East Down.
  // Positive X is forward (north).
  // Positive Y is starboard (east).
  // Positive Z (if used) is below the robot (down).
  // "TURN_FROM_PING" is used to slow the wheel opposite of the obstacle.
  otherIndex[constrainInt(MY_PORT_INDEX_0, 0, 1)] = MY_STARBOARD_INDEX_1;
  otherIndex[constrainInt(MY_STARBOARD_INDEX_1, 0, 1)] = MY_PORT_INDEX_0;
  cogLoops[constrainInt(MY_RC_LOOP_INDEX_0, 0, 1)] = 0;
  cogLoops[constrainInt(MY_PING_LOOP_INDEX_1, 0, 1)] = 0;
  oldLoops[constrainInt(MY_RC_LOOP_INDEX_0, 0, 1)] = 0;
  oldLoops[constrainInt(MY_PING_LOOP_INDEX_1, 0, 1)] = 0;
  PING_CONCERN_RANGE_50 = (MY_PING_CAUTION_60 - MY_PING_DANGER_10);
  cog_run(MonitorRcInput, 128);
  cog_run(MonitorPings, 128);
  while(1) {
    CheckDebug();
    if ((frontRearPing[MY_FRONT_INDEX_0] < MY_PING_CAUTION_60  || (pingDistance[MY_PORT_INDEX_0] < MY_PING_CAUTION_60  || pingDistance[MY_STARBOARD_INDEX_1] < MY_PING_CAUTION_60))  && (speedFromRc[MY_PORT_INDEX_0] > 0  && speedFromRc[MY_STARBOARD_INDEX_1] > 0)) {
      PingForwardSpeed();
    }
    else if (frontRearPing[MY_REAR_INDEX_1] < MY_PING_CAUTION_60  && (speedFromRc[MY_PORT_INDEX_0] < 0  && speedFromRc[MY_STARBOARD_INDEX_1] < 0)) {
      ReversePingSpeed();
    }else {
      for (mainIndex = 0; mainIndex <= 1; mainIndex++) {
        finalSpeed[constrainInt(mainIndex, 0, 1)] = speedFromRc[mainIndex];

      }}

    drive_speed(finalSpeed[MY_PORT_INDEX_0], finalSpeed[MY_STARBOARD_INDEX_1]);
  }

}

// ------ Functions ------
void MonitorRcInput() {
  while(1) {
    // Removed throttle control from this version.
    aileron = (pulse_in(0, 1));
    elevator = (pulse_in(1, 1));
    // Using "MAX_ROBOT_SPEED" instead of "maxSpeed" from throttle.
    inputCommand[constrainInt(MY_CENTER_PULSE_1485, 0, 1)] = elevator - MY_CENTER_PULSE_1485;
    inputCommand[constrainInt(MY_CENTER_PULSE_1485, 0, 1)] = (aileron - MY_CENTER_PULSE_1485) * -1;
    speedFromRcRaw[constrainInt(MY_CENTER_PULSE_1485, 0, 1)] = inputCommand[MY_CENTER_PULSE_1485] + inputCommand[MY_CENTER_PULSE_1485];
    speedFromRcRaw[constrainInt(MY_CENTER_PULSE_1485, 0, 1)] = inputCommand[MY_CENTER_PULSE_1485] - inputCommand[MY_CENTER_PULSE_1485];
    for (rcIndex = MY_CENTER_PULSE_1485; rcIndex <= MY_CENTER_PULSE_1485; rcIndex++) {
      speedFromRc[constrainInt(rcIndex, 0, 1)] = (speedFromRcRaw[rcIndex] * MY_CENTER_PULSE_1485) / MY_CENTER_PULSE_1485;

    }cogLoops[MY_CENTER_PULSE_1485]++;
  }
}

void CheckDebug() {
  for (mainIndex = MY_RC_LOOP_INDEX_0; mainIndex <= MY_PING_LOOP_INDEX_1; mainIndex++) {
    frozenLoops[constrainInt(mainIndex, 0, 1)] = cogLoops[mainIndex] - oldLoops[mainIndex];

  }for (mainIndex = MY_RC_LOOP_INDEX_0; mainIndex <= MY_PING_LOOP_INDEX_1; mainIndex++) {
    loopDifference[constrainInt(mainIndex, 0, 1)] = frozenLoops[mainIndex] - oldLoops[mainIndex];
    if (1 == 0) {
      print("%s%d", "loopDiff[", mainIndex);
      print("%s%d\r", "] = ", loopDifference[mainIndex]);
    }


  }if (loopDifference[MY_RC_LOOP_INDEX_0] > MY_LOOP_PER_DEBUG_50  || loopDifference[MY_PING_LOOP_INDEX_1] > MY_LOOP_PER_DEBUG_50) {
    for (mainIndex = MY_RC_LOOP_INDEX_0; mainIndex <= MY_PING_LOOP_INDEX_1; mainIndex++) {
      if (loopDifference[mainIndex] == 0) {
        loopCountStopped[mainIndex]++;
        if (loopCountStopped[mainIndex] > MY_STOPPED_LOOPS_THRESHOLD_2) {
          WarningText();
        }

      }
      else {
        loopCountStopped[constrainInt(mainIndex, 0, 1)] = 0;
      }


    }print("%s%d\r", "Aileron = ", aileron);
    print("%s%d\r", "Elevator = ", elevator);
    for (mainIndex = 0; mainIndex <= 1; mainIndex++) {
      print(("inputCommand["));
      printIndexXy();
      print("%s%d\r", "] = ", inputCommand[mainIndex]);

    }for (mainIndex = 0; mainIndex <= 1; mainIndex++) {
      print(("sFRRaw["));
      printIndexSide();
      print("%s%d", "] = ", speedFromRcRaw[mainIndex]);
      print((", sFRc["));
      printIndexSide();
      print("%s%d", "] = ", speedFromRc[mainIndex]);
      print((", fSpeed["));
      printIndexSide();
      print("%s%d\r", "] = ", finalSpeed[mainIndex]);
      print(("pingDistance["));
      printIndexSide();
      print("%s%d\r", "] = ", pingDistance[mainIndex]);
      print(("frontRearPing["));
      printIndexFrontRear();
      print("%s%d\r", "] = ", frontRearPing[mainIndex]);

    }LoopText();
    for (mainIndex = MY_RC_LOOP_INDEX_0; mainIndex <= MY_PING_LOOP_INDEX_1; mainIndex++) {
      oldLoops[constrainInt(mainIndex, 0, 1)] = frozenLoops[mainIndex];

    }}

}

void MonitorPings() {
  // ping pin 14 in front
  // ping pin 16 left
  // 17 right ping pin
  // 15 rear ping pin
  while(1) {
    pingDistance[constrainInt(MY_PORT_INDEX_0, 0, 1)] = ping_cm(16);
    pingDistance[constrainInt(MY_STARBOARD_INDEX_1, 0, 1)] = ping_cm(17);
    frontRearPing[constrainInt(MY_FRONT_INDEX_0, 0, 1)] = ping_cm(14);
    frontRearPing[constrainInt(MY_REAR_INDEX_1, 0, 1)] = ping_cm(15);
    cogLoops[MY_PING_LOOP_INDEX_1]++;
  }
}

void PingForwardSpeed() {
  if (frontRearPing[MY_FRONT_INDEX_0] < MY_PING_DANGER_10  || (pingDistance[MY_PORT_INDEX_0] < MY_PING_DANGER_10  || pingDistance[MY_STARBOARD_INDEX_1] < MY_PING_DANGER_10)) {
    // One distance below danger level.
    // Too close. Backup.
    // Only allow negative speeds.
    for (mainIndex = 0; mainIndex <= 1; mainIndex++) {
      finalSpeed[constrainInt(mainIndex, 0, 1)] = 0;

    }}
  else if (pingDistance[MY_PORT_INDEX_0] < MY_PING_CAUTION_60  && pingDistance[MY_STARBOARD_INDEX_1] < MY_PING_CAUTION_60) {
    // Obstacle.
    // Slow down both wheels based on distance.
    FindFastestSide();
    FindClosestPing();
    // Scale allowed speed by distance to closest obstacle.
    if (frontRearPing[MY_FRONT_INDEX_0] < pingDistance[closestPing]) {
      scaledPingSpeed = (((frontRearPing[MY_FRONT_INDEX_0] - MY_PING_DANGER_10) * MY_MAX_PING_SPEED_60) / PING_CONCERN_RANGE_50);
    }
    else {
      scaledPingSpeed = (((pingDistance[closestPing] - MY_PING_DANGER_10) * MY_MAX_PING_SPEED_60) / PING_CONCERN_RANGE_50);
    }

    // Scale each speed by this new limit.
    for (mainIndex = MY_PORT_INDEX_0; mainIndex <= MY_STARBOARD_INDEX_1; mainIndex++) {
      finalSpeed[constrainInt(mainIndex, 0, 1)] = (speedFromRc[mainIndex] * scaledPingSpeed) / speedFromRc[fastestSide];

    }}else {
    // Only one obstacle.
    // Turn away from obstacle.
    // Reduce opposite speed to turn away fro obstacle.
    FindClosestPing();
    finalSpeed[constrainInt(otherIndex[closestPing], 0, 1)] = (speedFromRc[otherIndex[closestPing]] < ((speedFromRc[closestPing] * MY_TURN_FROM_PING_75) / 100) ? speedFromRc[otherIndex[closestPing]] : ((speedFromRc[closestPing] * MY_TURN_FROM_PING_75) / 100));
    finalSpeed[constrainInt(closestPing, 0, 1)] = speedFromRc[closestPing];
  }

}

void printIndexSide() {
  if (mainIndex == MY_STARBOARD_INDEX_1) {
    print(("Starboard"));
  }
  else {
    print(("Port"));
  }

}

void printIndexXy() {
  if (mainIndex == MY_Y_INDEX_1) {
    print(("Y"));
  }
  else {
    print(("X"));
  }

}

void printIndexFrontRear() {
  if (mainIndex == MY_REAR_INDEX_1) {
    print(("Rear"));
  }
  else {
    print(("Front"));
  }

}

void FindFastestSide() {
  if (speedFromRc[MY_PORT_INDEX_0] < speedFromRc[MY_STARBOARD_INDEX_1]) {
    fastestSide = MY_STARBOARD_INDEX_1;
  }
  else {
    fastestSide = MY_PORT_INDEX_0;
  }

}

void printIndexLoop() {
  if (mainIndex == MY_PING_LOOP_INDEX_1) {
    print(("Ping"));
  }
  else {
    print(("RC"));
  }

}

void FindClosestPing() {
  if (pingDistance[MY_STARBOARD_INDEX_1] < pingDistance[MY_PORT_INDEX_0]) {
    closestPing = MY_STARBOARD_INDEX_1;
  }
  else {
    closestPing = MY_PORT_INDEX_0;
  }

}

void WarningText() {
  print(("Cog Warning!"));
  print("\r");
  LoopText();
}

void ReversePingSpeed() {
  if (frontRearPing[MY_REAR_INDEX_1] < MY_PING_DANGER_10) {
    for (mainIndex = MY_PORT_INDEX_0; mainIndex <= MY_STARBOARD_INDEX_1; mainIndex++) {
      finalSpeed[constrainInt(mainIndex, 0, 1)] = 0;

    }}
  else {
    FindFastestSide();
    scaledPingSpeed = (-1 * (((frontRearPing[MY_REAR_INDEX_1] - MY_PING_DANGER_10) * MY_MAX_PING_SPEED_60) / PING_CONCERN_RANGE_50));
    // Scale each speed by this new limit.
    // Use speed from "otherIndex" to find fastest reverse speed.
    // "fastestSide" will be slowest side in reverse.
    for (mainIndex
 = MY_PORT_INDEX_0; mainIndex <= MY_STARBOARD_INDEX_1; mainIndex++) {
      finalSpeed[constrainInt(mainIndex, 0, 1)] = (speedFromRc[mainIndex] * scaledPingSpeed) / speedFromRc[otherIndex[fastestSide]];

    }}

}

void LoopText() {
  for (mainIndex = MY_RC_LOOP_INDEX_0; mainIndex <= MY_PING_LOOP_INDEX_1; mainIndex++) {
    print(("cogLoops["));
    printIndexLoop();
    print("%s%d", "] = ", cogLoops[mainIndex]);
    print((", oldLoops["));
    printIndexLoop();
    print("%s%d\r", "] = ", oldLoops[mainIndex]);

  }}