/// @file CKS-Shield-1-Test.ino
/// @brief Arduino program for demonstrating and testing the CKS Shield 1 hardware.

/// @copyright No copyright, 2019, Garth Zeglin.  This file is explicitly placed in the public domain.

/****************************************************************/
// Library imports.
#include <Servo.h>

/****************************************************************/
/**** Hardware pin assignments **********************************/
/****************************************************************/

// The following pin assignments correspond to the hardware on the CKS Shield 1 Rev A board.
// More hardware details can be found at https://courses.ideate.cmu.edu/16-223/f2019/text/resources/CKS-1-Shield.html

// Analog inputs.
const int PHOTO1_PIN  = A0; /// photo-interrupter input, value decreases when object present
const int PHOTO2_PIN  = A1; /// photo-interrupter input, value decreases when object present
const int SWITCH1_PIN = A2; /// active-low switch input
const int SWITCH2_PIN = A3; /// active-low switch input
const int SWITCH3_PIN = A4; /// active-low switch input
const int SWITCH4_PIN = A5; /// active-low switch input

// If the optional ADXL335 accelerometer is used, it replaces switches 3,4,5.
// N.B. If the optional I2C interface is used, A4 and A5 are not available.
const int ACCELX_PIN  = A3;
const int ACCELY_PIN  = A4;
const int ACCELZ_PIN  = A5;

// Digital outputs. D0 and D1 are reserved for use as serial port RX/TX.

// Please note that many of these pins have alternate connections or may be used
// as GPIO; please refer to the documentation and specific configuration of your
// board.

const int RX0_PIN         = 0;  /// Serial receive (normally from USB, optionally from TTL Serial connector)
const int TX0_PIN         = 1;  /// Serial transmit(normally to USB, optionally to TTL Serial connector)
const int SERVO4_PIN      = 2;  /// servo PWM output
const int MOSFET1_PIN     = 3;  /// active-high MOSFET driver output
const int SERVO3_PIN      = 4;  /// servo PWM output
const int MOTA_PWM1_PIN   = 5;  /// DRV8833 dual H-bridge AIN1 (motor A, pin 1)
const int MOTA_PWM2_PIN   = 6;  /// DRV8833 dual H-bridge AIN2 (motor A, pin 2)
const int SERVO2_PIN      = 7;  /// servo PWM output
const int SERVO1_PIN      = 8;  /// servo PWM output
const int MOTB_PWM1_PIN   = 9;  /// DRV8833 dual H-bridge BIN1 (motor B, pin 1)
const int MOTB_PWM2_PIN   = 10; /// DRV8833 dual H-bridge BIN2 (motor B, pin 2)
const int MOSFET2_PIN     = 11; /// active-high MOSFET driver output
const int LED1_PIN        = 12; /// active-low LED on "LED1" connector, or GPIO
const int ONBOARD_LED_PIN = 13; /// active-high LED on Arduino

// If using a WS2801 digital LED strand on the SPI output, the following will displace other usage:
const int STRAND_DATA_PIN = 11; /// WS2801 LED strand data output (yellow wire on strand) (SPI MOSI)
const int STRAND_CLK_PIN  = 13; /// WS2801 LED strand clock output (green wire on strand) (SPI SCK)

/****************************************************************/
/**** Global variables and constants ****************************/
/****************************************************************/

// The baud rate is the number of bits per second transmitted over the serial port.
const long BAUD_RATE = 115200;

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

bool running = true;  ///< Global flag to enable the autonomous demo motions.

/****************************************************************/
/**** Standard entry points for Arduino system ******************/
/****************************************************************/

