/* Have 1/d forces which affect motion
   -3/d in the center, 1/d at each of 5 points, sampled randomly and replaced */

/* starts in middle of both */

#define CENTER_SCALE -3.
#define SIDE_SCALE 1.

#define LOWER_RANGE 10.
#define UPPER_RANGE 10.
#define LOWER_SCALE .5
#define UPPER_SCALE 1.

#define LOWER_SLOT 3
#define UPPER_SLOT 1

#define LOWER_POT 0
#define UPPER_POT 2

#define RECCOUNT 5
#define COUNTLOOP 100

int recents[RECCOUNT][2];
int mlower = 0;
int mupper = 0;
int lower_eq;
int upper_eq;

void main() {
  int i;
  float lower_acc, upper_acc;
  int count = 0;
  int reccurr = 0;

  while (!start_button());

  off(LOWER_SLOT);
  off(UPPER_SLOT);

  lower_eq = analog(LOWER_POT);
  upper_eq = analog(UPPER_POT);

  for (i = 0; i < RECCOUNT; i++) {
    recents[i][0] = (int) (LOWER_RANGE * sin(2. * 3.14159 * ((float) i) / ((float) RECCOUNT)));
    recents[i][1] = (int) (UPPER_RANGE * cos(2. * 3.14159 * ((float) i) / ((float) RECCOUNT)));
  }

  while (!stop_button()) {
    printf("%d: M: %d, %d; P: %d, %d\n", count, mlower, mupper, analog(LOWER_POT), analog(UPPER_POT));

    lower_acc = CENTER_SCALE * LOWER_SCALE * ((float) (analog(LOWER_POT) - lower_eq)) / (safe(analog(LOWER_POT), lower_eq) / LOWER_RANGE);
    upper_acc = CENTER_SCALE * UPPER_SCALE * ((float) (analog(UPPER_POT) - upper_eq)) / (safe(analog(UPPER_POT), upper_eq) / UPPER_RANGE);
    for (i = 0; i < RECCOUNT; i++) {
      lower_acc += SIDE_SCALE * LOWER_SCALE * ((float) (analog(LOWER_POT) - recents[i][0])) / (safe(analog(LOWER_POT), recents[i][0]) / LOWER_RANGE);
      upper_acc += SIDE_SCALE * UPPER_SCALE * ((float) (analog(UPPER_POT) - recents[i][1])) / (safe(analog(UPPER_POT), recents[i][0]) / UPPER_RANGE);
    }

    mlower += (int) lower_acc;
    mupper += (int) upper_acc;
    if (mlower > 100)
      mlower = 100;
    if (mlower < -100)
      mlower = -100;
    if (mupper > 100)
      mupper = 100;
    if (mupper < -100)
      mupper = -100;
    if (mlower > 80 || mlower < -80)
      motor(LOWER_SLOT, mlower);
    else
      off(LOWER_SLOT);
    if (mupper > 80 || mupper < -80)
      motor(UPPER_SLOT, mupper);
    else
      off(UPPER_SLOT);

    count = (count + 1) % COUNTLOOP;
    recents[reccurr][0] = analog(LOWER_POT);
    recents[reccurr++][1] = analog(UPPER_POT);
    reccurr = reccurr % RECCOUNT;
  }
}

float safe(int a, int b) {
  if (a == b)
    return .5;
  else
    return (float) ((a - b)*(a - b));
}