TFTLCD::TFTLCD(uint8_t cs, uint8_t cd, uint8_t wr, uint8_t rd, uint8_t reset) {
  _cs = cs;
  _cd = cd;
  _wr = wr;
  _rd = rd;
  _reset = reset;
  
  rotation = 0;
  _width = TFTWIDTH;
  _height = TFTHEIGHT;

  // disable the LCD
  digitalWrite(_cs, HIGH);
  pinMode(_cs, OUTPUT);  
  
  digitalWrite(_cd, HIGH);
  pinMode(_cd, OUTPUT);  
  
  digitalWrite(_wr, HIGH);
  pinMode(_wr, OUTPUT);  
  
  digitalWrite(_rd, HIGH);
  pinMode(_rd, OUTPUT);  

  digitalWrite(_reset, HIGH); 
  pinMode(_reset, OUTPUT); 

  csport = digitalPinToPort(_cs);
  cdport = digitalPinToPort(_cd);
  wrport = digitalPinToPort(_wr);
  rdport = digitalPinToPort(_rd);

  cspin = digitalPinToBitMask(_cs);
  cdpin = digitalPinToBitMask(_cd);
  wrpin = digitalPinToBitMask(_wr);
  rdpin = digitalPinToBitMask(_rd);

  cursor_y = cursor_x = 0;
  textsize = 1;
  textcolor = 0xFFFF;
}

// Constructor for breakout board (configurable LCD control lines).
// Can still use this w/shield, but parameters are ignored.
Adafruit_TFTLCD::Adafruit_TFTLCD(
  uint8_t cs, uint8_t cd, uint8_t wr, uint8_t rd, uint8_t reset) :
  Adafruit_GFX(TFTWIDTH, TFTHEIGHT) {

#ifndef USE_ADAFRUIT_SHIELD_PINOUT
  // Convert pin numbers to registers and bitmasks
  _reset     = reset;
  #ifdef __AVR__
    csPort     = portOutputRegister(digitalPinToPort(cs));
    cdPort     = portOutputRegister(digitalPinToPort(cd));
    wrPort     = portOutputRegister(digitalPinToPort(wr));
    rdPort     = portOutputRegister(digitalPinToPort(rd));
  #endif
  #if defined(__SAM3X8E__)
    csPort     = digitalPinToPort(cs);
    cdPort     = digitalPinToPort(cd);
    wrPort     = digitalPinToPort(wr);
    rdPort     = digitalPinToPort(rd);
  #endif
  csPinSet   = digitalPinToBitMask(cs);
  cdPinSet   = digitalPinToBitMask(cd);
  wrPinSet   = digitalPinToBitMask(wr);
  rdPinSet   = digitalPinToBitMask(rd);
  csPinUnset = ~csPinSet;
  cdPinUnset = ~cdPinSet;
  wrPinUnset = ~wrPinSet;
  rdPinUnset = ~rdPinSet;
  #ifdef __AVR__
    *csPort   |=  csPinSet; // Set all control bits to HIGH (idle)
    *cdPort   |=  cdPinSet; // Signals are ACTIVE LOW
    *wrPort   |=  wrPinSet;
    *rdPort   |=  rdPinSet;
  #endif
  #if defined(__SAM3X8E__)
    csPort->PIO_SODR  |=  csPinSet; // Set all control bits to HIGH (idle)
    cdPort->PIO_SODR  |=  cdPinSet; // Signals are ACTIVE LOW
    wrPort->PIO_SODR  |=  wrPinSet;
    rdPort->PIO_SODR  |=  rdPinSet;
  #endif
  pinMode(cs, OUTPUT);    // Enable outputs
  pinMode(cd, OUTPUT);
  pinMode(wr, OUTPUT);
  pinMode(rd, OUTPUT);
  if(reset) {
    digitalWrite(reset, HIGH);
    pinMode(reset, OUTPUT);
  }
#endif

  init();
}

void Shift74HC595::fastWrite(uint8_t pin, uint8_t val) {
    
	uint8_t bit = digitalPinToBitMask(pin);
    volatile uint8_t *out = portOutputRegister(digitalPinToPort(pin));
    if (val == LOW) {
		*out &= ~bit;
	}
    else {
		*out |= bit;
	}
    
};

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

	if (port == NOT_A_PIN) return;

	out = portOutputRegister(port);

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

