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

	if (port == NOT_A_PORT) return LOW;

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

void MeRGBLed::reset(uint8_t port){
        _port = port;
	s2 = mePort[port].s2;
	s1 = mePort[port].s1;
	pinMask = digitalPinToBitMask(s2);
	ws2812_port = portOutputRegister(digitalPinToPort(s2));
	ws2812_port_reg = portModeRegister(digitalPinToPort(s2));
        //*ws2812_port_reg |= pinMask;
        pinMode(s2,OUTPUT);
}

void TclClass::begin() {
#ifdef TCL_SPI
  // Set the SPI parameters
  SPI.begin();
  SPI.setBitOrder(MSBFIRST);
  SPI.setDataMode(SPI_MODE0);
  SPI.setClockDivider(SPI_CLOCK_DIV2);
#endif
#ifdef TCL_DIO
  pinMode(TCL_CLOCKPIN, OUTPUT);
  pinMode(TCL_DATAPIN, OUTPUT);
  clkport     = portOutputRegister(digitalPinToPort(TCL_CLOCKPIN));
  clkpinmask  = digitalPinToBitMask(TCL_CLOCKPIN);
  dataport    = portOutputRegister(digitalPinToPort(TCL_DATAPIN));
  datapinmask = digitalPinToBitMask(TCL_DATAPIN);
  *clkport   &= ~clkpinmask;
  *dataport  &= ~datapinmask;
#endif
}

void PCintPort::detachInterrupt(uint8_t pin)
{
	uint8_t portNum = digitalPinToPort(pin);
	if (portNum == NOT_A_PORT) {
		//Serial.println("NOT_A_PORT det");
		return;
	} 
	PCintPort& port = PCintPort::pcIntPorts[portNum - 2];
	port.delPin(digitalPinToBitMask(pin));
}

bool gpio_pin_read(const uint8_t pin_no)
{
	const uint8_t port = digitalPinToPort(pin_no);
	if (!port) return false;

	const uint8_t mask = digitalPinToBitMask(pin_no);
	volatile uint8_t *ptr_in = portInputRegister(port);

	return (*ptr_in & mask)!=0;
}

DHT::DHT(uint8_t pin, uint8_t type, uint8_t count) {
    _pin = pin;
    _type = type;
    _bit = digitalPinToBitMask(pin);
    _port = digitalPinToPort(pin);
    _maxcycles = microsecondsToClockCycles(1000);  // 1 millisecond timeout for
    // reading pulses from DHT sensor.
    // Note that count is now ignored as the DHT reading algorithm adjusts itself
    // basd on the speed of the processor.
}

Adafruit_VS1053::Adafruit_VS1053(int8_t mosi, int8_t miso, int8_t clk,
                                 int8_t rst, int8_t cs, int8_t dcs, int8_t dreq) {
    _mosi = mosi;
    _miso = miso;
    _clk = clk;
    _reset = rst;
    _cs = cs;
    _dcs = dcs;
    _dreq = dreq;

    useHardwareSPI = false;

    clkportreg = portOutputRegister(digitalPinToPort(_clk));
    clkpin = digitalPinToBitMask(_clk);
    misoportreg = portInputRegister(digitalPinToPort(_miso));
    misopin = digitalPinToBitMask(_miso);
    mosiportreg = portOutputRegister(digitalPinToPort(_mosi));
    mosipin = digitalPinToBitMask(_mosi);
}

//************************************************************************
void tone(uint8_t _pin, unsigned int frequency, unsigned long duration)
{
// uint32_t tonePeriod;
uint8_t port;

	// Should have an error check here for pin number out of range.
	//*	there is no standard on the number of pins. Since we want this to work on all versions of the PIC32
	//*	I have set it to 112 for now which is the largest I/O pin count on a pic32
	if ((frequency > 0) && (_pin < (NUM_DIGITAL_PINS)))
	{
			
		// If a tone is currently playing on a different pin, the function is
		// documented to have no effect. If playing on the same pin, change
		// the frequency. If not currently playing, initialize the timer.
		// This is currently hard coded to use timer1.
		if (tone_pin == 255)
		{
                    // No tone currently playing. Init the timer.
                    T1CON = TACON_PS_256;
                    setIntVector(_TIMER_1_VECTOR, Timer1Handler);
                    clearIntFlag(_TIMER_1_IRQ);
                    setIntPriority(_TIMER_1_VECTOR, _T1_IPL_IPC, _T1_SPL_IPC);
                    setIntEnable(_TIMER_1_IRQ);
		}
		else if (_pin != tone_pin)
		{
			// Tone currently playing on another pin. ignore this call.
			return;
		}

		// Determine which port and bit are requested.
		tone_pin		=	_pin; 
		port			=	digitalPinToPort(_pin);
		tone_pin_port	=	(p32_ioport *)portRegisters(port);
		tone_pin_mask	=	digitalPinToBitMask(_pin);

		// Ensure that the pin is a digital output
		pinMode(_pin, OUTPUT);

		// Duration 0 means to play forever until stopped. Other values
		// mean to play for that many milliseconds.
		if (duration > 0)
		{
			timer1_toggle_count	=	(2 * frequency * duration) / 1000;
		}
		else
		{
			timer1_toggle_count	=	-1;
		}

		TMR1		= 0;
		PR1			= ((__PIC32_pbClk / 256) / 2 / frequency);
		T1CONSET	= TACON_ON;
	}
}

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(10);
  digitalWrite(17, LOW);


  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;
}

