# Each servo corresponds to one of the legs
front_right = crickit.continuous_servo_1
front_left = crickit.continuous_servo_2
rear_right = crickit.continuous_servo_3
rear_left = crickit.continuous_servo_4

# Useful groups of legs
all_legs = [front_right, front_left, rear_right, rear_left]
front_legs = [front_right, front_left]
rear_legs = [rear_right, rear_left]
right_legs = [front_right, rear_right]
left_legs = [front_left, rear_left]

# Each servo corresponds to one of the legs
front_right = crickit.continuous_servo_1
front_left = crickit.continuous_servo_2
rear_right = crickit.continuous_servo_3
rear_left = crickit.continuous_servo_4

# Useful groups of legs
all_legs = [front_right, front_left, rear_right, rear_left]
front_legs = [front_right, front_left]
rear_legs = [rear_right, rear_left]
right_legs = [front_right, rear_right]
left_legs = [front_left, rear_left]

# The sign (+1/-1) for forward motion for each servo
direction_values = {front_right: +1, 
                    front_left: -1, 
                    rear_right: +1, 
                    rear_left: -1}

# The sign (+1/-1) for forward motion for each servo
direction_values = {front_right: +1, 
                    front_left: -1, 
                    rear_right: +1, 
                    rear_left: -1}

pwm_ranges = {front_right: (500, 2400), 
              front_left: (500, 2400), 
              rear_right: (500, 2400), 
              rear_left: (500, 2400)}

pwm_ranges = {front_right: (500, 2400), 
              front_left: (500, 2400), 
              rear_right: (500, 2400), 
              rear_left: (500, 2400)}

def init():
    for leg in all_legs:
        limits = pwm_ranges[leg]
        leg.set_pulse_width_range(min_pulse=limits[0], max_pulse=limits[1])
        leg.throttle = 0

def init():
    for leg in all_legs:
        limits = pwm_ranges[leg]
        leg.set_pulse_width_range(min_pulse=limits[0], max_pulse=limits[1])
        leg.throttle = 0

def forward(servo_or_servos, speed):
    if type(servo_or_servos) == list:
        for servo in servo_or_servos:
            servo.throttle = speed * direction_values[servo]
    else:
        servo_or_servos.throttle = speed * direction_values[servo_or_servos]


def reverse(servo_or_servos, speed):
    if type(servo_or_servos) == list:
        for servo in servo_or_servos:
            servo.throttle = speed * -1 * direction_values[servo]
    else:
        servo_or_servos.throttle = speed * -1 * direction_values[servo_or_servos]


def stop(servo_or_servos):
    if type(servo_or_servos) == list:
        for servo in servo_or_servos:
            servo.throttle = 0
    else:
        servo_or_servos.throttle = 0

def forward(servo_or_servos, speed):
    if type(servo_or_servos) == list:
        for servo in servo_or_servos:
            servo.throttle = speed * direction_values[servo]
    else:
        servo_or_servos.throttle = speed * direction_values[servo_or_servos]


def reverse(servo_or_servos, speed):
    if type(servo_or_servos) == list:
        for servo in servo_or_servos:
            servo.throttle = speed * -1 * direction_values[servo]
    else:
        servo_or_servos.throttle = speed * -1 * direction_values[servo_or_servos]


def stop(servo_or_servos):
    if type(servo_or_servos) == list:
        for servo in servo_or_servos:
            servo.throttle = 0
    else:
        servo_or_servos.throttle = 0

def rotate_clockwise(speed):
    forward(left_legs, speed)
    reverse(right_legs, speed)


def rotate_counterclockwise(speed):
    forward(right_legs, speed)
    reverse(left_legs, speed)


def crawl_forward(speed):
    forward(all_legs, speed)


def crawl_backward(speed):
    reverse(all_legs, speed)

def rotate_clockwise(speed):
    forward(left_legs, speed)
    reverse(right_legs, speed)


def rotate_counterclockwise(speed):
    forward(right_legs, speed)
    reverse(left_legs, speed)


def crawl_forward(speed):
    forward(all_legs, speed)


def crawl_backward(speed):
    reverse(all_legs, speed)

def wag(speed):
    tail.throttle = speed
    time.sleep(0.1)
    tail.throttle = 0.0
    time.sleep(0.25)

    
def wag_for(seconds):
    target_time = time.monotonic() + seconds
    wag_throttle = 1.0
    while time.monotonic() < target_time:
        wag(wag_throttle)
        wag_throttle *= -1.0

def wag(speed):
    tail.throttle = speed
    time.sleep(0.1)
    tail.throttle = 0.0
    time.sleep(0.25)

    
def wag_for(seconds):
    target_time = time.monotonic() + seconds
    wag_throttle = 1.0
    while time.monotonic() < target_time:
        wag(wag_throttle)
        wag_throttle *= -1.0

def demo1():
    crawl_forward(0.5)
    wag_for(5.0)
    rotate_clockwise(0.25)
    wag_for(3.0)
    crawl_backward(0.5)
    wag_for(2.0)
    rotate_counterclockwise(0.25)
    wag_for(3.0)
    crawl_forward(0.5)
    wag_for(5.0)
    stop(all_legs)

