/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*------------------------- [c]2020 - D. Ottensmeyer ------------------------*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*~
 ~ Target:    Arduino NANO with RoboDuck (created by the "Fakultät für Physik
 ~            der Universität Regensburg", Germany, in 2019).
 ~            Link: http://www.physik.uni-regensburg.de/rsl/roboduck.html
 ~ Example:   RoboDuck_Demo_01
 ~ Version:   0.90
 ~ Author(s): D. Ottensmeyer
 ~
 ~ Description:
 ~ In this example we show a first test for the RoboDuck.
 */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Defines:
//#define SERVO_CENTER
//#define SERVO_DEFAULT
#define DEMO_RUN
// Demos (choose 1..4 of them!):
#define WALK
#define LEDS
#define BEEPER
#define ULTRASONIC

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Includes:
#include <RoboDuck.h>

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Constants:

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Variables:

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void setup() {
/*
  // Initialize serial:
  Serial.begin(9600); 
  Serial.println("Arduino NANO RoboDuck Test 1!");
  Serial.println("");
  Serial.println("PLEASE ENSURE THAT ROBODUCK CAN MOVE FREELY!!!");
  Serial.println("");
*/

// Assign Arduino Nano ports to the LEDs:
  pinMode(LED_BUILTIN, OUTPUT);     // built-in LED pin
  pinMode(LED_R_GREEN_PIN, OUTPUT); // pin 14 (A0, PC0)
  pinMode(LED_L_RED_PIN, OUTPUT);   // pin 15 (A1, PC1)

// Assign an Arduino Nano port to the Beeper:
  pinMode(BEEPER_PIN, OUTPUT);      // pin 5 (PD5)

#ifdef SERVO_CENTER
  Servo_Init();
  AllServo_Center();
  while(1);
#endif

#ifdef SERVO_DEFAULT
  Servo_Init();  
  AllServo_Default();
  while(1);
#endif

#ifdef DEMO_RUN 
  Servo_Init();
  AllServo_Default();
  delay(2000);
#endif
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void loop() {

#ifdef WALK
// Some simple walking moves:
  Walkahead(5, 30);
  delay(500);
  Walkback(5, 30);
  delay(500);
  Turnleft(3);
  delay(500);
  Walkahead(5, 30);
  delay(500);
  Turnleft(3);
  delay(500);
  Walkahead(5, 30);
  delay(500);
  Turnright(3);
  delay(500);
  Walkahead(5, 30);
  delay(500);
  Turnleft(3);
  delay(500);
#endif

#ifdef LEDS
// Test "Quack-quack" and LEDs:
  Quack();
  int z;
  z = 0;
  for (int a = 0; a < 25; a++) {
    LEDs(z);
    z++;
    if(z > 7) {
      z = 0;
      Quack(); // "Quack-quack" sound
    }
    delay(500);
  }
  #endif

  #ifdef BEEPER
  // Dance & sing "Alle meine Entchen" (All My Ducklings):
  ServoWrite(RIGHT_FOOT, defPos(RIGHT_FOOT) - 30, 15, false);
  ServoWrite(LEFT_FOOT, defPos(LEFT_FOOT) + 30, 15, false);
  ServoWrite(HEAD, defPos(HEAD) + 30, 20, false);
  Beep(NOTE_D4, QUARTER_NOTE);
  Beep(NOTE_E4, QUARTER_NOTE);
  Beep(NOTE_FS4, QUARTER_NOTE);
  Beep(NOTE_G4, QUARTER_NOTE);

  ServoWrite(RIGHT_FOOT, defPos(RIGHT_FOOT) - 40, 15, false);
  ServoWrite(LEFT_FOOT, defPos(LEFT_FOOT) + 40, 15, false);
  ServoWrite(HEAD, defPos(HEAD) - 30, 20, false);
  Beep(NOTE_A4, HALF_NOTE);
  Beep(NOTE_A4, HALF_NOTE);

  ServoWrite(RIGHT_FOOT, defPos(RIGHT_FOOT) - 50, 15, false);
  ServoWrite(LEFT_FOOT, defPos(LEFT_FOOT) + 50, 15, false);
  ServoWrite(HEAD, defPos(HEAD) + 30, 20, false);
  Beep(NOTE_B4, QUARTER_NOTE);
  Beep(NOTE_B4, QUARTER_NOTE);
  Beep(NOTE_B4, QUARTER_NOTE);
  Beep(NOTE_B4, QUARTER_NOTE);

  ServoWrite(RIGHT_FOOT, defPos(RIGHT_FOOT) - 40, 15, false);
  ServoWrite(LEFT_FOOT, defPos(LEFT_FOOT) + 40, 15, false);
  ServoWrite(HEAD, defPos(HEAD) - 30, 20, false);
  Beep(NOTE_A4, WHOLE_NOTE);

  ServoWrite(RIGHT_FOOT, defPos(RIGHT_FOOT) - 50, 15, false);
  ServoWrite(LEFT_FOOT, defPos(LEFT_FOOT) + 50, 15, false);
  ServoWrite(HEAD, defPos(HEAD) + 30, 20, false);
  Beep(NOTE_B4, QUARTER_NOTE);
  Beep(NOTE_B4, QUARTER_NOTE);
  Beep(NOTE_B4, QUARTER_NOTE);
  Beep(NOTE_B4, QUARTER_NOTE);

  ServoWrite(RIGHT_FOOT, defPos(RIGHT_FOOT) - 40, 15, false);
  ServoWrite(LEFT_FOOT, defPos(LEFT_FOOT) + 40, 15, false);
  ServoWrite(HEAD, defPos(HEAD) - 30, 20, false);
  Beep(NOTE_A4, WHOLE_NOTE);

  ServoWrite(RIGHT_FOOT, defPos(RIGHT_FOOT) - 30, 15, false);
  ServoWrite(LEFT_FOOT, defPos(LEFT_FOOT) + 30, 15, false);
  ServoWrite(HEAD, defPos(HEAD) + 30, 20, false);
  Beep(NOTE_G4, QUARTER_NOTE);
  Beep(NOTE_G4, QUARTER_NOTE);
  Beep(NOTE_G4, QUARTER_NOTE);
  Beep(NOTE_G4, QUARTER_NOTE);

  ServoWrite(RIGHT_FOOT, defPos(RIGHT_FOOT) - 20, 15, false);
  ServoWrite(LEFT_FOOT, defPos(LEFT_FOOT) + 20, 15, false);
  ServoWrite(HEAD, defPos(HEAD) - 30, 20, false);
  Beep(NOTE_FS4, HALF_NOTE);
  Beep(NOTE_FS4, HALF_NOTE);

  ServoWrite(RIGHT_FOOT, defPos(RIGHT_FOOT) - 30, 15, false);
  ServoWrite(LEFT_FOOT, defPos(LEFT_FOOT) + 30, 15, false);
  ServoWrite(HEAD, defPos(HEAD) + 30, 20, false);
  Beep(NOTE_A4, QUARTER_NOTE);
  Beep(NOTE_A4, QUARTER_NOTE);
  Beep(NOTE_A4, QUARTER_NOTE);
  Beep(NOTE_A4, QUARTER_NOTE);

  FeetServo_Default();
  ServoWrite(HEAD, defPos(HEAD), 20, false);
  Beep(NOTE_D4, HALF_NOTE);
  delay(HALF_NOTE);

  delay(1000);
  Quack();
#endif

#ifdef ULTRASONIC
// Test Ultrasonic Sensor:
  LEDs(ALL_OFF);
  while(1) {
    if (TooClose()) LEDs(RED); // Too close: red LED
    else LEDs(GREEN);          // Else:      green LED
    delay(50); 
  }
#endif

while(1);
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