void IRLwrite(const uint8_t pin, uint16_t address, uint32_t command)
{
	// get the port mask and the pointers to the out/mode registers for faster access
	uint8_t bitMask = digitalPinToBitMask(pin);
	uint8_t port = digitalPinToPort(pin);
	volatile uint8_t * outPort = portOutputRegister(port);
	volatile uint8_t * modePort = portModeRegister(port);

	// set pin to OUTPUT and LOW
	*modePort |= bitMask;
	*outPort &= ~bitMask;

	// disable interrupts
	uint8_t oldSREG = SREG;
	cli();

	switch (irType) {

	case IR_NEC:
		// NEC only sends the data once
		if (command == 0xFFF)
			// send holding indicator
			IRLsend<0, 0, NEC_HZ, 0, NEC_MARK_LEAD, NEC_SPACE_HOLDING,
			        0, 0, 0, 0>
			        (outPort, bitMask, address, command);
		else
			// send data
			IRLsend<NEC_ADDRESS_LENGTH, NEC_COMMAND_LENGTH, NEC_HZ, IR_ADDRESS_FIRST, NEC_MARK_LEAD, NEC_SPACE_LEAD,
			        NEC_MARK_ZERO, NEC_MARK_ONE, NEC_SPACE_ZERO, NEC_SPACE_ONE>
			        (outPort, bitMask, address, command);
		break;

	case IR_PANASONIC: //TODO test
		// send data
		IRLsend<PANASONIC_ADDRESS_LENGTH, PANASONIC_COMMAND_LENGTH, PANASONIC_HZ, IR_ADDRESS_FIRST, PANASONIC_MARK_LEAD, PANASONIC_SPACE_LEAD,
		        PANASONIC_MARK_ZERO, PANASONIC_MARK_ONE, PANASONIC_SPACE_ZERO, PANASONIC_SPACE_ONE>
		        (outPort, bitMask, address, command);
		break;

	case IR_SONY12: //TODO test, address -1?
		// repeat 3 times
		for (uint8_t i = 0; i < 3; i++)
			// send data
			IRLsend<SONY_ADDRESS_LENGTH_12, SONY_COMMAND_LENGTH_12, SONY_HZ, IR_COMMAND_FIRST, SONY_MARK_LEAD, SONY_SPACE_LEAD,
			        SONY_MARK_ZERO, SONY_MARK_ONE, SONY_SPACE_ZERO, SONY_SPACE_ONE>
			        (outPort, bitMask, address, command);
		break;
	}

	// enable interrupts
	SREG = oldSREG;

	// set pin to INPUT again to be save
	*modePort &= ~bitMask;
}

// Initialization code common to both 'B' and 'R' type displays
void Adafruit_ST7735::commonInit(const uint8_t *cmdList) {

  constructor(ST7735_TFTWIDTH, ST7735_TFTHEIGHT);
  colstart  = rowstart = 0; // May be overridden in init func

  pinMode(_rs, OUTPUT);
  pinMode(_cs, OUTPUT);
  csport    = portOutputRegister(digitalPinToPort(_cs));
  cspinmask = digitalPinToBitMask(_cs);
  rsport    = portOutputRegister(digitalPinToPort(_rs));
  rspinmask = digitalPinToBitMask(_rs);

  if(hwSPI) { // Using hardware SPI
    SPI.begin();
    SPI.setClockDivider(SPI_CLOCK_DIV4); // 4 MHz (half speed)
    SPI.setBitOrder(MSBFIRST);
    SPI.setDataMode(SPI_MODE0);
  } else {
    pinMode(_sclk, OUTPUT);
    pinMode(_sid , OUTPUT);
    clkport     = portOutputRegister(digitalPinToPort(_sclk));
    clkpinmask  = digitalPinToBitMask(_sclk);
    dataport    = portOutputRegister(digitalPinToPort(_sid));
    datapinmask = digitalPinToBitMask(_sid);
    *clkport   &= ~clkpinmask;
    *dataport  &= ~datapinmask;
  }

  // toggle RST low to reset; CS low so it'll listen to us
  *csport &= ~cspinmask;
  if (_rst) {
    pinMode(_rst, OUTPUT);
    digitalWrite(_rst, HIGH);
    delay(500);
    digitalWrite(_rst, LOW);
    delay(500);
    digitalWrite(_rst, HIGH);
    delay(500);
  }

  if(cmdList) commandList(cmdList);
}