def demo1():
    crawl_forward(0.5)
    wag_for(5.0)
    rotate_clockwise(0.25)
    wag_for(3.0)
    crawl_backward(0.5)
    wag_for(2.0)
    rotate_counterclockwise(0.25)
    wag_for(3.0)
    crawl_forward(0.5)
    wag_for(5.0)
    stop(all_legs)

"""
Continuous servo based walking/waddling/etc robot.

Adafruit invests time and resources providing this open source code.
Please support Adafruit and open source hardware by purchasing
products from Adafruit!

Written by Dave Astels for Adafruit Industries
Copyright (c) 2018 Adafruit Industries
Licensed under the MIT license.

All text above must be included in any redistribution.
"""

import time
from adafruit_crickit import crickit

tail = crickit.dc_motor_1

# Each servo corresponds to one of the legs
front_right = crickit.continuous_servo_1
front_left = crickit.continuous_servo_2
rear_right = crickit.continuous_servo_3
rear_left = crickit.continuous_servo_4

# Useful groups of legs
all_legs = [front_right, front_left, rear_right, rear_left]
front_legs = [front_right, front_left]
rear_legs = [rear_right, rear_left]
right_legs = [front_right, rear_right]
left_legs = [front_left, rear_left]

# The sign (+1/-1) for forward motion for each servo
direction_values = {front_right: +1,
                    front_left: -1,
                    rear_right: +1,
                    rear_left: -1}

# Tweak the pwn ranges for each servo so that throttle of 0 stops the motor
pwm_ranges = {front_right: (500, 2400),
              front_left: (500, 2400),
              rear_right: (500, 2400),
              rear_left: (500, 2400)}


def init():
    for leg in all_legs:
        limits = pwm_ranges[leg]
        leg.set_pulse_width_range(min_pulse=limits[0], max_pulse=limits[1])
        leg.throttle = 0


def wag(speed):
    tail.throttle = speed
    time.sleep(0.1)
    tail.throttle = 0.0
    time.sleep(0.25)


def wag_for(seconds):
    target_time = time.monotonic() + seconds
    wag_throttle = 1.0
    while time.monotonic() < target_time:
        wag(wag_throttle)
        wag_throttle *= -1


def forward(servo_or_servos, speed):
    if isinstance(servo_or_servos, list):
        for servo in servo_or_servos:
            servo.throttle = speed * direction_values[servo]
    else:
        servo_or_servos.throttle = speed * direction_values[servo_or_servos]


def reverse(servo_or_servos, speed):
    if isinstance(servo_or_servos, list):
        for servo in servo_or_servos:
            servo.throttle = speed * -1 * direction_values[servo]
    else:
        servo_or_servos.throttle = speed * -1 * direction_values[servo_or_servos]


def stop(servo_or_servos):
    if isinstance(servo_or_servos, list):
        for servo in servo_or_servos:
            servo.throttle = 0
    else:
        servo_or_servos.throttle = 0


def rotate_clockwise(speed):
    forward(left_legs, speed)
    reverse(right_legs, speed)


def rotate_counterclockwise(speed):
    forward(right_legs, speed)
    reverse(left_legs, speed)


def crawl_forward(speed):
    forward(all_legs, speed)


def crawl_backward(speed):
    reverse(all_legs, speed)


def turtle():
    stop([rear_right, rear_left])
    stop(rear_left)
    forward(front_right, 0.5)
    forward(front_left, 0.5)


def snake_step():
    stop(all_legs)
    forward(right_legs, 0.5)
    time.sleep(1.0)
    stop(right_legs)
    forward(left_legs, 0.5)
    time.sleep(1.0)
    stop(left_legs)


init()

def demo1():
    crawl_forward(0.5)
    wag_for(5.0)
    rotate_clockwise(0.25)
    wag_for(3.0)
    crawl_backward(0.5)
    wag_for(2.0)
    rotate_counterclockwise(0.25)
    wag_for(3.0)
    crawl_forward(0.5)
    wag_for(5.0)
    stop(all_legs)

demo1()

"""
Continuous servo based walking/waddling/etc robot.

Adafruit invests time and resources providing this open source code.
Please support Adafruit and open source hardware by purchasing
products from Adafruit!

Written by Dave Astels for Adafruit Industries
Copyright (c) 2018 Adafruit Industries
Licensed under the MIT license.

All text above must be included in any redistribution.
"""

import time
from adafruit_crickit import crickit

tail = crickit.dc_motor_1

# Each servo corresponds to one of the legs
front_right = crickit.continuous_servo_1
front_left = crickit.continuous_servo_2
rear_right = crickit.continuous_servo_3
rear_left = crickit.continuous_servo_4

# Useful groups of legs
all_legs = [front_right, front_left, rear_right, rear_left]
front_legs = [front_right, front_left]
rear_legs = [rear_right, rear_left]
right_legs = [front_right, rear_right]
left_legs = [front_left, rear_left]

# The sign (+1/-1) for forward motion for each servo
direction_values = {front_right: +1,
                    front_left: -1,
                    rear_right: +1,
                    rear_left: -1}