ITDB02::ITDB02(int RS, int WR,int CS, int RST, byte display)
{ 
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
   DDRA = 0xFF;
#else
   DDRD = 0xFF;
#endif
	P_RS	= portOutputRegister(digitalPinToPort(RS));
	B_RS	= digitalPinToBitMask(RS);
	P_WR	= portOutputRegister(digitalPinToPort(WR));
	B_WR	= digitalPinToBitMask(WR);
	P_CS	= portOutputRegister(digitalPinToPort(CS));
	B_CS	= digitalPinToBitMask(CS);
	P_RST	= portOutputRegister(digitalPinToPort(RST));
	B_RST	= digitalPinToBitMask(RST);
	pinMode(RS,OUTPUT);
	pinMode(WR,OUTPUT);
	pinMode(CS,OUTPUT);
	pinMode(RST,OUTPUT);
	displaymodel = display;
}

//************************************************************************
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 < 112))
	{
			
		// 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	=	T1_PS_1_256;
			mT1ClearIntFlag();
			ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_3 | T1_INT_SUB_PRIOR_1);
		}
		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	=	portOutputRegister(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			=	((F_CPU / 256) / 2 / frequency);
		T1CONSET	=	T1_ON;
	}
}

// Change pin assignments post-constructor, using arbitrary pins:
void Adafruit_WS2801::updatePins(uint8_t dpin, uint8_t cpin) {

  if(begun == true) { // If begin() was previously invoked...
    // If previously using hardware SPI, turn that off:
    if(hardwareSPI == true) SPI.end();
    // Regardless, now enable output on 'soft' SPI pins:
    pinMode(dpin, OUTPUT);
    pinMode(cpin, OUTPUT);
  } // Otherwise, pins are not set to outputs until begin() is called.

  // Note: any prior clock/data pin directions are left as-is and are
  // NOT restored as inputs!

  hardwareSPI = false;
  datapin     = dpin;
  clkpin      = cpin;
  clkport     = portOutputRegister(digitalPinToPort(cpin));
  clkpinmask  = digitalPinToBitMask(cpin);
  dataport    = portOutputRegister(digitalPinToPort(dpin));
  datapinmask = digitalPinToBitMask(dpin);
}

/*
 * Constructor for the sensor.  It remembers the pin number and the
 * type of sensor, and initializes internal variables.
 */
DHT_nonblocking::DHT_nonblocking( uint8_t pin, uint8_t type )
	: _pin( pin ),
	  _type( type ),
	  _bit( digitalPinToBitMask( pin ) ),
	  _port( digitalPinToPort( pin ) ),
	  _maxcycles( microsecondsToClockCycles( 1000 ) )
{
  dht_state = DHT_IDLE;

  pinMode( _pin, INPUT );
  digitalWrite( _pin, HIGH );
}

// ------------------------------------------------------------------------------
// Initialize, set the baudrate and enable RX
// ------------------------------------------------------------------------------
void gSoftSerial::begin(uint16_t baudRate)
{
  pinMode(rxPin, INPUT_PULLUP);
  pinMode(txPin, OUTPUT);
  *txPort |= txBitMask;		// high = idle
  delay(1);             	// need some idle time
  
  rxBitMask = digitalPinToBitMask(rxPin);
  rxPort = portInputRegister(digitalPinToPort(rxPin));
  txBitMask = digitalPinToBitMask(txPin);
  txPort = portOutputRegister(digitalPinToPort(txPin));
 
#if F_CPU == 8000000L
  // Have to use timer 2 for an 8 MHz system.
  TCCR2A = 0x00;
  TCCR2B = 0x03;	// divide by 32
#endif
 
  setBaudRate(baudRate);
  listen();
}

void SendOnlySoftwareSerial::setTX(uint8_t tx)
{
  // First write, then set output. If we do this the other way around,
  // the pin would be output low for a short while before switching to
  // output hihg. Now, it is input with pullup for a short while, which
  // is fine. With inverse logic, either order is fine.
  digitalWrite(tx, _inverse_logic ? LOW : HIGH);
  pinMode(tx, OUTPUT);
  _transmitBitMask = digitalPinToBitMask(tx);
  uint8_t port = digitalPinToPort(tx);
  _transmitPortRegister = portOutputRegister(port);
}