/// Standard Arduino initialization function to configure the system.  This
/// function is called once after reset to initialize the program.
void setup()
{
  // configure the actuator pins as soon as possible.
  pinMode(MOSFET1_PIN, OUTPUT);
  pinMode(MOSFET2_PIN, OUTPUT);

  pinMode(MOTA_PWM1_PIN, OUTPUT);
  pinMode(MOTA_PWM2_PIN, OUTPUT);

  pinMode(MOTB_PWM1_PIN, OUTPUT);
  pinMode(MOTB_PWM2_PIN, OUTPUT);
    
  pinMode(ONBOARD_LED_PIN, OUTPUT);
  pinMode(LED1_PIN,        OUTPUT);  

  digitalWrite(MOSFET1_PIN, LOW);
  digitalWrite(MOSFET2_PIN, LOW);

  digitalWrite(MOTA_PWM1_PIN, LOW);
  digitalWrite(MOTA_PWM2_PIN, LOW);

  digitalWrite(MOTB_PWM1_PIN, LOW);
  digitalWrite(MOTB_PWM2_PIN, LOW);
    
  digitalWrite(ONBOARD_LED_PIN, LOW);
  digitalWrite(LED1_PIN,        HIGH);

  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);
  servo3.attach(SERVO3_PIN);
  servo4.attach(SERVO4_PIN);

  // initialize the Serial port for the user debugging console
  Serial.begin(BAUD_RATE);

  // send a message as a diagnostic
  Serial.println(__FILE__ " awake.");
}

/****************************************************************/
/// Standard Arduino polling function to handle all I/O and periodic processing.
/// This function is called repeatedly as fast as possible from within the
/// built-in library to poll program events.  This loop should never be allowed
/// to stall or block so that all tasks can be constantly serviced.
void loop()
{
  // The timestamp in microseconds for the last polling cycle, used to compute
  // the exact interval between output updates.
  static unsigned long last_update_clock = 0;

  // Read the microsecond clock.
  unsigned long now = micros();

  // Compute the time elapsed since the last poll.  This will correctly handle wrapround of
  // the 32-bit long time value given the properties of twos-complement arithmetic.
  unsigned long interval = now - last_update_clock;
  last_update_clock = now;

  // Begin the polling cycle.
  poll_console_input(interval);
  poll_servos(interval);
  poll_beeps(interval);
  poll_motors(interval);
}

/****************************************************************/
/****************************************************************/
// Configure the control lines for one half of a DRV8833 driver.
// @param pwm  Motor PWM value ranging from -255 to 255, with zero representing stopped.
// @param IN1_PIN  Integer pin value for either AIN1 or BIN1.
// @param IN2_PIN  Integer pin value for either AIN2 or BIN2.
static void set_motor_pwm(int pwm, int IN1_PIN, int IN2_PIN)
{
  if (pwm < 0) {  // reverse speeds
    analogWrite(IN1_PIN, -pwm);
    digitalWrite(IN2_PIN, LOW);

  } else { // stop or forward
    digitalWrite(IN1_PIN, LOW);
    analogWrite(IN2_PIN, pwm);
  }
}
/****************************************************************/
// Wrapper on strcmp for clarity of code.  Returns true if strings are
// identical.
static int string_equal(char *str1, const char str2[])
{
  return !strcmp(str1, str2);
}

