/* Measures the length (in microseconds) of a pulse on the pin; state is HIGH
 * or LOW, the type of pulse to measure.  Works on pulses from 2-3 microseconds
 * to 3 minutes in length, but must be called at least a few dozen microseconds
 * before the start of the pulse.
 *
 * ATTENTION:
 * this function relies on micros() so cannot be used in noInterrupt() context
 */
unsigned long pulseInLong(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 16 clock cycles per iteration.
	unsigned long numloops = 0;
	unsigned long maxloops = microsecondsToClockCycles(timeout);

	// wait for any previous pulse to end
	while ((*portInputRegister(port) & bit) == stateMask)
		if (numloops++ == maxloops)
			return 0;

	// wait for the pulse to start
	while ((*portInputRegister(port) & bit) != stateMask)
		if (numloops++ == maxloops)
			return 0;

	unsigned long start = micros();
	// wait for the pulse to stop
	while ((*portInputRegister(port) & bit) == stateMask) {
		if (numloops++ == maxloops)
			return 0;
	}
	return micros() - start;
}

unsigned long Ping::pulseIn(uint8_t pin, uint8_t state, unsigned long timeout, unsigned long timeoutstop)
{
	uint8_t bit = digitalPinToBitMask(pin);
	uint8_t port = digitalPinToPort(pin);
	uint8_t stateMask = (state ? bit : 0);
	unsigned long width = 0;

	unsigned long numloops = 0;
	unsigned long maxloops = microsecondsToClockCycles(timeout) / 16;
	unsigned long maxloopsstop = microsecondsToClockCycles(timeoutstop) / 16 ;

	// wait for any previous pulse to end
	while ((*portInputRegister(port) & bit) == stateMask)
		if (numloops++ == maxloops)
			return 0;

	// wait for the pulse to start
	while ((*portInputRegister(port) & bit) != stateMask)
		if (numloops++ == maxloops)
			return 0;

	// wait for the pulse to stop
	while ((*portInputRegister(port) & bit) == stateMask)
                if (width++ == maxloopsstop)
                        return 5000 ;

	return clockCyclesToMicroseconds(width * 20 + 16);
}

unsigned long pulseIn(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);
    unsigned long width = 0; // keep initialization out of time critical area

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

    // wait for any previous pulse to end
    while ((*portInputRegister(port) & bit) == stateMask)
        if (numloops++ == maxloops)
            return 0;
    // wait for the pulse to start
    while ((*portInputRegister(port) & bit) != stateMask)
        if (numloops++ == maxloops)
            return 0;

    // wait for the pulse to stop
    while ((*portInputRegister(port) & bit) == stateMask)
        width++;

    // convert the reading to microseconds. The loop has been determined
    // to be 10 clock cycles long and have about 16 clocks between the edge
    // and the start of the loop. There will be some error introduced by
    // the interrupt handlers.
    return clockCyclesToMicroseconds(width * 10 + 16);
}

/* Measures the length (in microseconds) of a pulse on the pin; state is HIGH
 * or LOW, the type of pulse to measure.  Works on pulses from 2-3 microseconds
 * to 3 minutes in length, but must be called at least a few dozen microseconds
 * before the start of the pulse.
 *
 * ATTENTION:
 * this function relies on micros() so cannot be used in noInterrupt() context
 */
unsigned long pulseInLong(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);

  unsigned long startMicros = micros();

  // wait for any previous pulse to end
  while ((*portInputRegister(port) & bit) == stateMask) {
    if (micros() - startMicros > timeout)
      return 0;
  }

  // wait for the pulse to start
  while ((*portInputRegister(port) & bit) != stateMask) {
    if (micros() - startMicros > timeout)
      return 0;
  }

  unsigned long start = micros();
  // wait for the pulse to stop
  while ((*portInputRegister(port) & bit) == stateMask) {
    if (micros() - startMicros > timeout)
      return 0;
  }
  return micros() - start;
}

