import processing.serial.*;

Serial port = null;
String portName;
int lastInput = 0;
int[] command = new int[2];
int[] lastCommand = new int[2];
int speed = 9;
int lastSpeed = 0;
int speedStep = (255 - 130) / 8;
boolean running = true;
boolean leftFrontBumper = false;
boolean rightFrontBumper = false;
boolean rearBumper = false;

int DIR_FORWARD = 0x1;
int DIR_BACKWARD = 0x2;
int DIR_LEFT = 0x4;
int DIR_RIGHT = 0x8;
int BEGIN_COMMAND = 0x7F;

// Sensor Data
int LEFT_FRONT_BUMPER = 0x1;
int RIGHT_FRONT_BUMPER = 0x2;
int REAR_BUMPER = 0x4;

SimpleDateFormat df = new SimpleDateFormat("hh:mm:ss.SSS");

void setup() {
  size(1, 1);

  portName = Serial.list()[0];
  try {
    port = new Serial(this, portName, 9600);
  } catch (Exception e) {
    e.printStackTrace();
  }
}

String timestamp() {
  return df.format(new Date());
}

void draw() {

  if (rightFrontBumper) {
    println(timestamp() + " ----RIGHT FRONT IMPACT!----");
    rightFrontBumper();
  }
  if (leftFrontBumper) {
    println(timestamp() + " ----LEFT FRONT IMPACT!-----");
    leftFrontBumper();
  }
  if (rearBumper) {
    println(timestamp() + " -------REAR IMPACT!--------");
    rearBumper();
  }

  speed = 9;
  left();
  forward(1200);
  right();
  forward(800);
  stop();

  left();
  backward(1200);
  stop();
  right();

  forward(2500);
  stop();

  left();
  backward(4000);

  end();
}

void leftFrontBumper() {
  leftFrontBumper = false;
  running = true;

  left();
  speed = 9;
  backward(800);
  straight();
  end();
}

void rightFrontBumper() {
  rightFrontBumper = false;
  running = true;

  right();
  speed = 9;
  backward(800);
  straight();
  end();
}

void rearBumper() {
  rearBumper = false;
  running = true;

  speed = 9;
  forward(1000);
  straight();
  end();
}






void end() {
  if (!running) return;
  stop();
  straight();
  println(timestamp() + " -----------END------------");
  println("Press 's' to restart program or use arrow keys to control vehicle.");
  running = false;
}

void interruptibleDelay(int millis) {
  int start = millis();
  int d;
  while (running) {
    int timeLeftToWait = millis-(millis()-start);
    d = min(10, timeLeftToWait);
    if (d <= 0) return;
    delay(d);
  }
}


void forward(int millis) {
  if (!running) return;
  forward();
  interruptibleDelay(millis);
  stop();
}

void forward() {
  if (!running) return;
  doForward();
}

void doForward() {
  command[0] = command[0] & ~DIR_BACKWARD;
  command[0] = command[0] | DIR_FORWARD;
  sendCommand();
}

void backward(int millis) {
  if (!running) return;
  doBackward();
  interruptibleDelay(millis);
  stop();
}

void backward() {
  if (!running) return;
  doBackward();
}

void doBackward() {
  command[0] = command[0] & ~DIR_FORWARD;
  command[0] = command[0] | DIR_BACKWARD;
  sendCommand();
}

void left() {
  if (!running) return;
  doLeft();
}

void doLeft() {
  command[0] = command[0] & ~DIR_RIGHT;
  command[0] = command[0] | DIR_LEFT;
  sendCommand();
}

void right() {
  if (!running) return;
  doRight();
}

void doRight() {
  command[0] = command[0] & ~DIR_LEFT;
  command[0] = command[0] | DIR_RIGHT;
  sendCommand();
}

void straight() {
  if (!running) return;
  doStraight();
}

void doStraight() {
  command[0] = command[0] & ~DIR_RIGHT;
  command[0] = command[0] & ~DIR_LEFT;
  sendCommand();
}

void stop() {
  if (!running) return;
  doStop();
}

void doStop() {
  command[0] = command[0] & ~DIR_BACKWARD;
  command[0] = command[0] & ~DIR_FORWARD;
  sendCommand();
}



void serialEvent(Serial p) {
  int input = p.read();
  lastInput = input;
  processInput(input);
} 


void processInput(int input) {
  if ((input & LEFT_FRONT_BUMPER) != 0) {
    stop();
    leftFrontBumper = true;
    running = false;
  }

  if ((input & RIGHT_FRONT_BUMPER) != 0) {
    stop();
    rightFrontBumper = true;
    running = false;
  }

  if ((input & REAR_BUMPER) != 0) {
    stop();
    rearBumper = true;
    running = false;
  }
}


void sendCommand() {
  if (!isNewCommand()) {
    return;
  }
  if (port != null) {
    port.write(BEGIN_COMMAND);
    port.write(command[0]);
    command[1] = 255 - ((9 - speed) * speedStep);
    port.write(command[1]);
    if ((command[0] & DIR_FORWARD) > 0) {
      print(timestamp() + " FORWARD\t");
    }
    if ((command[0] & DIR_BACKWARD) > 0) {
      print(timestamp() + " BACKWARD\t");
    }
    if (((command[0] & DIR_FORWARD) == 0) && ((command[0] & DIR_BACKWARD) == 0)) {
      print(timestamp() + " STOP\t");
    }

    if ((command[0] & DIR_LEFT) > 0) {
      print("LEFT\t");
    }
    if ((command[0] & DIR_RIGHT) > 0) {
      print("RIGHT\t");
    }
    if (((command[0] & DIR_LEFT) == 0) && ((command[0] & DIR_RIGHT) == 0)) {
      print("STRAIGHT\t");
    }
    println("SPEED=" + speed);
    

    lastCommand[0] = command[0];
    lastCommand[1] = command[1];
    lastSpeed = speed;
  }
}

boolean isNewCommand() {
  return ((command[0] != lastCommand[0]) || (command[1] != lastCommand[1]) || (speed != lastSpeed));
}

void keyPressed() {
  if (key == CODED) {
    if (keyCode == UP) {
      doForward();
    }
    if (keyCode == DOWN) {
      doBackward();
    }
    if (keyCode == LEFT) {
      doLeft();
    }
    if (keyCode == RIGHT) {
      doRight();
    }
  }
  if ((key >= '1') && (key <= '9')) {
    speed = 9 - ('9' - key);
    println("set speed = " + speed);
  }
  if (key == 's') {
    running = true;
  }
}

void keyReleased() {
  if (key == CODED) {
    if (keyCode == UP) {
      doStop();
    }
    if (keyCode == DOWN) {
      doStop();
    }
    if (keyCode == LEFT) {
      doStraight();
    }
    if (keyCode == RIGHT) {
      doStraight();
    }
  }
}