/****************************************************************/
/// Process an input message tokenized from a line of console input.
/// New commands may be defined by adding additional clauses into the if-else structures.
///   @param argc   number of argument tokens
///   @param argv   array of pointers to strings, one per token
static void parse_user_input(int argc, char *argv[])
{
  // Interpret the first token as a command symbol.
  char *command = argv[0];

  /* -- process zero-argument commands --------------------------- */
  if (argc == 1) {
    if (string_equal(command, "run")) {
      running = true;
    }

    else if (string_equal(command, "stop")) {
      running = false;      
    }

    else if (string_equal(command, "inputs")) {
      Serial.println("Raw sensor values:");
      Serial.print("A0: "); Serial.println(analogRead(A0));
      Serial.print("A1: "); Serial.println(analogRead(A1));
      Serial.print("A2: "); Serial.println(analogRead(A2));
      Serial.print("A3: "); Serial.println(analogRead(A3));
      Serial.print("A4: "); Serial.println(analogRead(A4));
      Serial.print("A5: "); Serial.println(analogRead(A5));
    }

    else {
      Serial.println("unrecognized command.");
    }
  }

  /* -- process one-argument commands --------------------------- */
  else if (argc == 2) {
    long value = atol(argv[1]);

    // Set the onboard LED on or off.
    if (string_equal(command, "led")) {
      digitalWrite(ONBOARD_LED_PIN, value);
    }
    
    else if (string_equal(command, "ma")) {
      set_motor_pwm(value, MOTA_PWM1_PIN, MOTA_PWM2_PIN);
    }

    else if (string_equal(command, "mb")) {
      set_motor_pwm(value, MOTB_PWM1_PIN, MOTB_PWM2_PIN);
    }

    else if (string_equal(command, "beep")) {
      switch(value) {
      case 1: tone(MOSFET1_PIN, 440, 50); break;
      case 2: tone(MOSFET2_PIN, 660, 50); break;	
      }
    }

    else {
      Serial.println("unrecognized single-argument command.");
    }
  }
  /* -- process two-argument commands --------------------------- */
  else if (argc == 3) {
    long value1 = atol(argv[1]);
    long value2 = atol(argv[2]);

    if (string_equal(command, "sv")) {
      switch(value1) {
      case 1: servo1.write(value2); break;
      case 2: servo2.write(value2); break;
      case 3: servo3.write(value2); break;
      case 4: servo4.write(value2); break;
      }
    }

    else {
      Serial.println("unrecognized two-argument command.");
    }
  }

  else {
    Serial.println("unrecognized command format.");
  }
}

/****************************************************************/
/// Polling function to process messages arriving over the serial port.  Each
/// iteration through this polling function processes at most one character.  It
/// records the input message line into a buffer while simultaneously dividing it
/// into 'tokens' delimited by whitespace.  Each token is a string of
/// non-whitespace characters, and might represent either a symbol or an integer.
/// Once a message is complete, parse_user_input() is called.
///
/// @param elapsed number of microseconds elapsed since last update
void poll_console_input(unsigned long elapsed)
{
  const int MAX_LINE_LENGTH = 80;  // The maximum message line length.
  const int MAX_TOKENS = 10;       // The maximum number of tokens in a single message.

  static char input_buffer[MAX_LINE_LENGTH];   // buffer for input characters
  static char *argv[MAX_TOKENS];                 // buffer for pointers to tokens
  static int chars_in_buffer = 0;  // counter for characters in buffer
  static int chars_in_token = 0;   // counter for characters in current partially-received token (the 'open' token)
  static int argc = 0;             // counter for tokens in argv
  static int error = 0;            // flag for any error condition in the current message

  (void) elapsed;  // no-op to suppress compiler warning

  // Check if at least one byte is available on the serial input.
  if (Serial.available()) {
    int input = Serial.read();

    // If the input is a whitespace character, end any currently open token.
    if (isspace(input)) {
      if (!error && chars_in_token > 0) {
	if (chars_in_buffer == MAX_LINE_LENGTH) error = 1;
	else {
	  input_buffer[chars_in_buffer++] = 0;  // end the current token
	  argc++;                               // increase the argument count
	  chars_in_token = 0;                   // reset the token state
	}
      }

      // If the whitespace input is an end-of-line character, then pass the message buffer along for interpretation.
      if (input == '\r' || input == '\n') {

	// if the message included too many tokens or too many characters, report an error
	if (error) Serial.println("excessive input error");

	// else process any complete message
	else if (argc > 0) parse_user_input(argc, argv);

	// reset the full input state
	error = chars_in_token = chars_in_buffer = argc = 0;
      }
    }

    // Else the input is a character to store in the buffer at the end of the current token.
    else {
      // if beginning a new token
      if (chars_in_token == 0) {

	// if the token array is full, set an error state
	if (argc == MAX_TOKENS) error = 1;

	// otherwise save a pointer to the start of the token
	else argv[ argc ] = &input_buffer[chars_in_buffer];
      }

      // the save the input and update the counters
      if (!error) {
	if (chars_in_buffer == MAX_LINE_LENGTH) error = 1;
	else {
	  input_buffer[chars_in_buffer++] = input;
	  chars_in_token++;
	}
      }
    }
  }
}
/****************************************************************/
// Demo state machine to create a pattern of motion on the servos.