void SoftSPIClass::begin(byte SCK_, byte MOSI_, byte MISO_, byte SS_){
	_SS = SS_;
	_SCK = SCK_;
	_MISO = MISO_;
	_MOSI = MOSI_;
	
	byte MOSIport = digitalPinToPort(_MOSI);
	byte SSport = digitalPinToPort(_SS);
	byte SCKport = digitalPinToPort(_SCK);
	byte MISOport = digitalPinToPort(_MISO);
		
	if ((MOSIport == NOT_A_PIN) ||
		(  SSport == NOT_A_PIN) ||
		( SCKport == NOT_A_PIN) ||
		(MISOport == NOT_A_PIN) ){
		end();
	} else {
		_running = true;
		pinMode(_MOSI, OUTPUT);
		pinMode(_MISO, INPUT);
		pinMode(_SCK, OUTPUT);
		pinMode(_SS, OUTPUT);
		_MOSI_PORT = portOutputRegister(MOSIport);
		_MOSI_HIGH = digitalPinToBitMask(_MOSI);
		_MOSI_LOW = ~_MOSI_HIGH;
		_SCK_PORT = portOutputRegister(SCKport);
		_SCK_HIGH = digitalPinToBitMask(_SCK);
		_SCK_LOW = ~_SCK_HIGH;
		_SS_PORT = portOutputRegister(SSport);
		_SS_HIGH = digitalPinToBitMask(_SS);
		_SS_LOW = ~_SS_HIGH;
		_MISO_PIN = portInputRegister(MISOport);
		_MISO_MASK = digitalPinToBitMask(_MISO);
		*_SS_PORT |= _SS_HIGH;
		*_SCK_PORT &= _SCK_LOW;
		*_MOSI_PORT &= _MOSI_LOW;
		
		//Default to Mode0.
		_mode = SPI_MODE0;
		transferType = &SoftSPIClass::transferMode0;
	}
}

unsigned int readCapacitivePin(int pinToMeasure) {
// Variables used to translate from Arduino to AVR pin naming
volatile uint8_t* port;
volatile uint8_t* ddr;
volatile uint8_t* pin;
// Here we translate the input pin number from
// Arduino pin number to the AVR PORT, PIN, DDR,
// and which bit of those registers we care about.
byte bitmask;
port = portOutputRegister(digitalPinToPort(pinToMeasure));
ddr = portModeRegister(digitalPinToPort(pinToMeasure));
bitmask = digitalPinToBitMask(pinToMeasure);
pin = portInputRegister(digitalPinToPort(pinToMeasure));
// Discharge the pin first by setting it low and output
*port &= ~(bitmask);
*ddr |= bitmask;
delay(1);
// Make the pin an input with the internal pull-up on
*ddr &= ~(bitmask);
*port |= bitmask;
 
// Now see how long the pin to get pulled up. This manual unrolling of the loop
// decreases the number of hardware cycles between each read of the pin,
// thus increasing sensitivity.
uint8_t cycles = 17;
if (*pin & bitmask) { cycles = 0;}
else if (*pin & bitmask) { cycles = 1;}
else if (*pin & bitmask) { cycles = 2;}
else if (*pin & bitmask) { cycles = 3;}
else if (*pin & bitmask) { cycles = 4;}
else if (*pin & bitmask) { cycles = 5;}
else if (*pin & bitmask) { cycles = 6;}
else if (*pin & bitmask) { cycles = 7;}
else if (*pin & bitmask) { cycles = 8;}
else if (*pin & bitmask) { cycles = 9;}
else if (*pin & bitmask) { cycles = 10;}
else if (*pin & bitmask) { cycles = 11;}
else if (*pin & bitmask) { cycles = 12;}
else if (*pin & bitmask) { cycles = 13;}
else if (*pin & bitmask) { cycles = 14;}
else if (*pin & bitmask) { cycles = 15;}
else if (*pin & bitmask) { cycles = 16;}
 
// Discharge the pin again by setting it low and output
// It's important to leave the pins low if you want to 
// be able to touch more than 1 sensor at a time - if
// the sensor is left pulled high, when you touch
// two sensors, your body will transfer the charge between
// sensors.
*port &= ~(bitmask);
*ddr |= bitmask;
 
return cycles;
}

