// Arduino Licorice Launcher
// Vije Miller
// RF Tracking @ Warner K. robowarner.com

#include "EEPROM.h"
#include "Servo.h"

// Centering
// Too small adjusts too frequently
// Too large will pass over signal
#define buffering 26
// Output Pin
#define ANT_WAVEFORM_OUT 8
// Input Pin
#define SPEAKER_FROM_WALKIETALKIE A1
// Calibration Pin
#define CALIBRATE_IN 11

// Variables
uint16_t caliset = 0;
uint16_t voltage = 0;
int state = 1;

// Servo Assignment
Servo turn;
int turnpin = 6;
Servo loader;
int loaderpin = 9;
Servo fire;
int firepin = 10;
Servo arm;
int arm = 11;

// LED Pins
int red = 3;
int blue = 4;

void setup() {

  analogReference(DEFAULT);

  turn.attach(turnpin);
  loader.attach(loaderpin);
  fire.attach(firepin);
  arm.attach(armpin);

  pinMode(red, OUTPUT);
  pinMode(blue, OUTPUT);

  digitalWrite(red, LOW);
  digitalWrite(blue, LOW);

  Serial.begin(9600);

  delay(6000); // Allow Power Up

  pinMode(ANT_WAVEFORM_OUT, OUTPUT);
  pinMode(SPEAKER_FROM_WALKIETALKIE, INPUT);
  pinMode(CALIBRATE_IN, INPUT);
  digitalWrite(CALIBRATE_IN, HIGH);

  // Read EEPROM
  byte HByte =  EEPROM.read(1);
  byte LByte =  EEPROM.read(2);
  caliset = word(HByte, LByte);
  Serial.print("EEPROM Calibration number: ");
  Serial.println(caliset);

  // Calibration Check and Delay
  // Aim Receiver and Clear Area
  if (digitalRead(CALIBRATE_IN) == LOW) {
    delay(10000);
  }
}

void loop() {

  if (state == 1) {
    // Engage Licorice Weapon
    digitalWrite(red, HIGH);
    // Trigger Position Open
    fire.write(45);
    delay(1000);
    // Arm Launcher
    arm.write(180);
    delay(1500);
    // Lock Trigger
    fire.write(90);
    // Load Launcher
    loader.write(180);
    delay(330);
    // Stall Position
    loader.write(88);
    // Remove Arm
    arm.write(40);
    // Licorice Launcher is Hot
    digitalWrite(red, LOW);
    digitalWrite(blue, HIGH);
    state = 2;

  } else {
    // Hold for Command from JACOB
    if (Serial.available()) {
      char ser = Serial.read();
      switch (ser) {
        case 'L':
          load();
          break;
        case 'F':
          fire();
          break;
      }
    }
  }
}

void load() {
  // Load Launcher
  loader.write(180);
  delay(330);
  loader.write(88);
}

void fire() {

  // Antenna Switching Waveform
  digitalWrite(ANT_WAVEFORM_OUT, HIGH);
  delay(1);
  digitalWrite(ANT_WAVEFORM_OUT, LOW);
  delay(1);
  digitalWrite(ANT_WAVEFORM_OUT, HIGH);
  delay(1);
  digitalWrite(ANT_WAVEFORM_OUT, LOW);
  delay(1);
  digitalWrite(ANT_WAVEFORM_OUT, HIGH);
  delay(1);
  digitalWrite(ANT_WAVEFORM_OUT, LOW);
  delay(1);
  digitalWrite(ANT_WAVEFORM_OUT, HIGH);
  delay(1);
  digitalWrite(ANT_WAVEFORM_OUT, LOW);
  delay(1);

  // Read Speaker Voltage
  voltage = analogRead(SPEAKER_FROM_WALKIETALKIE);

  // If in Calibration Mode
  // Store to EEPROM
  if (digitalRead(CALIBRATE_IN) == LOW) {
    caliset = voltage;
    byte HByte = highByte(caliset);
    byte LByte = lowByte(caliset);
    EEPROM.write(1, HByte);
    EEPROM.write(2, LByte);
    delay(5000);
    endprogram();
  }

  // Stall Turning
  if (voltage > (caliset - buffering) && voltage < (caliset + buffering)) {
    // Fire
    launch();
  }
  // Turn Left
  if (voltage > (caliset + buffering)) {
    turn.write(180);
    delay(15);
    Serial.print("In, Cal\t");
    Serial.print(voltage);
    Serial.print("\t");
    Serial.println(caliset);
    // Return Cycle
    fire();
  }
  // Turn Right
  if (voltage < (caliset - buffering)) {
    turn.write(0);
    delay(15);
    Serial.print("In, Cal\t");
    Serial.print(voltage);
    Serial.print("\t");
    Serial.println(caliset);
    // Return Cycle
    fire();
  }

  delay(5);
  Serial.print("In, Cal\t");
  Serial.print(voltage);
  Serial.print("\t");
  Serial.println(caliset);
}

void launch() {
  // Release Trigger
  fire.write(45);
  delay(1000);
  // Reset
  state = 1;
  break;
}

void endprogram() {
  Serial.print("Calibrated to: ");
  Serial.print(caliset);
loopy:
  goto loopy;
}