ADS7843 :: ADS7843(uint32_t cs, uint32_t dclk, uint32_t din, uint32_t dout, uint32_t irq)
{
	_cs = cs;
	_dclk = dclk;
	_din = din;
	_dout = dout;
	_irq = irq;

	_cs_port = digitalPinToPort(_cs);
	_dclk_port = digitalPinToPort(_dclk);
	_din_port = digitalPinToPort(_din);
	_dout_port = digitalPinToPort(_dout);
	_irq_port = digitalPinToPort(_irq);

	_cs_pin = digitalPinToBitMask(_cs);
	_dclk_pin = digitalPinToBitMask(_dclk);
	_din_pin = digitalPinToBitMask(_din);
	_dout_pin = digitalPinToBitMask(_dout);
	_irq_pin = digitalPinToBitMask(_irq);

}

ArduCAM::ArduCAM(byte model,int CS)
{
#if defined(ESP8266)
	B_CS = CS;	
#else
	P_CS	= portOutputRegister(digitalPinToPort(CS));
	B_CS	= digitalPinToBitMask(CS);
#endif	
	pinMode(CS,OUTPUT);	
	//Must initialize the Bus default status
	sbi(P_CS, B_CS);	
	//flush_fifo();	
	sensor_model=model;
	switch(sensor_model)
	{
		case OV7660:
		case OV7670:
		case OV7675:
		case OV7725:
			sensor_addr = 0x42;
			break;
		case MT9D111_A: //Standard MT9D111 module
			sensor_addr = 0xba;
			break;			
		case MT9D111_B:	//Flex MT9D111 AF module
			sensor_addr = 0x90;
			break;
		case MT9M112:
			sensor_addr = 0x90;	
			break;		
		case MT9M001:
			sensor_addr = 0xba;	
			break;		
		case OV3640:
		case OV5640:
		case OV5642:
		case MT9T112:
		case MT9D112:
			sensor_addr = 0x78;
			break;
		case OV2640:
		case OV9650:
		case OV9655:
			sensor_addr = 0x60;
			break;
		case MT9V111:
			sensor_addr = 0xB8;
			break;		
		default:
			sensor_addr = 0x42;
			break;
	}
}

void digitalSwitch(uint8_t pin) {
	uint8_t bit = digitalPinToBitMask(pin);
	uint8_t port = digitalPinToPort(pin);
	volatile uint8_t *out;
	
	if (port == NOT_A_PIN) return;
		
	out = portOutputRegister(port);
	
	if ( *out & bit) *out &= ~bit;
	else *out |= bit;
}

DHT::DHT(uint8_t pin, uint8_t type/*, uint8_t count*/) {
  _pin = pin;
  _type = type;
  #ifdef __AVR
    _bit = digitalPinToBitMask(pin);
    _port = digitalPinToPort(pin);
  #endif
  _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.
}

void SoftSPIClass::begin(BYTE SSpin, BYTE SCKpin, BYTE MOSIpin, BYTE MISOpin) {
  _SCKpin = SCKpin;
  _MOSIpin = MOSIpin;
  _MISOpin = MISOpin;

  uint8_t p = digitalPinToPort(SSpin) - 1;
  uint8_t b = digitalPinToBitMask(SSpin);
  _SSset = __sets[p];
  _SSclr = __clrs[p];
  _SSport = __ports[p];
  _SSpin = b;

  p = digitalPinToPort(SCKpin) - 1;
  b = digitalPinToBitMask(SCKpin);
  _SCKset = __sets[p];
  _SCKclr = __clrs[p];
  _SCKport = __ports[p];
  _SCKpin = b;

  p = digitalPinToPort(MOSIpin) - 1;
  b = digitalPinToBitMask(MOSIpin);
  _MOSIset = __sets[p];
  _MOSIclr = __clrs[p];
  _MOSIport = __ports[p];
  _MOSIpin = b;

  p = digitalPinToPort(MISOpin) - 1;
  b = digitalPinToBitMask(MISOpin);
  _MISOset = __sets[p];
  _MISOclr = __clrs[p];
  _MISOport = __ports[p];
  _MISOpin = b;

  pinMode(SSpin, OUTPUT);
  pinMode(SCKpin, OUTPUT);
  pinMode(MISOpin, INPUT);
  pinMode(MOSIpin, OUTPUT);
  digitalWrite(SSpin, HIGH);
  digitalWrite(SCKpin, LOW);
}

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

	// initialize this instance's variables
	// Serial.begin(9600);		// for debugging
	error = 1;
	loopTimingFactor = 310;		// determined empirically -  a hack

	CS_Timeout_Millis = (2000 * (float)loopTimingFactor * (float)F_CPU) / 16000000;
	CS_AutocaL_Millis = 20000;

	// Serial.print("timwOut =  ");
	// Serial.println(CS_Timeout_Millis);

	// get pin mapping and port for send Pin - from PinMode function in core