//
// Constructor
//
// The pins are not activated until begin() is called.
//
SoftwareWire::SoftwareWire(uint8_t sdaPin, uint8_t sclPin, boolean pullups, boolean detectClockStretch)
{
  _sdaPin = sdaPin;
  _sclPin = sclPin;
  _pullups = pullups;
  _stretch = detectClockStretch;
  
  setClock( 100000UL);       // set default 100kHz
  
  // Set default timeout to 1000 ms. 
  // 1 second is very long, 10ms would be more appropriate.
  // However, the Arduino libraries use often a default timeout of 1 second.
  setTimeout( 1000L);        

  // Turn Arduino pin numbers into PORTx, DDRx, and PINx
  uint8_t port;

  port = digitalPinToPort(_sdaPin);
  _sdaBitMask  = digitalPinToBitMask(_sdaPin);
  _sdaPortReg  = portOutputRegister(port);
  _sdaDirReg   = portModeRegister(port);
  _sdaPinReg   = portInputRegister(port);      // PinReg is the input register, not the Arduino pin.

  port = digitalPinToPort(_sclPin);
  _sclBitMask  = digitalPinToBitMask(_sclPin);
  _sclPortReg  = portOutputRegister(port);
  _sclDirReg   = portModeRegister(port);
  _sclPinReg   = portInputRegister(port);
}

float Servotor32::ping(){
  //PB0 for Trigger (17)
  //PB7 for Echo (11)
  
  pinMode(17,OUTPUT);
  pinMode(11,INPUT);

  long duration; 
  float cm;
  digitalWrite(17, LOW); 
  delayMicroseconds(2); 
  digitalWrite(17, HIGH); 
  delayMicroseconds(5); 
  digitalWrite(17, LOW); 
  
//  duration = pulseIn(11, HIGH, 100000);
  
  uint8_t bit = digitalPinToBitMask(11);
  uint8_t port = digitalPinToPort(11);
  uint8_t stateMask = (HIGH ? bit : 0);
  
  unsigned long startCount = 0;
  unsigned long endCount = 0;
  unsigned long width = 0; // keep initialization out of time critical area
  
  // convert the timeout from microseconds to a number of times through
  // the initial loop; it takes 16 clock cycles per iteration.
  unsigned long numloops = 0;
  unsigned long maxloops = 500;
	
  // wait for any previous pulse to end
  while ((*portInputRegister(port) & bit) == stateMask)
    if (numloops++ == maxloops)
      return 0;
	
  // wait for the pulse to start
  while ((*portInputRegister(port) & bit) != stateMask)
    if (numloops++ == maxloops)
      return 0;
  
  startCount = micros_new();
  // wait for the pulse to stop
  while ((*portInputRegister(port) & bit) == stateMask) {
    if (numloops++ == maxloops)
      return 0;
    delayMicroseconds(10); //loop 'jams' without this
    if((micros_new() - startCount) > 58000 ){ // 58000 = 1000CM
      return 0;
      break;
    }
  } 
  duration = micros_new() - startCount; 
  //--------- end pulsein
  cm = (float)duration / 29.0 / 2.0; 
  return cm;
}

/* Measures the length (in microseconds) of a pulse on the pin; state is HIGH
 * or LOW, the type of pulse to measure.  Works on pulses from 2-3 microseconds
 * to 3 minutes in length, but must be called at least a few dozen microseconds
 * before the start of the pulse. */
unsigned long pulseIn(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);
	unsigned long width = 0; // keep initialization out of time critical area
	
	// convert the timeout from microseconds to a number of times through
	// the initial loop; it takes 16 clock cycles per iteration.
	unsigned long numloops = 0;
	unsigned long maxloops = microsecondsToClockCycles(timeout) / 16;
	
	// wait for any previous pulse to end
	while ((*portInputRegister(port) & bit) == stateMask)
		if (numloops++ == maxloops)
			return 0;
	
	// wait for the pulse to start
	while ((*portInputRegister(port) & bit) != stateMask)
		if (numloops++ == maxloops)
			return 0;
	
	// wait for the pulse to stop
	while ((*portInputRegister(port) & bit) == stateMask) {
		if (numloops++ == maxloops)
			return 0;
		width++;
	}

	// convert the reading to microseconds. There will be some error introduced by
	// the interrupt handlers.

	// Conversion constants are compiler-dependent, different compiler versions
	// have different levels of optimization.
#if __GNUC__==4 && __GNUC_MINOR__==3 && __GNUC_PATCHLEVEL__==2
	// avr-gcc 4.3.2
	return clockCyclesToMicroseconds(width * 21 + 16);
