#include <Arduino.h>
#include <MuVisionSensor.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#define I2C_MODE
#define MU_ADDRESS    0x60


MuVisionSensor Mu(MU_ADDRESS);
#define RX 7
#define TX 8

SoftwareSerial ss(RX, TX);

int Sensor1 = A0; // connect ir sensor module to Arduino pin 7
int Sensor2 = 9; // connect ir sensor module to Arduino pin 9
int Sensor3 = 10; // connect ir sensor module to Arduino pin 10
int Sensor4 = 11; // connect ir sensor module to Arduino pin 11
int Move = 3; // connect ir sensor module to Arduino pin 11
int Speed1 = 4;
int Speed2 = 5;
int Stop =   6;
int  data1, data2, data3, data4;
void setup()
{
  Serial.begin(9600); // Init Serila at 115200 Baud
  ss.begin(9600);
  Serial.println("Serial Working"); // Test to check if serial is working or not
  pinMode(13, OUTPUT);
  digitalWrite(13, 0);
  pinMode(Sensor1, INPUT); // IR Sensor pin INPUT
  pinMode(Sensor2, INPUT); // IR Sensor pin INPUT
  pinMode(Sensor3, INPUT); // IR Sensor pin INPUT
  pinMode(Sensor4, INPUT); // IR Sensor pin INPUT
  pinMode(Move,    OUTPUT ); // IR Sensor pin INPUT
  pinMode(Speed1,    OUTPUT ); // IR Sensor pin INPUT
  pinMode(Speed2,    OUTPUT ); // IR Sensor pin INPUT
  pinMode(Stop,    OUTPUT ); // IR Sensor pin INPUT
  digitalWrite (Move , LOW);
  digitalWrite (Speed1 , LOW);
  digitalWrite (Speed2 , LOW);
  digitalWrite (Stop , LOW);
  /////////////////////////Cam
  uint8_t err = MU_ERROR_FAIL;
  Wire.begin();
  err = Mu.begin(&Wire);
  if (err == MU_OK) {
    Serial.println("MU initialized.");
  } else {
    do {
      Serial.println("fail to initialize MU! Please check protocol "
                     "version or make sure MU is working on the "
                     "correct port with correct mode.");
      delay(5000);
    } while (1);
  }
  // enable vision: number card
  Mu.VisionBegin(VISION_COLOR_RECOGNITION);
  Mu.write(VISION_COLOR_RECOGNITION, kXValue, 100);                  // set detect region center x value(0~100)
  Mu.write(VISION_COLOR_RECOGNITION, kYValue, 100);                  // set detect region center y value(0~100)
  Mu.write(VISION_COLOR_RECOGNITION, kWidthValue, 50);               // set detect region center width value(0~100)
  Mu.write(VISION_COLOR_RECOGNITION, kHeightValue, 50);              // set detect region center height value(0~100)
  Mu.CameraSetAwb(kLockWhiteBalance);

}

void loop()
{
  int sensorStatus1 = digitalRead(Sensor1);
  int sensorStatus2 = digitalRead(Sensor2);
  int sensorStatus3 = digitalRead(Sensor3);
  int sensorStatus4 = digitalRead(Sensor4);
  data1 = sensorStatus1;
  data2 = sensorStatus2;
  data3 = sensorStatus3;
  data4 = sensorStatus4;

  Serial.print(sensorStatus1);
  Serial.print(" ");
  Serial.print(sensorStatus2);
  Serial.print(" ");
  Serial.print(sensorStatus3);
  Serial.print(" ");
  Serial.println(sensorStatus4);
  sendTxData();

  if (sensorStatus1 == 0 || sensorStatus2 == 0 || sensorStatus3 == 0 || sensorStatus4 == 0 )
  {
    colorswitch();
    // if the pin is high turn off the onboard Led
    digitalWrite(Move, HIGH); // LED LOW
    Serial.println("Motion Start!"); // print Motion Detected! on the serial monitor window
  }
  else
  {
    //else turn on the onboard LED
    digitalWrite(Move, LOW); // LED High
    Serial.println("Motion Stop!"); // print Motion Ended! on the serial monitor window
  }


}


void colorswitch() {

  if (Mu.GetValue(VISION_COLOR_RECOGNITION, kStatus)) {                    // update vision result and get status, 0: undetected, other: detected
    switch (Mu.GetValue(VISION_COLOR_RECOGNITION, kLabel)) {               // get vision result: label value

      case MU_COLOR_GREEN:
        Serial.println("green");
        Serial.println("Speed [1] + Motion Start!");
        digitalWrite (Speed1 , HIGH);
        digitalWrite (Speed2 , LOW);
        digitalWrite (Stop , LOW);
        digitalWrite(13, 1);
        delay(500);
        break;
      case MU_COLOR_PURPLE:
        Serial.println("purple");
        Serial.println("Motion Stop!!!! [Color Detected]");
        digitalWrite (Speed1 , LOW);
        digitalWrite (Speed2 , LOW);
        digitalWrite (Stop , HIGH);
        delay(500);
        digitalWrite (Speed1 , LOW);
        digitalWrite (Speed2 , LOW);
        digitalWrite (Stop , LOW);
        digitalWrite(13, 0);
        delay(500);
        break;
      case MU_COLOR_YELLOW:
        Serial.println("yellow");
        Serial.println("Speed [2] + Motion Start!");
        digitalWrite (Speed1 , LOW);
        digitalWrite (Speed2 , HIGH);
        digitalWrite (Stop , LOW);
        digitalWrite(13, 0);
        delay(500);
        break;
      default:
        digitalWrite (Speed1 , LOW);
        digitalWrite (Speed2 , LOW);
        digitalWrite (Stop , LOW);
        //delay(500);
        break;
    }
  }
}

void sendTxData() {
  ss.print(data1); ss.print("A");
  ss.print(data2); ss.print("B");
  ss.print(data3); ss.print("C");
  ss.print(data4); ss.print("D");
  ss.print("\n");
  Serial.print(data1); Serial.print("A");
  Serial.print(data2); Serial.print("B");
  Serial.print(data3); Serial.print("C");
  Serial.print(data4); Serial.print("D");
  Serial.print("\n");
}