# Tweak the pwn ranges for each servo so that throttle of 0 stops the motor
pwm_ranges = {front_right: (500, 2400),
              front_left: (500, 2400),
              rear_right: (500, 2400),
              rear_left: (500, 2400)}


def init():
    for leg in all_legs:
        limits = pwm_ranges[leg]
        leg.set_pulse_width_range(min_pulse=limits[0], max_pulse=limits[1])
        leg.throttle = 0


def wag(speed):
    tail.throttle = speed
    time.sleep(0.1)
    tail.throttle = 0.0
    time.sleep(0.25)


def wag_for(seconds):
    target_time = time.monotonic() + seconds
    wag_throttle = 1.0
    while time.monotonic() < target_time:
        wag(wag_throttle)
        wag_throttle *= -1


def forward(servo_or_servos, speed):
    if isinstance(servo_or_servos, list):
        for servo in servo_or_servos:
            servo.throttle = speed * direction_values[servo]
    else:
        servo_or_servos.throttle = speed * direction_values[servo_or_servos]


def reverse(servo_or_servos, speed):
    if isinstance(servo_or_servos, list):
        for servo in servo_or_servos:
            servo.throttle = speed * -1 * direction_values[servo]
    else:
        servo_or_servos.throttle = speed * -1 * direction_values[servo_or_servos]


def stop(servo_or_servos):
    if isinstance(servo_or_servos, list):
        for servo in servo_or_servos:
            servo.throttle = 0
    else:
        servo_or_servos.throttle = 0


def rotate_clockwise(speed):
    forward(left_legs, speed)
    reverse(right_legs, speed)


def rotate_counterclockwise(speed):
    forward(right_legs, speed)
    reverse(left_legs, speed)


def crawl_forward(speed):
    forward(all_legs, speed)


def crawl_backward(speed):
    reverse(all_legs, speed)


def turtle():
    stop([rear_right, rear_left])
    stop(rear_left)
    forward(front_right, 0.5)
    forward(front_left, 0.5)


def snake_step():
    stop(all_legs)
    forward(right_legs, 0.5)
    time.sleep(1.0)
    stop(right_legs)
    forward(left_legs, 0.5)
    time.sleep(1.0)
    stop(left_legs)


init()

def demo1():
    crawl_forward(0.5)
    wag_for(5.0)
    rotate_clockwise(0.25)
    wag_for(3.0)
    crawl_backward(0.5)
    wag_for(2.0)
    rotate_counterclockwise(0.25)
    wag_for(3.0)
    crawl_forward(0.5)
    wag_for(5.0)
    stop(all_legs)

demo1()

Adafruit_Crickit crickit;
seesaw_Servo legs[] = {seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit)};

const int front_right = 0;
const int front_left = 1;
const int rear_right = 2;
const int rear_left = 3;

// Left and right motors turn in the opposite direction
const float motor_directions[4] = {+1.0, -1.0, +1.0, -1.0};

// pins to connect each servo to
const int servo_pins[4] = {CRICKIT_SERVO1, 
                           CRICKIT_SERVO2, 
                           CRICKIT_SERVO3, 
                           CRICKIT_SERVO4};

// PWM ranges for each motor, tune these so that setting the angle to 90 stops the motor
int pwm_ranges[4][2] = {{500, 2400}, 
                        {500, 2400}, 
                        {500, 2400}, 
                        {500, 2400}};

const __FlashStringHelper *leg_names[] = {F("Front right"), 
                                          F("Front left"), 
                                          F("Rear right"), 
                                          F("Rear left")};

Adafruit_Crickit crickit;
seesaw_Servo legs[] = {seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit)};

const int front_right = 0;
const int front_left = 1;
const int rear_right = 2;
const int rear_left = 3;

// Left and right motors turn in the opposite direction
const float motor_directions[4] = {+1.0, -1.0, +1.0, -1.0};

// pins to connect each servo to
const int servo_pins[4] = {CRICKIT_SERVO1, 
                           CRICKIT_SERVO2, 
                           CRICKIT_SERVO3, 
                           CRICKIT_SERVO4};

// PWM ranges for each motor, tune these so that setting the angle to 90 stops the motor
int pwm_ranges[4][2] = {{500, 2400}, 
                        {500, 2400}, 
                        {500, 2400}, 
                        {500, 2400}};

const __FlashStringHelper *leg_names[] = {F("Front right"), 
                                          F("Front left"), 
                                          F("Rear right"), 
                                          F("Rear left")};

void error(const __FlashStringHelper *err)
{
  digitalWrite(13, HIGH);
#ifdef DEBUG
  Serial.println(err);
#endif
  while (1);
}


void log(const __FlashStringHelper *msg)
{
#ifdef DEBUG
  Serial.println(msg);
#endif
}

void error(const __FlashStringHelper *err)
{
  digitalWrite(13, HIGH);
#ifdef DEBUG
  Serial.println(err);
#endif
  while (1);
}


void log(const __FlashStringHelper *msg)
{
#ifdef DEBUG
  Serial.println(msg);
#endif
}

int speed_to_angle(float speed)
{
  return (int)(speed * 90.0 + 90.0);
}