#elif __GNUC__==4 && __GNUC_MINOR__==8 && __GNUC_PATCHLEVEL__==1
	// avr-gcc 4.8.1
	return clockCyclesToMicroseconds(width * 24 + 16);
#elif __GNUC__<=4 && __GNUC_MINOR__<=3
	// avr-gcc <=4.3.x
	#warning "pulseIn() results may not be accurate"
	return clockCyclesToMicroseconds(width * 21 + 16);
#else
	// avr-gcc >4.3.x
	#warning "pulseIn() results may not be accurate"
	return clockCyclesToMicroseconds(width * 24 + 16);
#endif

}

/**
 * Initialize SCL/SDA pins and set the bus high.
 *
 * @param[in] sdaPin The software SDA pin number.
 *
 * @param[in] sclPin The software SCL pin number.
 */
void SoftI2cMaster::begin(uint8_t sclPin, uint8_t sdaPin) {
  uint8_t port;

  // Get bit mask and address of scl registers.
  _sclBit = digitalPinToBitMask(sclPin);
  port = digitalPinToPort(sclPin);
  _sclDDR = portModeRegister(port);
  volatile uint8_t* sclOutReg = portOutputRegister(port);

  // Get bit mask and address of sda registers.
  _sdaBit = digitalPinToBitMask(sdaPin);
  port = digitalPinToPort(sdaPin);
  _sdaDDR = portModeRegister(port);
  _sdaInReg = portInputRegister(port);
  volatile uint8_t* sdaOutReg = portOutputRegister(port);

  // Clear PORT bit for scl and sda.
  uint8_t s = SREG;
  noInterrupts();
  *sclOutReg &= ~_sclBit;
  *sdaOutReg &= ~_sdaBit;
  SREG = s;

  // Set scl and sda high.
  writeScl(HIGH);
  writeSda(HIGH);
}

// ---------------------------------------------------------------------------
// NewPing constructor
// ---------------------------------------------------------------------------
NewPing::NewPing(uint8_t trigger_pin, uint8_t echo_pin, unsigned int max_cm_distance) {
	_triggerBit = digitalPinToBitMask(trigger_pin); // Get the port register bitmask for the trigger pin.
	_echoBit = digitalPinToBitMask(echo_pin);       // Get the port register bitmask for the echo pin.

	_triggerOutput = portOutputRegister(digitalPinToPort(trigger_pin)); // Get the output port register for the trigger pin.
	_echoInput = portInputRegister(digitalPinToPort(echo_pin));         // Get the input port register for the echo pin.

	_triggerMode = (uint8_t *) portModeRegister(digitalPinToPort(trigger_pin)); // Get the port mode register for the trigger pin.

#if ROUNDING_ENABLED == false
	_maxEchoTime = min(max_cm_distance + 1, (unsigned int) MAX_SENSOR_DISTANCE + 1) * US_ROUNDTRIP_CM; // Calculate the maximum distance in uS (no rounding).
#else
	_maxEchoTime = min(max_cm_distance, (unsigned int) MAX_SENSOR_DISTANCE) * US_ROUNDTRIP_CM + (US_ROUNDTRIP_CM / 2); // Calculate the maximum distance in uS.
#endif

#if defined (__arm__) && defined (TEENSYDUINO)
	pinMode(echo_pin, INPUT);     // Set echo pin to input (on Teensy 3.x (ARM), pins default to disabled, at least one pinMode() is needed for GPIO mode).
	pinMode(trigger_pin, OUTPUT); // Set trigger pin to output (on Teensy 3.x (ARM), pins default to disabled, at least one pinMode() is needed for GPIO mode).
#endif

#if defined (ARDUINO_AVR_YUN)
	pinMode(echo_pin, INPUT);     // Set echo pin to input on the Arduino Yun, not sure why it doesn't default this way.
#endif

#if ONE_PIN_ENABLED != true
	*_triggerMode |= _triggerBit; // Set trigger pin to output.
#endif
}

