#include <math.h>
#include "pico/stdlib.h"

#include "pico_motor_shim.hpp"

/*
Demonstrates how to create two Motor objects and control them together.
*/

using namespace motor;

// How many sweeps of the motors to perform
const uint SWEEPS = 2;

// The number of discrete sweep steps
const uint STEPS = 10;

// The time in milliseconds between each step of the sequence
const uint STEPS_INTERVAL_MS = 500;

// How far from zero to drive the motors when sweeping
constexpr float SPEED_EXTENT = 1.0f;

// Create an array of motor pointers
const pin_pair motor_pins[] = {pico_motor_shim::MOTOR_1, pico_motor_shim::MOTOR_2};
const uint NUM_MOTORS = count_of(motor_pins);
Motor *motors[NUM_MOTORS];


int main() {
  stdio_init_all();

  // Fill the array of motors, and initialise them. Up to 8 motors can be created
  for(auto m = 0u; m < NUM_MOTORS; m++) {
    motors[m] = new Motor(motor_pins[m]);
    motors[m]->init();
  }

  // Uncomment the below lines to reverse
  // the driving direction of a motor
  // motors[0].direction(REVERSED_DIR);
  // motors[1].direction(REVERSED_DIR);

  // Enable all motors
  for(auto m = 0u; m < NUM_MOTORS; m++) {
    motors[m]->enable();
  }
  sleep_ms(2000);

  // Drive at full positive
  for(auto m = 0u; m < NUM_MOTORS; m++) {
    motors[m]->full_positive();
  }
  sleep_ms(2000);

  // Stop all moving
  for(auto m = 0u; m < NUM_MOTORS; m++) {
    motors[m]->stop();
  }
  sleep_ms(2000);

  // Drive at full negative
  for(auto m = 0u; m < NUM_MOTORS; m++) {
    motors[m]->full_negative();
  }
  sleep_ms(2000);

  // Coast all to a gradual stop
  for(auto m = 0u; m < NUM_MOTORS; m++) {
    motors[m]->coast();
  }
  sleep_ms(2000);

  // Do a sine speed sweep
  for(auto j = 0u; j < SWEEPS; j++) {
    for(auto i = 0u; i < 360; i++) {
      float speed = sin(((float)i * (float)M_PI) / 180.0f) * SPEED_EXTENT;
      for(auto m = 0u; m < NUM_MOTORS; m++) {
        motors[m]->speed(speed);
      }
      sleep_ms(20);
    }
  }

  // Do a stepped speed sweep
  for(auto j = 0u; j < SWEEPS; j++) {
    for(auto i = 0u; i < STEPS; i++) {
      for(auto m = 0u; m < NUM_MOTORS; m++) {
        motors[m]->to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
      }
      sleep_ms(STEPS_INTERVAL_MS);
    }
    for(auto i = 0u; i < STEPS; i++) {
      for(auto m = 0u; m < NUM_MOTORS; m++) {
        motors[m]->to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT);
      }
      sleep_ms(STEPS_INTERVAL_MS);
    }
  }

  // Disable the motors
  for(auto m = 0u; m < NUM_MOTORS; m++) {
    motors[m]->disable();
  }
}