Adafruit_SSD1351::Adafruit_SSD1351(uint8_t cs, uint8_t rs, uint8_t sid, uint8_t sclk, uint8_t rst) : Adafruit_GFX(SSD1351WIDTH, SSD1351HEIGHT) {
    _cs = cs;
    _rs = rs;
    _sid = sid;
    _sclk = sclk;
    _rst = rst;

    csport      = portOutputRegister(digitalPinToPort(cs));
    cspinmask   = digitalPinToBitMask(cs);

    rsport      = portOutputRegister(digitalPinToPort(rs));
    rspinmask   = digitalPinToBitMask(rs);

    sidport      = portOutputRegister(digitalPinToPort(sid));
    sidpinmask   = digitalPinToBitMask(sid);

    sclkport      = portOutputRegister(digitalPinToPort(sclk));
    sclkpinmask   = digitalPinToBitMask(sclk);

}

FORCE_INLINE uint8_t enqueue_set_output_switch_state_command(const uint8_t *queue_command, uint8_t queue_command_length)
{
  const uint8_t number_of_switches = queue_command_length/3;

  if ((number_of_switches * 3) != queue_command_length)
    return generate_enqueue_insufficient_bytes_error(3*(number_of_switches+1), queue_command_length);
  
  const uint8_t length = sizeof(SetOutputSwitchStateQueueCommand) + 
                            ((number_of_switches - 1) * sizeof(DeviceBitState));
  uint8_t *insertion_point = CommandQueue::GetCommandInsertionPoint(length);
  
  if (insertion_point == 0)
    return ENQUEUE_ERROR_QUEUE_FULL;
  
  SetOutputSwitchStateQueueCommand *cmd = (SetOutputSwitchStateQueueCommand *)insertion_point;
    
  cmd->command_type = QUEUE_COMMAND_STRUCTS_TYPE_SET_OUTPUT_SWITCH_STATE;
  cmd->num_bits = number_of_switches;
  
  uint8_t device_number, device_state;

  for (uint8_t i = 0; i < queue_command_length; i+=3)
  {
    device_number = queue_command[i+1];
    device_state = queue_command[i+2];
    
    switch(queue_command[i])
    {
    case PM_DEVICE_TYPE_SWITCH_OUTPUT:
    {
      if (!Device_OutputSwitch::IsInUse(device_number))
        return PARAM_APP_ERROR_TYPE_INVALID_DEVICE_NUMBER;

      // assume that Device_OutputSwitch::SetPin checks that pin maps to valid port
      const uint8_t pin = Device_OutputSwitch::GetPin(device_number);
      
      DeviceBitState *output_state = (DeviceBitState *)cmd->output_bit_info + i;
      
        // could further optimize this by including the actual address of the output/mode register
        // but this is sufficient for now
      output_state->device_number = device_number;
      output_state->device_reg = (uint8_t *)portOutputRegister(digitalPinToPort(pin));
      output_state->device_bit = digitalPinToBitMask(pin);
      output_state->device_state = device_state;
      break;
    }    
    default:
      return PARAM_APP_ERROR_TYPE_INVALID_DEVICE_TYPE;
    }
  }  
  
  CommandQueue::EnqueueCommand(length);
  return ENQUEUE_SUCCESS;
}

/*
 * attach an interrupt to a specific pin using pin change interrupts.
 */
void PCintPort::attachInterrupt(uint8_t pin, PCIntvoidFuncPtr userFunc, int mode)
{
	uint8_t portNum = digitalPinToPort(pin);
	if ((portNum == NOT_A_PORT) || (userFunc == NULL)) {
		return;
	}
	// map pin to PCIR register
	uint8_t portIndex = portNum - 2;
	PCintPort& port = PCintPort::pcIntPorts[portIndex];
	port.addPin(mode,digitalPinToBitMask(pin),userFunc);
}

/**
 * digitalWrite
 *
 * set binary state
 *
 * @param pin pin mumber
 * @param state output state
 */
void digitalWrite(uint8_t pin, uint8_t state)
{
	uint8_t bit = digitalPinToBitMask(pin);
	uint8_t port = digitalPinToPort(pin);
	volatile uint8_t *out = portOutputRegister(port);
   
  if (state)
    *out |= bit;
  else
    *out &= ~bit;
}

void Hackscribble_Ferro::_initialiseChipSelect()
{	
	uint8_t timer = digitalPinToTimer(_chipSelect);
	_bit = digitalPinToBitMask(_chipSelect);
	uint8_t port = digitalPinToPort(_chipSelect);
	if (port == NOT_A_PIN) return;
	// If the pin that support PWM output, we need to turn it off
	// before doing a digital write.
	if (timer != NOT_ON_TIMER) turnOffPWM(timer);
	_out = portOutputRegister(port);
}

/* 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

}

IRRemote::IRRemote(uint8_t pin) {
  _irpin = pin;
  _irpinport = digitalPinToPort(_irpin);
  _irpinmask = digitalPinToBitMask(_irpin);
  

  
  timedout = false;
  timing = false;
  maxwaiting = 0;
}