NewPing::NewPing(uint8_t trigger_pin, uint8_t echo_pin, unsigned int max_cm_distance) {
#if DO_BITWISE == true
	_triggerBit = digitalPinToBitMask(trigger_pin); // Get the port register bitmask for the trigger pin.
	_echoBit = digitalPinToBitMask(echo_pin);       // Get the port register bitmask for the echo pin.

	_triggerOutput = portOutputRegister(digitalPinToPort(trigger_pin)); // Get the output port register for the trigger pin.
	_echoInput = portInputRegister(digitalPinToPort(echo_pin));         // Get the input port register for the echo pin.

	_triggerMode = (uint8_t *) portModeRegister(digitalPinToPort(trigger_pin)); // Get the port mode register for the trigger pin.
#else
	_triggerPin = trigger_pin;
	_echoPin = echo_pin;
#endif

	set_max_distance(max_cm_distance); // Call function to set the max sensor distance.

#if (defined (__arm__) && (defined (TEENSYDUINO) || defined(PARTICLE))) || DO_BITWISE != true
	pinMode(echo_pin, INPUT);     // Set echo pin to input (on Teensy 3.x (ARM), pins default to disabled, at least one pinMode() is needed for GPIO mode).
	pinMode(trigger_pin, OUTPUT); // Set trigger pin to output (on Teensy 3.x (ARM), pins default to disabled, at least one pinMode() is needed for GPIO mode).
#endif

#if defined (ARDUINO_AVR_YUN)
	pinMode(echo_pin, INPUT);     // Set echo pin to input for the Arduino Yun, not sure why it doesn't default this way.
#endif

#if ONE_PIN_ENABLED != true && DO_BITWISE == true
	*_triggerMode |= _triggerBit; // Set trigger pin to output.
#endif
}

int digitalRead(uint8_t pin)
{
	uint32_t bit = digitalPinToBitMask(pin);
	uint32_t port = digitalPinToPort(pin);


    if(pin & 0x8000)
    {
        //Analog Pins
        pin &= 0x7FFF;

        if(GpioDataRegs.AIODAT.all & (1UL << pin))
            return HIGH;
        else
            return LOW;

    }
    else
    {

    	if (port == NOT_A_PORT) return LOW;

        //Digital Pins

	    if (*portInputRegister(port) & bit) 
            return HIGH;
        else
	        return LOW;

    }
    return LOW;
}

/* 
 * Expect the signal line to be at the specified level for a period of time and
 * return a count of loop cycles spent at that level (this cycle count can be
 * used to compare the relative time of two pulses).  If more than 1 ms 
 * ellapses without the level changing then the call fails with a 0 response.
 * This is adapted from Arduino's pulseInLong function (which is only available
 * in the very latest IDE versions):
 *     https://github.com/arduino/Arduino/blob/master/hardware/arduino/avr/cores/arduino/wiring_pulse.c
 */
uint32_t DHT::expectPulse(bool level) {
    uint32_t count = 0;
    /* 
     * On AVR platforms use direct GPIO port access as it is much faster and 
     * better for catching pulses that are 10's of microseconds in length:
     */
    #ifdef __AVR
        uint8_t portState = level ? _bit : 0;
    
        while ((*portInputRegister(_port) & _bit) == portState) {
            if (count++ >= _maxCycles) {
                return (0); // Exceeded timeout, fail.
            }
        }
    /* 
     * Otherwise fall back to using digitalRead (this seems to be necessary on 
     * ESP8266 right now, perhaps bugs in direct port access functions?).
     */
    #else
        while (digitalRead(_pin) == level) {
            if (count++ >= _maxCycles) {
                return (0); // Exceeded timeout, fail.
            }
        }
    #endif

    return (count);
}

void PCATTACH::PCint(uint8_t port) {
  uint8_t bit;
  uint8_t curr;
  uint8_t mask;
  uint8_t pin;

  // get the pin states for the indicated port.
  curr = *portInputRegister(port+2);
  mask = curr ^ PCintLast[port];
  PCintLast[port] = curr;
  // mask is pins that have changed. screen out non pcint pins.
  if ((mask &= *port_to_pcmask[port]) == 0) {
    return;
  }
  // mask is pcint pins that have changed.
  for (uint8_t i=0; i < 8; i++) {
    bit = 0x01 << i;
    if (bit & mask) {
      pin = port * 8 + i;
      if (PCintFunc[pin] != NULL) {
        PCintFunc[pin]();
      }
    }
  }
}