void set_leg(int leg, float speed)
{
  int angle = speed_to_angle(speed * motor_directions[leg]);
#ifdef DEBUG
  Serial.print(F("Setting "));
  Serial.print(leg_names[leg]);
  Serial.print(F(" to "));
  Serial.println(angle);
#endif
  legs[leg].write(angle);
}

int speed_to_angle(float speed)
{
  return (int)(speed * 90.0 + 90.0);
}


void set_leg(int leg, float speed)
{
  int angle = speed_to_angle(speed * motor_directions[leg]);
#ifdef DEBUG
  Serial.print(F("Setting "));
  Serial.print(leg_names[leg]);
  Serial.print(F(" to "));
  Serial.println(angle);
#endif
  legs[leg].write(angle);
}

// Stop the listed motors
// -1 required as the last argument

void stop(int leg, ...)
{
  va_list args;
  va_start(args, leg);
  log(F("Stop"));
  while (leg != -1) {
    set_leg(leg, 0.0);
    leg = va_arg(args, int);
  }
  va_end(args);
}

void stop_all()
{
  stop(front_right, front_left, rear_right, rear_left, -1);
}

// Stop the listed motors
// -1 required as the last argument

void stop(int leg, ...)
{
  va_list args;
  va_start(args, leg);
  log(F("Stop"));
  while (leg != -1) {
    set_leg(leg, 0.0);
    leg = va_arg(args, int);
  }
  va_end(args);
}

void stop_all()
{
  stop(front_right, front_left, rear_right, rear_left, -1);
}

void forward(float speed, ...)
{
  va_list args;
  va_start(args, speed);
  int leg = va_arg(args, int);
  log(F("Forward"));
  while (leg != -1) {
    set_leg(leg, speed * motor_directions[leg]);
    leg = va_arg(args, int);
  }

  va_end(args);
}


void forward_all(float speed)
{
  forward(speed, front_right, front_left, rear_right, rear_left, -1);
}


void reverse(float speed, ...)
{
  va_list args;
  va_start(args, speed);
  int leg = va_arg(args, int);
  log(F("Reverse"));
  while (leg != -1) {
    set_leg(leg, speed * -1 * motor_directions[leg]);
    leg = va_arg(args, int);
  }

  va_end(args);
}


void reverse_all(float speed)
{
  reverse(speed, front_right, front_left, rear_right, rear_left, -1);
}

void forward(float speed, ...)
{
  va_list args;
  va_start(args, speed);
  int leg = va_arg(args, int);
  log(F("Forward"));
  while (leg != -1) {
    set_leg(leg, speed * motor_directions[leg]);
    leg = va_arg(args, int);
  }

  va_end(args);
}


void forward_all(float speed)
{
  forward(speed, front_right, front_left, rear_right, rear_left, -1);
}


void reverse(float speed, ...)
{
  va_list args;
  va_start(args, speed);
  int leg = va_arg(args, int);
  log(F("Reverse"));
  while (leg != -1) {
    set_leg(leg, speed * -1 * motor_directions[leg]);
    leg = va_arg(args, int);
  }

  va_end(args);
}


void reverse_all(float speed)
{
  reverse(speed, front_right, front_left, rear_right, rear_left, -1);
}

void rotate_clockwise(float speed)
{
  forward(speed, front_left, rear_left, -1);
  reverse(speed, front_right, rear_right, -1);
}


void rotate_counterclockwise(float speed)
{
  forward(speed, front_right, rear_right, -1);
  reverse(speed, front_left, rear_left, -1);
}

void rotate_clockwise(float speed)
{
  forward(speed, front_left, rear_left, -1);
  reverse(speed, front_right, rear_right, -1);
}


void rotate_counterclockwise(float speed)
{
  forward(speed, front_right, rear_right, -1);
  reverse(speed, front_left, rear_left, -1);
}

seesaw_Motor tail(&crickit);
boolean tail_power = 0.5;

//...

void wag(float speed)
{
  tail.throttle(speed);
  delay(75);
  tail.throttle(0.0);
  delay(50);
}

//...

void loop()
{
  wag(tail_power);
  tail_power *= -1.0;
  //...
}

seesaw_Motor tail(&crickit);
boolean tail_power = 0.5;

//...

void wag(float speed)
{
  tail.throttle(speed);
  delay(75);
  tail.throttle(0.0);
  delay(50);
}

//...

void loop()
{
  wag(tail_power);
  tail_power *= -1.0;
  //...
}

void loop()
{
  // wag(tail_power);
  tail_power *= -1.0;

  // Wait for new data to arrive
  uint8_t len = readPacket(&ble, BLE_READPACKET_TIMEOUT);
  if (len == 0) return;

  // Got a packet!
  // printHex(packetbuffer, len);

   // Buttons
  if (packetbuffer[1] == 'B') {
    uint8_t buttnum = packetbuffer[2] - '0';
    boolean pressed = packetbuffer[3] - '0';

#ifdef DEBUG
    Serial.print ("Button "); Serial.print(buttnum);
    if (pressed) {
      Serial.println(" pressed");
    } else {
      Serial.println(" released");
    }
#endif
    switch(buttnum) {
    case 1:
      if (pressed) {
        demo1();
      }
      break;
    case 2:
      if (pressed) {
        demo2();
      }
      break;
    case 3:
      if (pressed) {
        demo3();
      }
      break;
    case 4:
      if (pressed) {
        demo4();
      }
      break;
    case 5:
      if (pressed) {
        rotate_counterclockwise(0.5);
      } else {
        stop_all();
      }
      break;
    case 6:
      if (pressed) {
        rotate_clockwise(0.5);
      } else {
        stop_all();
      }
      break;
    case 7:
      if (pressed) {
        reverse_all(0.5);
      } else {
        stop_all();
      }
      break;
    case 8:
      if (pressed) {
        forward_all(0.5);
      } else {
        stop_all();
      }
      break;
    }
  }
}

