#include "btn99x0_motor_control.hpp"
#include "btn99x0_half_bridge.hpp"
#include <Adafruit_NeoPixel.h>

using namespace btn99x0;

#define HB1_IN 1 // PWM Pin for DC-Shield
#define HB2_IN 2 // PWM Pin for DC-Shield
#define HB1_INH 3 // Inhibit PIN for Halfbridge 1 on DC shield
#define HB2_INH 4 // Inhibit PIN for Halfbridge 2 on DC shield
#define HB1_Isense A1 // Diagnosis pin for Half-bridge 1
#define HB2_Isense A0 // Diagnosis pin for Half-bridge 2
#define NEO_PIN 3 // NeoPixel pin

#define SPEED 150


io_pins_t hb1_io_pins
{
  HB1_Isense,
  HB1_IN,
  HB1_INH
};

io_pins_t hb2_io_pins
{
  HB2_Isense, 
  HB2_IN,
  HB2_INH
};

hw_conf_t hw_conf =
{
    2000, // Resistor on the DC shield for the Diagnosis pin
    3.3,  // Maximum voltage on the ADC Pin
    1023
};

DCShield shield(hb1_io_pins, hb2_io_pins, hw_conf);
MotorControl btn_motor_control(shield);

Adafruit_NeoPixel neoPixel = Adafruit_NeoPixel(8, NEO_PIN, NEO_GRB + NEO_KHZ800);



void setup() {
  Serial.begin(9600);
  Serial.println("Serial initialized");

  delay(5000);
  btn_motor_control.begin();
  delay(2000);

  // Set the slew rate
  btn_motor_control.set_slew_rate(SLEW_RATE_LEVEL_7);

  neoPixel.begin();
}

void dispense() {
  Serial.println("Dispensing...");
  neoPixel.setPixelColor(0, neoPixel.Color(255, 255, 0)); // Yellow
  neoPixel.show();
  delay(500);
  neoPixel.setPixelColor(0, neoPixel.Color(255, 0, 0)); // Red
  neoPixel.show();
  delay(500);

  for (int i = 0; i < 2; i++) { // Dispense motor control
    btn_motor_control.set_speed(-SPEED); // Turn motor in positive direction
    delay(400); // Short delay
    btn_motor_control.set_speed(+SPEED); // Turn motor in negative direction
    delay(100); // Short delay
  }

  btn_motor_control.set_speed(0); // Stop the motor after dispensing
  neoPixel.setPixelColor(0, neoPixel.Color(0, 0, 0)); // Turn off NeoPixel
  neoPixel.show();
  delay(3000);
  
}

void loop() {
  int radarValue = digitalRead(5); // Read from digital pin 5
  if (radarValue == 1) {
    dispense(); // Run the dispense function when pin 5 is 0
  }
}