/// Countdown for the polling timer, in microseconds.
long servo_timer = 0;

/// Interval between updates, in microseconds.  Defaults to 20 Hz.
long servo_interval = 50000;

/// Servo demo motion frame counter.
long servo_frame = 0;

/// Polling function to update the servo motion generator state machine.  This should be called as frequently as possible.
/// @param interval number of microseconds elapsed since last update
void poll_servos(unsigned long interval)
{
  servo_timer -= interval;
  if (servo_timer < 0) {
    servo_timer += servo_interval;
    if (running) {
      servo_frame = servo_frame + 1;
      
      // create a series of wave-like movements across all four servos
      int servo     = (servo_frame / 20) % 4;
      int direction = (servo_frame / 80) % 2;
      int angle     = (servo_frame % 20) * 9;

      if (direction) angle = 180 - angle;
      switch(servo) {
      case 0: servo1.write(angle); break;
      case 1: servo2.write(angle); break;
      case 2: servo3.write(angle); break;
      case 3: servo4.write(angle); break;
      }
    }
  }
}

/****************************************************************/
// Demo state machine to create a pattern of beeps.

/// Countdown for the polling timer, in microseconds.
long beep_timer = 0;

/// Interval between updates, in microseconds.  Defaults to once every pi seconds.
long beep_interval = 3141592;

/// Beep melody position.
int beep_note = 0;

/// Beep melody table.  Pitches are in Hz.
const int beep_freq[] = {262, 294, 330, 349, 392, 440, 494, 523, 0};

/// Polling function to update the tone generator state machine.  This should be called as frequently as possible.
/// @param interval number of microseconds elapsed since last update
void poll_beeps(unsigned long interval)
{
  beep_timer -= interval;
  if (beep_timer < 0) {
    beep_timer += beep_interval;
    
    if (running) {
      int freq = beep_freq[beep_note];
      tone(MOSFET1_PIN, freq, 25);

      beep_note = beep_note + 1;
      if (beep_freq[beep_note] == 0) beep_note = 0;
    }
  }
}

/****************************************************************/
// Demo state machine to create a pattern of motion on the motors.

/// Countdown for the polling timer, in microseconds.
long motor_timer = 0;

/// Interval between updates, in microseconds.  Defaults to 20 Hz.
long motor_interval = 50000;

/// Motor demo motion frame counter.
long motor_frame = 0;

/// Polling function to update the DC motor motion generator state machine.  This should be called as frequently as possible.
/// @param interval number of microseconds elapsed since last update
void poll_motors(unsigned long interval)
{
  motor_timer -= interval;
  if (motor_timer < 0) {
    motor_timer += motor_interval;
    if (running) {
      motor_frame = motor_frame + 1;
      
      // create a series of wave-like movements across both motors
      int phase_a   = (motor_frame % 50);
      int phase_b   = (motor_frame % 70);
      int pwm_a     = map(phase_a, 0, 50, -255, 255);
      int pwm_b     = map(phase_b, 0, 70, -255, 255);
      set_motor_pwm(constrain(pwm_a, -255, 255), MOTA_PWM1_PIN, MOTA_PWM2_PIN);
      set_motor_pwm(constrain(pwm_b, -255, 255), MOTB_PWM1_PIN, MOTB_PWM2_PIN);
    }
  }
}

/****************************************************************/