void loop()
{
  // wag(tail_power);
  tail_power *= -1.0;

  // Wait for new data to arrive
  uint8_t len = readPacket(&ble, BLE_READPACKET_TIMEOUT);
  if (len == 0) return;

  // Got a packet!
  // printHex(packetbuffer, len);

   // Buttons
  if (packetbuffer[1] == 'B') {
    uint8_t buttnum = packetbuffer[2] - '0';
    boolean pressed = packetbuffer[3] - '0';

#ifdef DEBUG
    Serial.print ("Button "); Serial.print(buttnum);
    if (pressed) {
      Serial.println(" pressed");
    } else {
      Serial.println(" released");
    }
#endif
    switch(buttnum) {
    case 1:
      if (pressed) {
        demo1();
      }
      break;
    case 2:
      if (pressed) {
        demo2();
      }
      break;
    case 3:
      if (pressed) {
        demo3();
      }
      break;
    case 4:
      if (pressed) {
        demo4();
      }
      break;
    case 5:
      if (pressed) {
        rotate_counterclockwise(0.5);
      } else {
        stop_all();
      }
      break;
    case 6:
      if (pressed) {
        rotate_clockwise(0.5);
      } else {
        stop_all();
      }
      break;
    case 7:
      if (pressed) {
        reverse_all(0.5);
      } else {
        stop_all();
      }
      break;
    case 8:
      if (pressed) {
        forward_all(0.5);
      } else {
        stop_all();
      }
      break;
    }
  }
}

// Continuous servo based walking/waddling/etc robot.

// Bluetooth code is from Feather M0 Bluefruit controller example.
// Explainatory comments kept intact.

// Adafruit invests time and resources providing this open source code.
// Please support Adafruit and open source hardware by purchasing
// products from Adafruit!

// Written by Dave Astels for Adafruit Industries
// Copyright (c) 2018 Adafruit Industries
// Licensed under the MIT license.

// All text above must be included in any redistribution.

#include <stdarg.h>
#include <string.h>
#include <Arduino.h>
#include <SPI.h>
#include "Adafruit_BLE.h"
#include "Adafruit_BluefruitLE_SPI.h"
#include "Adafruit_BluefruitLE_UART.h"

#include "BluefruitConfig.h"

#include "Adafruit_Crickit.h"
#include "seesaw_servo.h"
#include "seesaw_motor.h"

#define FACTORYRESET_ENABLE         1
#define MINIMUM_FIRMWARE_VERSION    "0.6.6"
#define MODE_LED_BEHAVIOUR          "MODE"

// function prototypes over in packetparser.cpp
uint8_t readPacket(Adafruit_BLE *ble, uint16_t timeout);
float parsefloat(uint8_t *buffer);
void printHex(const uint8_t * data, const uint32_t numBytes);

// the packet buffer
extern uint8_t packetbuffer[];

//#define DEBUG 1


Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);


//------------------------------------------------------------------------------
// setup crickit

Adafruit_Crickit crickit;
seesaw_Servo legs[] = {seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit)};

const int front_right = 0;
const int front_left = 1;
const int rear_right = 2;
const int rear_left = 3;

seesaw_Motor tail(&crickit);
float tail_power = 0.5;

//------------------------------------------------------------------------------
// conditional output routines

void error(const __FlashStringHelper *err)
{
  digitalWrite(13, HIGH);
#ifdef DEBUG
  Serial.println(err);
#endif
  while (1);
}


void log(const __FlashStringHelper *msg)
{
#ifdef DEBUG
  Serial.println(msg);
#endif
}

//------------------------------------------------------------------------------
// Motor Control

// Left and right motors turn in the opposite direction
const float motor_directions[4] = {+1.0, -1.0, +1.0, -1.0};

// pins to connect each servo to
const int servo_pins[4] = {CRICKIT_SERVO1, CRICKIT_SERVO2, CRICKIT_SERVO3, CRICKIT_SERVO4};

// PWM ranges for each motor, tune these so that setting the angle to 90 stops the motor
int pwm_ranges[4][2] = {{500, 2400}, {500, 2400}, {500, 2400}, {500, 2400}};

const __FlashStringHelper *leg_names[] = {F("Front right"), F("Front left"), F("Rear right"), F("Rear left")};


int speed_to_angle(float speed)
{
  return (int)(speed * 90.0 + 90.0);
}