#ifdef NUM_DIGITAL_PINS
	if (sendPin >= NUM_DIGITAL_PINS) error = -1;
	if (receivePin >= NUM_DIGITAL_PINS) error = -1;
#endif

	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();
	leastTotal = 0x0FFFFFFFL;   // input large value for autocalibrate begin
	lastCal = millis();         // set millis for start
}

void ServoOut::update(bool p_pinsChanged)
{
    uint16_t remainingTime = m_pauseLength;
    uint8_t idx = 0;

    const uint16_t* values = getRawOutputChannels();
    for (uint8_t i = 0; i < RC_MAX_CHANNELS; ++i)
    {
        if (m_pins[i] != 0 && values[i] != 0)
        {
            if (p_pinsChanged)
            {
                uint8_t mask = digitalPinToBitMask(m_pins[i]);
                uint8_t port = digitalPinToPort(m_pins[i]);
                volatile uint8_t* out = portInputRegister(port);

                RC_ASSERT_MINMAX(values[i], 0, 32766);

                TIMSK1 &= ~(1 << OCIE1B);
                m_timings[idx] = values[i] << 1;
                m_ports[idx] = static_cast<uint8_t>(reinterpret_cast<uint16_t>(out) & 0xFF);
                m_masks[idx] = mask;
                TIMSK1 |= (1 << OCIE1B);
            }
            else
            {
                RC_ASSERT_MINMAX(values[i], 0, 32766);

                TIMSK1 &= ~(1 << OCIE1B);
                m_timings[idx] = values[i] << 1;
                TIMSK1 |= (1 << OCIE1B);
            }

            if (remainingTime < values[i])
            {
                remainingTime = 0;
            }
            else
            {
                remainingTime -= values[i];
            }

            ++idx;
        }
    }

    TIMSK1 &= ~(1 << OCIE1B);
    m_timings[idx] = remainingTime << 1;
    m_ports[idx] = 0;
    m_masks[idx] = 0;
    TIMSK1 = (1 << OCIE1B);
}

DHT::DHT(uint8_t pin) : _sensorID(-1)
{
  _pin = pin;
  
  #ifdef __AVR
    _bit = digitalPinToBitMask(pin);
    _port = digitalPinToPort(pin);
  #endif
  
  // 1 millisecond timeout for reading pulses from DHT sensor.
  // The DHT reading algorithm adjusts itself based on the speed of the processor.
  _maxcycles = microsecondsToClockCycles(1000);
}

void pwm::pinSetting(uint8_t pin){
    uint8_t bit = digitalPinToBitMask(pin); // get pin bit
    uint8_t port = digitalPinToPort(pin);   // get pin port
    volatile uint8_t *sel;

    if (port == NOT_A_PORT) return; // pin on timer?

    sel = portSelRegister(port); // get the port function select register address
    *sel |= bit;                 // set bit in pin function select register
    /// set pin in output mode
    pinMode(pin, OUTPUT);

}

void pinMode(uint8_t pin, uint8_t mode) {
	uint8_t bit = digitalPinToBitMask(pin);
	uint8_t port = digitalPinToPort(pin);
	volatile uint8_t *reg;

	if (port == NOT_A_PIN) return;

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

	if (mode == INPUT) *reg &= ~bit;
	else *reg |= bit;
}