#include <SoftwareSerial.h>

#define SRF_TXRX        0x05
#define SRF_ADDRESS     0x01
#define GETRANGE        0x54

SoftwareSerial srf01 = SoftwareSerial(SRF_TXRX, SRF_TXRX);
SoftwareSerial srf02 = SoftwareSerial(0x04, 0x04);

#include <Adafruit_NeoPixel.h>
#ifdef __AVR__
#include <avr/power.h>
#endif

#define PIN 6

// Parameter 1 = number of pixels in strip
// Parameter 2 = Arduino pin number (most are valid)
// Parameter 3 = pixel type flags, add together as needed:
//   NEO_KHZ800  800 KHz bitstream (most NeoPixel products w/WS2812 LEDs)
//   NEO_KHZ400  400 KHz (classic 'v1' (not v2) FLORA pixels, WS2811 drivers)
//   NEO_GRB     Pixels are wired for GRB bitstream (most NeoPixel products)
//   NEO_RGB     Pixels are wired for RGB bitstream (v1 FLORA pixels, not v2)
//   NEO_RGBW    Pixels are wired for RGBW bitstream (NeoPixel RGBW products)
Adafruit_NeoPixel strip = Adafruit_NeoPixel(120, PIN, NEO_GRB + NEO_KHZ800);

void setup() {
  srf01.begin(9600);
  srf02.begin(9600);
  Serial.begin(9600);
  strip.begin();
  strip.show();
  delay(200);
  Serial.println("SRF01 fgdgfsfsdfds");
}

void loop() {
  int range = getRange1();
  int range2 = getRange2();
  turnOff();
  if ( range > 0 && range < 100 ) {
    for ( int i = 0; i < (strip.numPixels()/3); i++) {
      strip.setPixelColor(i, strip.Color(50, 0, 0));
    }
  }
  if ( range2 > 0 && range2 < 100) {
    for ( int i = strip.numPixels()/3*2; i < strip.numPixels(); i++) {
      strip.setPixelColor(i, strip.Color(50, 0, 0));
    }
  }
  if ( range > 0 && range < 100 && range2 > 0 && range2 < 100 ) {
    for ( int i = strip.numPixels()/3; i < strip.numPixels()/3*2; i++) {
      strip.setPixelColor(i, strip.Color(50, 0, 0));
    }
  }
  strip.show();
}

int getRange1() {
  srf01.listen();
  byte hByte, lByte;
  SRF01_Cmd(SRF_ADDRESS, GETRANGE);                       // Get the SRF01 to perform a ranging and send the data back to the arduino
  while (srf01.available() < 2);
  hByte = srf01.read();                                   // Get high byte
  lByte = srf01.read();                                   // Get low byte
  int range = ((hByte << 8) + lByte);                     // Put them together
  Serial.print("range1\t");
  Serial.println(range);
  return range;
}
int getRange2() {
  srf02.listen();
  byte hByte, lByte;
  SRF02_Cmd(SRF_ADDRESS, GETRANGE);                       // Get the SRF01 to perform a ranging and send the data back to the arduino
  while (srf02.available() < 2);
  hByte = srf02.read();                                   // Get high byte
  lByte = srf02.read();                                   // Get low byte
  int range = ((hByte << 8) + lByte);                     // Put them together
  Serial.print("range2\t");
  Serial.println(range);
  return range;
}

void SRF01_Cmd(byte Address, byte cmd) {              // Function to send commands to the SRF01
  pinMode(SRF_TXRX, OUTPUT);
  digitalWrite(SRF_TXRX, LOW);                        // Send a 2ms break to begin communications with the SRF01
  delay(2);
  digitalWrite(SRF_TXRX, HIGH);
  delay(1);
  srf01.write(Address);                               // Send the address of the SRF01
  srf01.write(cmd);                                   // Send commnd byte to SRF01
  pinMode(SRF_TXRX, INPUT);
  int availbleJunk = srf01.available();               // As RX and TX are the same pin it will have recieved the data we just sent out, as we dont want this we read it back and ignore it as junk before waiting for useful data to arrive
  for (int x = 0;  x < availbleJunk; x++) byte junk = srf01.read();
}
void SRF02_Cmd(byte Address, byte cmd) {              // Function to send commands to the SRF01
  pinMode(0x04, OUTPUT);
  digitalWrite(0x04, LOW);                        // Send a 2ms break to begin communications with the SRF01
  delay(2);
  digitalWrite(0x04, HIGH);
  delay(1);
  srf02.write(Address);                               // Send the address of the SRF01
  srf02.write(cmd);                                   // Send commnd byte to SRF01
  pinMode(0x04, INPUT);
  int availbleJunk = srf02.available();               // As RX and TX are the same pin it will have recieved the data we just sent out, as we dont want this we read it back and ignore it as junk before waiting for useful data to arrive
  for (int x = 0;  x < availbleJunk; x++) byte junk = srf02.read();
}

void turnOff()
{
  for (uint16_t i = 0; i < strip.numPixels(); i++)
  {
    strip.setPixelColor(i, strip.Color(0, 0, 0));
  }
}