void set_leg(int leg, float speed)
{
  int angle = speed_to_angle(speed * motor_directions[leg]);
#ifdef DEBUG
  Serial.print(F("Setting "));
  Serial.print(leg_names[leg]);
  Serial.print(F(" to "));
  Serial.println(angle);
#endif
  legs[leg].write(angle);
}


// Stop the listed motors
// -1 required as the last argument

void stop(int leg, ...)
{
  va_list args;
  va_start(args, leg);
  log(F("Stop"));
  while (leg != -1) {
    set_leg(leg, 0.0);
    leg = va_arg(args, int);
  }

  va_end(args);
}


void stop_all()
{
  stop(front_right, front_left, rear_right, rear_left, -1);
}


void forward(float speed, ...)
{
  va_list args;
  va_start(args, speed);
  int leg = va_arg(args, int);
  log(F("Forward"));
  while (leg != -1) {
    set_leg(leg, speed * motor_directions[leg]);
    leg = va_arg(args, int);
  }

  va_end(args);
}


void forward_all(float speed)
{
  forward(speed, front_right, front_left, rear_right, rear_left, -1);
}


void reverse(float speed, ...)
{
  va_list args;
  va_start(args, speed);
  int leg = va_arg(args, int);
  log(F("Reverse"));
  while (leg != -1) {
    set_leg(leg, speed * -1 * motor_directions[leg]);
    leg = va_arg(args, int);
  }

  va_end(args);
}


void reverse_all(float speed)
{
  reverse(speed, front_right, front_left, rear_right, rear_left, -1);
}


void rotate_clockwise(float speed)
{
  forward(speed, front_left, rear_left, -1);
  reverse(speed, front_right, rear_right, -1);
}


void rotate_counterclockwise(float speed)
{
  forward(speed, front_right, rear_right, -1);
  reverse(speed, front_left, rear_left, -1);
}


void initialize()
{
  stop(front_right, front_left, rear_right, rear_left, -1);
}


void wag(float speed)
{
#ifdef DEBUG
  Serial.print(F("Wag "));
  Serial.println(speed);
#endif
  tail.throttle(speed);
  delay(75);
  tail.throttle(0.0);
  delay(50);
}


//------------------------------------------------------------------------------
// Start things up

void setup()
{
  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);

#ifdef DEBUG
  while (!Serial);  // required for Flora & Micro
  delay(500);

  Serial.begin(115200);
#endif

  log(F("WobblyBot"));
  log(F("-----------------------------------------"));

  // Initialise the module
  log(F("Initialising the Bluefruit LE module: "));

  if ( !ble.begin(VERBOSE_MODE) )
  {
    error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
  }

  log( F("OK!") );

  if ( FACTORYRESET_ENABLE )
  {
    // Perform a factory reset to make sure everything is in a known state
    log(F("Performing a factory reset: "));
    if ( ! ble.factoryReset() ){
      error(F("Couldn't factory reset"));
    }
  }

   // Disable command echo from Bluefruit
  ble.echo(false);

  log(F("Requesting Bluefruit info:"));
  // Print Bluefruit information
  ble.info();

  log(F("Please use Adafruit Bluefruit LE app to connect in Controller mode"));
  log(F("Then activate/use the sensors, color picker, game controller, etc!\n"));

  ble.verbose(false);  // debug info is a little annoying after this point!

  // Wait for connection
  while (! ble.isConnected()) {
      delay(500);
  }

  log(F("******************************"));

  // LED Activity command is only supported from 0.6.6
  if ( ble.isVersionAtLeast(MINIMUM_FIRMWARE_VERSION) )
  {
    // Change Mode LED Activity
    log(F("Change LED activity to " MODE_LED_BEHAVIOUR));
    ble.sendCommandCheckOK("AT+HWModeLED=" MODE_LED_BEHAVIOUR);
  }

  // Set Bluefruit to DATA mode
  log( F("Switching to DATA mode!") );
  ble.setMode(BLUEFRUIT_MODE_DATA);

  log(F("******************************"));

  if (!crickit.begin()) {
    error(F("Error initializing CRICKIT!"));
  }
  log(F("Crickit started"));

  for (int leg = 0; leg < 4; leg++) {
    legs[leg].attach(servo_pins[leg], pwm_ranges[leg][0], pwm_ranges[leg][1]);
  }

  tail.attach(CRICKIT_MOTOR_A1, CRICKIT_MOTOR_A2);
}


// Fill these functions in with the movement scripts you want attached to
// the controller's 1-4 buttons

void demo1()
{
  forward_all(0.5);
  delay(5000);
  rotate_clockwise(0.5);
  delay(2000);
  forward_all(0.75);
  delay(4000);
  rotate_counterclockwise(0.5);
  delay(3000);
  stop_all();
}


void demo2()
{
}


void demo3()
{
}


void demo4()
{
}


//------------------------------------------------------------------------------
// Main loop

