//add RGB ✔
//add Buzzer + POT ✔
//add Ultrasonic ✔
//add LDR
//add LEDs
//<2✔><3~✔><4✔><5~✔><6~✔><7✔><8✔><9~✔><10~✔><11~✔><12✔><13>
//<A0✔><A1><A2><A3><A4✔><A5✔>
#include <Wire.h>
#include "SparkFun_Qwiic_Joystick_Arduino_Library.h"
//If library not found
//Click here to get the library: http://librarymanager/All#SparkFun_joystick

//RGB LED
#define redPin 5
#define greenPin 6
#define bluePin 9

//Buzzer + POT
#define buzzerPin 12

//Ultrasonic
#define trigPin 7
#define echoPin 8
long duration;
int distanceCm, distanceInch;

//LDR + LEDs
#define ldrPin A0
#define ledPin 10

// Clockwise and counter-clockwise definitions.
// This relies on how motors are wired, check and may need to swap.
#define FORWARD  0
#define REVERSE 1

// Motor definitions to make life easier:
#define MOTOR_A 0
#define MOTOR_B 1

// Pin Assignments //
//Default pins:
#define DIRA 2 // Direction control for motor A
#define PWMA 3  // PWM control (speed) for motor A
#define DIRB 4 // Direction control for motor B
#define PWMB 11 // PWM control (speed) for motor B

////Alternate pins:
//#define DIRA 8 // Direction control for motor A
//#define PWMA 9 // PWM control (speed) for motor A
//#define DIRB 7 // Direction control for motor B
//#define PWMB 10 // PWM control (speed) for motor B

int MapXValue = 0;      // joystick readings scaled  -255 to 255
int MapYValue = 0;      // scaled  -255 to 255

byte motorSpeed = 0;

uint8_t Address = 0x20; //Start address (Default 0x20)

JOYSTICK joystick; //Create instance of this object


// setupArdumoto initialize all pins
void setupArdumoto()
{
  // All pins should be setup as outputs:
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(DIRA, OUTPUT);
  pinMode(DIRB, OUTPUT);

  // Initialize all pins as low:
  digitalWrite(PWMA, LOW);
  digitalWrite(PWMB, LOW);
  digitalWrite(DIRA, LOW);
  digitalWrite(DIRB, LOW);
}

void setup()
{
  setupArdumoto();
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(bluePin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(ldrPin, INPUT);
  digitalWrite(ledPin, 0);
  Serial.begin(9600);   //begin serial communication with the computer
  //warning: need to open serial monitor or it may wait here

  if (joystick.begin(Wire, Address) == false)
  {
    Serial.println("Joystick does not appear to be connected. Please check wiring. Freezing...");
    while (1);
  }
}

void loop()
{
  //read analog in pins from joystick Qwiic
  int Joystick_X = joystick.getHorizontal();
  int Joystick_Y = joystick.getVertical();
  //
  //Scale values from joystick to range for motor -255 and 255 --center is 0
  MapXValue = map (Joystick_X, 0, 1023, -255, 255);
  MapYValue = map (Joystick_Y, 0, 1023, -255, 255);
  //
  if (analogRead(ldrPin) < 80) {
    led_ON();
    buzzer();
    RGBflashing();
    distanceM();
    if (distanceCm < 25) {
      digitalWrite(DIRA, REVERSE);
      digitalWrite(DIRB, REVERSE);
      delay(2000);
    }
    else {
      Motor_A(MapXValue);//drive the right wheel speed and direction from X
      Motor_B(MapYValue); //drive the left wheel speed and direction from Y
    }
  }
  else {
    led_OFF();
    buzzer();
    RGBflashing();
    distanceM();
    if (distanceCm < 25) {
      digitalWrite(DIRA, REVERSE);
      digitalWrite(DIRB, REVERSE);
      delay(2000);
    }
    else {
      Motor_A(MapXValue);//drive the right wheel speed and direction from X
      Motor_B(MapYValue); //drive the left wheel speed and direction from Y
    }
  }
  /*
  //read analog in pins from joystick Qwiic
  int Joystick_X = joystick.getHorizontal();
  int Joystick_Y = joystick.getVertical();
  //
  //Scale values from joystick to range for motor -255 and 255 --center is 0
  MapXValue = map (Joystick_X, 0, 1023, -255, 255);
  MapYValue = map (Joystick_Y, 0, 1023, -255, 255);
  //
  distanceM(); //distanceCm
  buzzer();
  RGBflashing();
  led_ON();
  led_OFF();
  //Output to motor
  Motor_A(MapXValue);//drive the right wheel speed and direction from X
  Motor_B(MapYValue); //drive the left wheel speed and direction from Y
  //
    Serial.print("X: ");         //print to serial monitor the X-coordinates of joystick
    Serial.print(Joystick_X);
    Serial.print(" ");
    Serial.print(MapXValue);

    Serial.print(" Y: ");        //print to serial monitor theY-coordinates of joystick
    Serial.print(Joystick_Y);
    Serial.print(" ");
    Serial.println(MapYValue);
  */

}// end of loop
//functions:
void Motor_A(int motorSpeed)                          //function for driving Motor_A
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(DIRA, FORWARD);                      //set pin 2 to high or FORWARD
  }
  else if (motorSpeed < 0)                            //if the motor should drive backwar (negative speed)
  {
    digitalWrite(DIRA, REVERSE);                      //set pin 1 to low or REVERSE
  }
  else                                                //if the motor should stop
  {
    digitalWrite(DIRA, 0);
    analogWrite(PWMA, 0);
  }
  analogWrite(PWMA, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered Joystick speed
}

/********************************************************************************/
void Motor_B(int motorSpeed)                           //function for driving the Motor_B
{
  if (motorSpeed > 0)                                  //if the motor should drive forward (positive speed)
  {
    digitalWrite(DIRB, FORWARD);                       //set pin 1 to high
  }
  else if (motorSpeed < 0)                             //if the motor should drive backward (negative speed)
  {
    digitalWrite(DIRB, REVERSE);                       //set pin 1 to low
  }
  else                                                //STOP Motor_B
  {
    digitalWrite(DIRB, 0);                              // STOP motor_B
    analogWrite(PWMB, 0);
  }
  analogWrite(PWMB, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered Joystick speed

}

void RGBflashing() {
  setColor(255, 0, 0);  // red
  delay(50);
  setColor(0, 255, 0);  // green
  delay(50);
  setColor(0, 0, 255);  // blue
  delay(50);
  setColor(255, 255, 0);  // yellow
  delay(50);
  setColor(80, 0, 80);  // purple
  delay(50);
  setColor(0, 255, 255);  // aqua
  delay(50);
}

void setColor(int red, int green, int blue)
{
#ifdef COMMON_ANODE
  red = 255 - red;
  green = 255 - green;
  blue = 255 - blue;
#endif
  analogWrite(redPin, red);
  analogWrite(greenPin, green);
  analogWrite(bluePin, blue);
}

void buzzer() {
  digitalWrite(buzzerPin, 1);
  delay(50);
  digitalWrite(buzzerPin, 0);
  delay(50);
}

void distanceM() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distanceCm = duration * 0.0340 / 2;
}

void led_ON() {
  digitalWrite(ledPin, 1);
}
void led_OFF() {
  digitalWrite(ledPin, 0);
}