// Fixed Sampling EEG Filter - BioAmp EXG Pill

#define SAMPLE_RATE 125
#define BAUD_RATE 115200
#define INPUT_PIN A0
#include <SoftwareSerial.h>
// Define motor control pins
const int motor1A = 13; // D13
const int motor1B = 12; // D12
const int motor2A = 9; // D9 (Corrected from D10)
const int motor2B = 8; // D8 (Corrected from D11)
int enA = 7;
int enB = 4;
int echo = 6; // ultrasonic
int trgr = 5; //ultrasonix

int inches = 0;
int cm = 0;

//button vals
int button = 7;
int buttonVal = 0;

//led test
int led = 8;
//BT initialize
SoftwareSerial mySerial(rx, tx); // RX, TX pins

void setup() {
  //button setup
  pinMode(button, INPUT);
  Serial.begin(BAUD_RATE);
  //Bluetooth Connection
  mySerial.begin(BAUD_RATE);
   pinMode(rx, INPUT);
  pinMode(tx,OUTPUT);
  // Set motor control pins as OUTPUT
  pinMode(motor1A, OUTPUT);
  pinMode(motor1B, OUTPUT);
  pinMode(motor2A, OUTPUT);
  pinMode(motor2B, OUTPUT);
  pinMode(A0, INPUT);
  //Serial.begin(BAUD_RATE); // Changed to use BAUD_RATE constant
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  //only for testing will be deleted later
  pinMode(led, OUTPUT);
}


void loop() {
  //button setup
  	buttonVal = digitalRead(button);
  	Serial.println(buttonVal);

  if(mySerial.available()){
    //=======CODING FOR BUTTON=======
    	if(buttonVal == 1)
      {
        motorStop();
      }
        //=======CODING FOR ULTRASONIC SENSOR=========
       // measure the ping time in cm
  cm = 0.01723 * readUltrasonicDistance(trgr, echo);
  // convert to inches by dividing by 2.54
  inches = (cm / 2.54);
      Serial.println();
  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.println("cm");
  delay(100); // Wait for 100 millisecond(s)
  if(inches == 30)
  {
    motorStop();
  }

    // Read the data from the serial port
    char receivedData = mySerial.read();
    Serial.print(receivedData);
    if (receivedData == 0) {
        motorStop();
        for(int j = 1; j <= 3; j++){
        digitalWrite(led, HIGH);
        delay(200);
        digitalWrite(led, LOW);
        delay(200);}
      }
      else if (receivedData == 1)
      {
        motorLeft(); // Corrected function name
        delay(1000);
        motorForward();
        delay(3000);
        motorStop();
      }
      else if (receivedData == 2)
      {
        motorRight(); // Corrected function name
        delay(1000);
        motorForward();
        delay(3000);
        motorStop();
      }
      else if(receivedData == 3)
      {
      motorForward();
      delay(3000);
      motorStop();
      }
      //FOR TESTING PURPOSE ONLY
      else if(receivedData == 4)
      {
        for(int i = 1; i <= 5; i++)
        {
        digitalWrite(led, HIGH);
        delay(500);
        digitalWrite(led, LOW);
        delay(500);
        }
      }
   }
  static unsigned long past = 0;
  unsigned long present = micros();
  unsigned long interval = present - past;
  past = present;

  // Run timer
  static long timer = 0;
  timer -= interval;

  // Sample
  if (timer < 0) {
    timer += 1000000 / SAMPLE_RATE;
    int sensor_value = analogRead(INPUT_PIN);
    float signal = EEGFilter(sensor_value);
    Serial.println(signal);
  }
}

// Band-Pass Butterworth IIR digital filter, generated using filter_gen.py.
// Sampling rate: 125.0 Hz, frequency: [0.5, 29.5] Hz.
// Filter is order 4, implemented as second-order sections (biquads).
// Reference:
// https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.butter.html
// https://courses.ideate.cmu.edu/16-223/f2020/Arduino/FilterDemos/filter_gen.py

void motorForward() {
  digitalWrite(motor1A, LOW);
  digitalWrite(motor1B, HIGH);
  digitalWrite(motor2A, HIGH);
  digitalWrite(motor2B, LOW);
  analogWrite(enA, 100); // Corrected to use PWM for speed control
  analogWrite(enB, 100); // Corrected to use PWM for speed control
}

void motorBackward() {
  digitalWrite(motor1A, LOW);
  digitalWrite(motor1B, HIGH);
  digitalWrite(motor2A, LOW);
  digitalWrite(motor2B, HIGH);
  analogWrite(enA, 100); // Corrected to use PWM for speed control
  analogWrite(enB, 100); // Corrected to use PWM for speed control
}

void motorStop() {
  digitalWrite(motor1A, LOW);
  digitalWrite(motor1B, LOW);
  digitalWrite(motor2A, LOW);
  digitalWrite(motor2B, LOW);
  analogWrite(enA, 0); // Corrected to stop the motor
  analogWrite(enB, 0); // Corrected to stop the motor
}

void motorRight() {
  digitalWrite(motor1A, LOW);
  digitalWrite(motor1B, HIGH);
  digitalWrite(motor2A, LOW);
  digitalWrite(motor2B, LOW);
  analogWrite(enA, 100); // Corrected to use PWM for speed control
  analogWrite(enB, 255); // Corrected to use PWM for speed control
}

void motorLeft() {
  digitalWrite(motor1A, LOW);
  digitalWrite(motor1B, LOW);
  digitalWrite(motor2A, HIGH);
  digitalWrite(motor2B, LOW);
  analogWrite(enA, 100); // Corrected to use PWM for speed control
  analogWrite(enB, 100); // Corrected to use PWM for speed control
}

long readUltrasonicDistance(int triggerPin, int echoPin)
{
  pinMode(triggerPin, OUTPUT);  // Clear the trigger
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  // Sets the trigger pin to HIGH state for 10 microseconds
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  // Reads the echo pin, and returns the sound wave travel time in microseconds
  return pulseIn(echoPin, HIGH);
}

float EEGFilter(float input) {
  float output = input;
  {
    static float z1, z2; // filter section state
    float x = output - -0.76828189 * z1 - 0.29112154 * z2;
    output = 0.00391759 * x + 0.00783519 * z1 + 0.00391759 * z2;
    z2 = z1;
    z1 = x;
  }
  {
    static float z1, z2; // filter section state
    float x = output - -0.99386185 * z1 - 0.67373813 * z2;
    output = 1.00000000 * x + 1.99744439 * z1 + 1.00000000 * z2;
    z2 = z1;
    z1 = x;
  }
  {
    static float z1, z2; // filter section state
    float x = output - -1.95557810 * z1 - 0.95654368 * z2;
    output = 1.00000000 * x + -1.99963446 * z1 + 1.00000000 * z2;
    z2 = z1;
    z1 = x;
  }
  {
    static float z1, z2; // filter section state
    float x = output - -1.97805789 * z1 - 0.97817022 * z2;
    output = 1.00000000 * x + -1.99999796 * z1 + 1.00000000 * z2;
    z2 = z1;
    z1 = x;
  }
  return output;
}