void loop()
{
  wag(tail_power);
  tail_power *= -1.0;

  // Wait for new data to arrive
  uint8_t len = readPacket(&ble, BLE_READPACKET_TIMEOUT);
  if (len == 0) return;

  // Got a packet!
  // printHex(packetbuffer, len);

   // Buttons
  if (packetbuffer[1] == 'B') {
    uint8_t buttnum = packetbuffer[2] - '0';
    boolean pressed = packetbuffer[3] - '0';

#ifdef DEBUG
    Serial.print ("Button "); Serial.print(buttnum);
    if (pressed) {
      Serial.println(" pressed");
    } else {
      Serial.println(" released");
    }
#endif
    switch(buttnum) {
    case 1:
      if (pressed) {
        demo1();
      }
      break;
    case 2:
      if (pressed) {
        demo2();
      }
      break;
    case 3:
      if (pressed) {
        demo3();
      }
      break;
    case 4:
      if (pressed) {
        demo4();
      }
      break;
    case 5:
      if (pressed) {
        rotate_counterclockwise(0.5);
      } else {
        stop_all();
      }
      break;
    case 6:
      if (pressed) {
        rotate_clockwise(0.5);
      } else {
        stop_all();
      }
      break;
    case 7:
      if (pressed) {
        reverse_all(0.5);
      } else {
        stop_all();
      }
      break;
    case 8:
      if (pressed) {
        forward_all(0.5);
      } else {
        stop_all();
      }
      break;
    }
  }
}

// Continuous servo based walking/waddling/etc robot.

// Bluetooth code is from Feather M0 Bluefruit controller example.
// Explainatory comments kept intact.

// Adafruit invests time and resources providing this open source code.
// Please support Adafruit and open source hardware by purchasing
// products from Adafruit!

// Written by Dave Astels for Adafruit Industries
// Copyright (c) 2018 Adafruit Industries
// Licensed under the MIT license.

// All text above must be included in any redistribution.

#include <stdarg.h>
#include <string.h>
#include <Arduino.h>
#include <SPI.h>
#include "Adafruit_BLE.h"
#include "Adafruit_BluefruitLE_SPI.h"
#include "Adafruit_BluefruitLE_UART.h"

#include "BluefruitConfig.h"

#include "Adafruit_Crickit.h"
#include "seesaw_servo.h"
#include "seesaw_motor.h"

#define FACTORYRESET_ENABLE         1
#define MINIMUM_FIRMWARE_VERSION    "0.6.6"
#define MODE_LED_BEHAVIOUR          "MODE"

// function prototypes over in packetparser.cpp
uint8_t readPacket(Adafruit_BLE *ble, uint16_t timeout);
float parsefloat(uint8_t *buffer);
void printHex(const uint8_t * data, const uint32_t numBytes);

// the packet buffer
extern uint8_t packetbuffer[];

//#define DEBUG 1


Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);


//------------------------------------------------------------------------------
// setup crickit

Adafruit_Crickit crickit;
seesaw_Servo legs[] = {seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit),
                       seesaw_Servo(&crickit)};

const int front_right = 0;
const int front_left = 1;
const int rear_right = 2;
const int rear_left = 3;

seesaw_Motor tail(&crickit);
float tail_power = 0.5;

//------------------------------------------------------------------------------
// conditional output routines

void error(const __FlashStringHelper *err)
{
  digitalWrite(13, HIGH);
#ifdef DEBUG
  Serial.println(err);
#endif
  while (1);
}


void log(const __FlashStringHelper *msg)
{
#ifdef DEBUG
  Serial.println(msg);
#endif
}

//------------------------------------------------------------------------------
// Motor Control

// Left and right motors turn in the opposite direction
const float motor_directions[4] = {+1.0, -1.0, +1.0, -1.0};

// pins to connect each servo to
const int servo_pins[4] = {CRICKIT_SERVO1, CRICKIT_SERVO2, CRICKIT_SERVO3, CRICKIT_SERVO4};

// PWM ranges for each motor, tune these so that setting the angle to 90 stops the motor
int pwm_ranges[4][2] = {{500, 2400}, {500, 2400}, {500, 2400}, {500, 2400}};

const __FlashStringHelper *leg_names[] = {F("Front right"), F("Front left"), F("Rear right"), F("Rear left")};


int speed_to_angle(float speed)
{
  return (int)(speed * 90.0 + 90.0);
}


void set_leg(int leg, float speed)
{
  int angle = speed_to_angle(speed * motor_directions[leg]);
#ifdef DEBUG
  Serial.print(F("Setting "));
  Serial.print(leg_names[leg]);
  Serial.print(F(" to "));
  Serial.println(angle);
#endif
  legs[leg].write(angle);
}


// Stop the listed motors
// -1 required as the last argument

void stop(int leg, ...)
{
  va_list args;
  va_start(args, leg);
  log(F("Stop"));
  while (leg != -1) {
    set_leg(leg, 0.0);
    leg = va_arg(args, int);
  }

  va_end(args);
}


void stop_all()
{
  stop(front_right, front_left, rear_right, rear_left, -1);
}


void forward(float speed, ...)
{
  va_list args;
  va_start(args, speed);
  int leg = va_arg(args, int);
  log(F("Forward"));
  while (leg != -1) {
    set_leg(leg, speed * motor_directions[leg]);
    leg = va_arg(args, int);
  }

  va_end(args);
}


void forward_all(float speed)
{
  forward(speed, front_right, front_left, rear_right, rear_left, -1);
}