// atsha204Class Constructor
// Feed this function the Arduino-ized pin number you want to assign to the ATSHA204's SDA pin
// This will find the DDRX, PORTX, and PINX registrs it'll need to point to to control that pin
// As well as the bit value for each of those registers
atsha204Class::atsha204Class(uint8_t pin)
{
    io_pin = pin;

    Serial.print("Pin: ");
    Serial.println(pin);

    pinBitmask = digitalPinToBitMask(pin);	// Find the bit value of the pin
    Serial.print("pinBitmask: ");
    Serial.println(pinBitmask);


    PORT_DATA_TYPE port = digitalPinToPort(pin);	// temoporarily used to get the next three registers

#ifdef IS_AVR
    // Point to data direction register port of pin
    device_port_DDR = portModeRegister(port);
#endif
    // Point to output register of pin
    device_port_OUT = portOutputRegister(port);
    // Point to input register of pin
    device_port_IN = portInputRegister(port);

    pinMode(pin, OUTPUT);
    digitalWrite(pin, HIGH);
}

CapSense::CapSense(uint8_t sendPin, uint8_t receivePin)
{
	uint8_t sPort, rPort;

	// initialize this instance's variables
    current_value = 0;
    sensor_mode = SENSOR_CHARGE;
	MaxTotal = 1024;
    
	// get pin mapping and port for send Pin - from PinMode function in core
	sBit =  digitalPinToBitMask(sendPin);			// get send pin's ports and bitmask
	sPort = digitalPinToPort(sendPin);
	sReg = portModeRegister(sPort);
	sOut = portOutputRegister(sPort);				// get pointer to output register   

	rBit = digitalPinToBitMask(receivePin);			// get receive pin's ports and bitmask 
	rPort = digitalPinToPort(receivePin);
	rReg = portModeRegister(rPort);
	rIn  = portInputRegister(rPort);
   	rOut = portOutputRegister(rPort);
	
	// get pin mapping and port for receive Pin - from digital pin functions in Wiring.c
    noInterrupts();
	*sReg |= sBit;              // set sendpin to OUTPUT 
    interrupts();
}

// common code for isr handler. "port" is the PCINT number.
// there isn't really a good way to back-map ports and masks to pins.
static void PCint(uint8_t port) {
  uint8_t bit;
  uint8_t curr;
  uint8_t mask;
  uint8_t pin;

  // get the pin states for the indicated port.
  curr = *portInputRegister(port+2);
  mask = curr ^ PCintLast[port];
  // mask is pins that have changed. screen out non pcint pins.
  if ((mask &= *PCintPortToInputMask[port]) == 0) {
    return;
  }
  // mask is pcint pins that have changed.
  for (uint8_t i=0; i < 8; i++) {
    bit = 0x01 << i;
    if (bit & mask) {
      pin = port * 8 + i;
      // Trigger interrupt if mode is CHANGE, or if mode is RISING and
      // the bit is currently high, or if mode is FALLING and bit is low.
      if ((PCintMode[pin] == CHANGE
          || ((PCintMode[pin] == RISING) && (curr & bit))
          || ((PCintMode[pin] == FALLING) && !(curr & bit)))
          && (PCintFunc[pin] != NULL)) {
        PCintFunc[pin]();
      }
    }
  }
  // Save current pin values
  PCintLast[port] = curr;
}

unsigned char GPIO::read(const unsigned char bit, const unsigned char port) {
	if (port == 0)
		return 0;
	if (*portInputRegister(port) & bit)
		return 1;
	return 0;
}

void I2CServerBranch::in() {
	char op=0b10;
  Wire.beginTransmission(serverId);
  Wire.write((hostPort<<2)|op);//codify operation on lower 2 bits
	int nbytes=Wire.requestFrom(serverId, 1);
  	*(portInputRegister(localPort))=Wire.read();
  Wire.endTransmission(serverId);
}

DHT22::DHT22(uint8_t pin)
{
    _bitmask = digitalPinToBitMask(pin);
    _baseReg = portInputRegister(digitalPinToPort(pin));
    _lastReadTime = millis();
    _lastHumidity = DHT22_ERROR_VALUE;
    _lastTemperature = DHT22_ERROR_VALUE;
}

/**
 * digitalRead
 *
 * read binary state
 *
 * @param pin pin mumber
 * 
 * @return input state
 */
uint8_t digitalRead(uint8_t pin)
{
	uint8_t bit = digitalPinToBitMask(pin);
	uint8_t port = digitalPinToPort(pin);
	volatile uint8_t *input = portInputRegister(port);

  return (((*input) & bit) > 0);
}