#include "SR04.h"
#include "wiring_private.h"
//TODO ^^ 
#define TRIG_PIN 12
#define ECHO_PIN 11
#define F_CPU     16000000L
#define portModeRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_mode_PGM + (P))) )
#define digitalPinToPort(P) ( pgm_read_byte( digital_pin_to_port_PGM + (P) ) )
#define digitalPinToBitMask(P) ( pgm_read_byte( digital_pin_to_bit_mask_PGM + (P) ) )
#define microsecondsToClockCycles(a) ( (a) * clockCyclesPerMicrosecond() )
#define clockCyclesPerMicrosecond() ( F_CPU / 1000000L )
#define clockCyclesToMicroseconds(a) ( (a) / clockCyclesPerMicrosecond() )
SR04 sr04 = SR04(ECHO_PIN,TRIG_PIN);
long a;
long b;


void digitalWriteM(uint8_t pin, uint8_t val);

void delayMicrosecondsM(unsigned int us)
{
	// for the 16 MHz clock on most Arduino boards

	// for a one-microsecond delay, simply return.  the overhead
	// of the function call takes 14 (16) cycles, which is 1us
	if (us <= 1) return; //  = 3 cycles, (4 when true)

	// the following loop takes 1/4 of a microsecond (4 cycles)
	// per iteration, so execute it four times for each microsecond of
	// delay requested.
	us <<= 2; // x4 us, = 4 cycles

	// account for the time taken in the preceding commands.
	// we just burned 19 (21) cycles above, remove 5, (5*4=20)
	// us is at least 8 so we can subtract 5
	us -= 5; // = 2 cycles,

        	// busy wait
	__asm__ __volatile__ (
		"1: sbiw %0,1" "\n\t" // 2 cycles
		"brne 1b" : "=w" (us) : "0" (us) // 2 cycles
	);
	// return = 4 cycles
}
void pinModeM2(uint8_t pin, uint8_t mode)
{
	uint8_t bit = digitalPinToBitMask(pin);
	uint8_t port = digitalPinToPort(pin);
	volatile uint8_t *reg, *out;

	if (port == NOT_A_PIN) return;

	// JWS: can I let the optimizer do this?
	reg = portModeRegister(port);
	out = portOutputRegister(port);

	if (mode == INPUT) { 
		uint8_t oldSREG = SREG;
                cli();
		*reg &= ~bit;
		*out &= ~bit;
		SREG = oldSREG;
	} else if (mode == INPUT_PULLUP) {
		uint8_t oldSREG = SREG;
                cli();
		*reg &= ~bit;
		*out |= bit;
		SREG = oldSREG;
	} else {
		uint8_t oldSREG = SREG;
                cli();
		*reg |= bit;
		SREG = oldSREG;
	}
}


void delayM(unsigned long ms)
{
	uint32_t start = micros();

	while (ms > 0) {
		yield();
		while ( ms > 0 && (micros() - start) >= 1000) {
			ms--;
			start += 1000;
		}
	}
}

unsigned long pulseInM(uint8_t pin, uint8_t state, unsigned long timeout)
{
	// cache the port and bit of the pin in order to speed up the
	// pulse width measuring loop and achieve finer resolution.  calling
	// digitalRead() instead yields much coarser resolution.
	uint8_t bit = digitalPinToBitMask(pin);
	uint8_t port = digitalPinToPort(pin);
	uint8_t stateMask = (state ? bit : 0);

	// convert the timeout from microseconds to a number of times through
	// the initial loop; it takes approximately 16 clock cycles per iteration
	unsigned long maxloops = microsecondsToClockCycles(timeout)/16;

	unsigned long width = countPulseASM(portInputRegister(port), bit, stateMask, maxloops);

	// prevent clockCyclesToMicroseconds to return bogus values if countPulseASM timed out
	if (width)
		return clockCyclesToMicroseconds(width * 16 + 16);
	else
		return 0;
}

void digitalWriteM(uint8_t pin, uint8_t val)
{
	uint8_t bit = digitalPinToBitMask(pin);
	uint8_t port = digitalPinToPort(pin);
	volatile uint8_t *out;

	out = portOutputRegister(port);

	uint8_t oldSREG = SREG;
	cli();

	if (val == LOW) {
		*out &= ~bit;
	} else {
		*out |= bit;
	}

	SREG = oldSREG;
}

void setup() {
   Serial.begin(9600);
   delay(1000);
   pinModeM2(2,OUTPUT); 
   pinModeM2(3,OUTPUT);
   pinModeM2(4,OUTPUT);
   pinModeM2(5,OUTPUT);
   pinModeM2(6,OUTPUT);
   pinModeM2(7,OUTPUT);
   pinModeM2(8,OUTPUT);
   pinModeM2(9,OUTPUT);
}
void loop() {
   a=sr04.Distance();
   Serial.print(a);
   Serial.println("cm");
   delay(1000);
   long b;
    b=a;
    if(b>255){
      b=255;
    }

   bool I;
   bool II;
   bool III;
   bool IV;
   bool V;
   bool VI;
   bool VII;
   bool VIII;
   
  I=b%2;

  b=b/2;
  II=b%2;
  b=b/2;
  III=b%2;
  b=b/2;
  IV=b%2;
  b=b/2;
  V=b%2;
  b=b/2;
  VI=b%2;
  b=b/2;
  VII=b%2;
  b=b/2;
  VIII=b%2;
  Serial.print(I);
  Serial.print(II);
  Serial.print(III);
  Serial.print(IV);
  Serial.print(V);
  Serial.print(VI);
  Serial.print(VII);
  Serial.println(VIII);
  digitalWriteM(2,I);
  digitalWriteM(3,II);
  digitalWriteM(4,III);
  digitalWriteM(5,IV);
  digitalWriteM(6,V);
  digitalWriteM(7,VI);
  digitalWriteM(8,VII);
  digitalWriteM(9,VIII);
}