void reverse(float speed, ...)
{
  va_list args;
  va_start(args, speed);
  int leg = va_arg(args, int);
  log(F("Reverse"));
  while (leg != -1) {
    set_leg(leg, speed * -1 * motor_directions[leg]);
    leg = va_arg(args, int);
  }

  va_end(args);
}


void reverse_all(float speed)
{
  reverse(speed, front_right, front_left, rear_right, rear_left, -1);
}


void rotate_clockwise(float speed)
{
  forward(speed, front_left, rear_left, -1);
  reverse(speed, front_right, rear_right, -1);
}


void rotate_counterclockwise(float speed)
{
  forward(speed, front_right, rear_right, -1);
  reverse(speed, front_left, rear_left, -1);
}


void initialize()
{
  stop(front_right, front_left, rear_right, rear_left, -1);
}


void wag(float speed)
{
#ifdef DEBUG
  Serial.print(F("Wag "));
  Serial.println(speed);
#endif
  tail.throttle(speed);
  delay(75);
  tail.throttle(0.0);
  delay(50);
}


//------------------------------------------------------------------------------
// Start things up

void setup()
{
  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);

#ifdef DEBUG
  while (!Serial);  // required for Flora & Micro
  delay(500);

  Serial.begin(115200);
#endif

  log(F("WobblyBot"));
  log(F("-----------------------------------------"));

  // Initialise the module
  log(F("Initialising the Bluefruit LE module: "));

  if ( !ble.begin(VERBOSE_MODE) )
  {
    error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
  }

  log( F("OK!") );

  if ( FACTORYRESET_ENABLE )
  {
    // Perform a factory reset to make sure everything is in a known state
    log(F("Performing a factory reset: "));
    if ( ! ble.factoryReset() ){
      error(F("Couldn't factory reset"));
    }
  }

   // Disable command echo from Bluefruit
  ble.echo(false);

  log(F("Requesting Bluefruit info:"));
  // Print Bluefruit information
  ble.info();

  log(F("Please use Adafruit Bluefruit LE app to connect in Controller mode"));
  log(F("Then activate/use the sensors, color picker, game controller, etc!\n"));

  ble.verbose(false);  // debug info is a little annoying after this point!

  // Wait for connection
  while (! ble.isConnected()) {
      delay(500);
  }

  log(F("******************************"));

  // LED Activity command is only supported from 0.6.6
  if ( ble.isVersionAtLeast(MINIMUM_FIRMWARE_VERSION) )
  {
    // Change Mode LED Activity
    log(F("Change LED activity to " MODE_LED_BEHAVIOUR));
    ble.sendCommandCheckOK("AT+HWModeLED=" MODE_LED_BEHAVIOUR);
  }

  // Set Bluefruit to DATA mode
  log( F("Switching to DATA mode!") );
  ble.setMode(BLUEFRUIT_MODE_DATA);

  log(F("******************************"));

  if (!crickit.begin()) {
    error(F("Error initializing CRICKIT!"));
  }
  log(F("Crickit started"));

  for (int leg = 0; leg < 4; leg++) {
    legs[leg].attach(servo_pins[leg], pwm_ranges[leg][0], pwm_ranges[leg][1]);
  }

  tail.attach(CRICKIT_MOTOR_A1, CRICKIT_MOTOR_A2);
}


// Fill these functions in with the movement scripts you want attached to
// the controller's 1-4 buttons

void demo1()
{
  forward_all(0.5);
  delay(5000);
  rotate_clockwise(0.5);
  delay(2000);
  forward_all(0.75);
  delay(4000);
  rotate_counterclockwise(0.5);
  delay(3000);
  stop_all();
}


void demo2()
{
}


void demo3()
{
}


void demo4()
{
}


//------------------------------------------------------------------------------
// Main loop

void loop()
{
  wag(tail_power);
  tail_power *= -1.0;

  // Wait for new data to arrive
  uint8_t len = readPacket(&ble, BLE_READPACKET_TIMEOUT);
  if (len == 0) return;

  // Got a packet!
  // printHex(packetbuffer, len);

   // Buttons
  if (packetbuffer[1] == 'B') {
    uint8_t buttnum = packetbuffer[2] - '0';
    boolean pressed = packetbuffer[3] - '0';

#ifdef DEBUG
    Serial.print ("Button "); Serial.print(buttnum);
    if (pressed) {
      Serial.println(" pressed");
    } else {
      Serial.println(" released");
    }
#endif
    switch(buttnum) {
    case 1:
      if (pressed) {
        demo1();
      }
      break;
    case 2:
      if (pressed) {
        demo2();
      }
      break;
    case 3:
      if (pressed) {
        demo3();
      }
      break;
    case 4:
      if (pressed) {
        demo4();
      }
      break;
    case 5:
      if (pressed) {
        rotate_counterclockwise(0.5);
      } else {
        stop_all();
      }
      break;
    case 6:
      if (pressed) {
        rotate_clockwise(0.5);
      } else {
        stop_all();
      }
      break;
    case 7:
      if (pressed) {
        reverse_all(0.5);
      } else {
        stop_all();
      }
      break;
    case 8:
      if (pressed) {
        forward_all(0.5);
      } else {
        stop_all();
      }
      break;
    }
  }
}