void pinToggleInit(int pinNum)
{
	togglePinNum = digitalPinToBitMask(pinNum);
	togglePinMode = (uint32_t *) portModeRegister(digitalPinToPort(pinNum));
	togglePinOut = (uint32_t *) portOutputRegister(digitalPinToPort(pinNum));
	
	// set pin 13 port as output
	*togglePinMode |= togglePinNum;
}

void MeRGBLed::reset(MEPORT port, uint8_t slot)
{
    s2 = mePort[port].s2;
    s1 = mePort[port].s1;
    if(slot == SLOT2)
    {
        pinMask = digitalPinToBitMask(s2);
        ws2812_port = portOutputRegister(digitalPinToPort(s2));
        pinMode(s2, OUTPUT);
    }
    else
    {
        pinMask = digitalPinToBitMask(s1);
        ws2812_port = portOutputRegister(digitalPinToPort(s1));
        pinMode(s1, OUTPUT);
    }
    setNumber(4);
}

/**
 * \par Function
 *   reset
 * \par Description
 *   Reset the LED available data PIN by its RJ25 port and slot.
 * \param[in]
 *   port - RJ25 port from PORT_1 to M2
 * \param[in]
 *   slot - SLOT1 or SLOT2
 * \par Output
 *   None
 * \return
 *   None
 * \par Others
 *   None
 */
void MeRGBLed::reset(uint8_t port,uint8_t slot)
{
  _port = port;
  s2    = mePort[port].s2;
  s1    = mePort[port].s1;
  if(SLOT2 == slot)
  {
    pinMask     = digitalPinToBitMask(s2);
    ws2812_port = portOutputRegister(digitalPinToPort(s2) );
    pinMode(s2, OUTPUT);
  }
  else
  {
    pinMask     = digitalPinToBitMask(s1);
    ws2812_port = portOutputRegister(digitalPinToPort(s1) );
    pinMode(s1, OUTPUT);
  }
}

FrequencyTask::FrequencyTask(int outPin, float freq) : Task(0, &(FrequencyTask::step))
{
  pinMode(outPin, OUTPUT);
  
  _bitMask = digitalPinToBitMask(outPin);
  _portRegister = portOutputRegister(digitalPinToPort(outPin));
  
  this->setFrequency(freq);
}

// 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...
    // 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!

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

CapSense::CapSense(uint8_t sendPin, uint8_t receivePin){
	
	// 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
	
	sBit =  digitalPinToBitMask(sendPin);            // get send pin's ports and bitmask
	sPort = digitalPinToPort(sendPin);
	sOut = portOutputRegister(sPort);                // get pointer to output register   
	
	 rBit = digitalPinToBitMask(receivePin);         // get receive pin's ports and bitmask 
	 rPort = digitalPinToPort(receivePin);
	
   	rOut = portOutputRegister(rPort);
	
	if (sPort == NOT_A_PORT) {error = -1;           // this does not appear to work
	// Serial.println("bad pin");
	}
	sReg = portModeRegister(sPort);
	
	// get pin mapping and port for receive Pin - from digital pin functions in Wiring.c
	
	
	if (rPort == NOT_A_PORT){ error = -1;          // this does not appear to work
	// Serial.println("bad pin");
	}
	rReg = portModeRegister(rPort);
	rOut = portOutputRegister(rPort);
	rIn  = portInputRegister(rPort);
	
    *sReg |= sBit;         // set sendpin to OUTPUT 
    leastTotal = 0x0FFFFFFFL;   // input large value for autocalibrate begin
    lastCal = millis();         // set millis for start
	
}

void Adafruit_GP9002::begin(void) {
  // set pin directions
  pinMode(_dc, OUTPUT);
  pinMode(_cs, OUTPUT);

  if (! hwSPI) {
    pinMode(_mosi, OUTPUT);
    pinMode(_miso, INPUT);
    pinMode(_sclk, OUTPUT);
    
    clkport     = portOutputRegister(digitalPinToPort(_sclk));
    clkpinmask  = digitalPinToBitMask(_sclk);
    mosiport    = portOutputRegister(digitalPinToPort(_mosi));
    mosipinmask = digitalPinToBitMask(_mosi);
    misopin = portInputRegister(digitalPinToPort(_miso));
    misopinmask = digitalPinToBitMask(_miso);
  } else {
    SPI.begin();
    SPI.setClockDivider(SPI_CLOCK_DIV4);
    SPI.setBitOrder(MSBFIRST);
    SPI.setDataMode(SPI_MODE0);
  }
  csport    = portOutputRegister(digitalPinToPort(_cs));
  cspinmask = digitalPinToBitMask(_cs);
  dcport    = portOutputRegister(digitalPinToPort(_dc));
  dcpinmask = digitalPinToBitMask(_dc);

  command(GP9002_DISPLAY);
  dataWrite(GP9002_DISPLAY_MONOCHROME);
  command(GP9002_LOWERADDR1);
  dataWrite(0x0);
  command(GP9002_HIGHERADDR1);
  dataWrite(0x0);
  command(GP9002_LOWERADDR2);
  dataWrite(0x0);
  command(GP9002_HIGHERADDR2);
  dataWrite(0x4);
  command(GP9002_OR);
  command(GP9002_CLEARSCREEN);
  command(GP9002_DISPLAY1ON);

  // hold the address so we can read and then write
  command(GP9002_ADDRHELD);
}

ArduCAM::ArduCAM(byte model,int CS)
{ 
#if defined(ESP8266) || defined(TEENSYDUINO)
	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 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
}

//
// Constructor
// prepares the digitalWrite()
FastShiftOut::FastShiftOut(const uint8_t datapin, const uint8_t clockpin, const uint8_t bitOrder)
{
    _bitorder = bitOrder;
    _value = -1;
    pinMode(datapin, OUTPUT);
    pinMode(clockpin, OUTPUT);

    // uint8_t _datatimer  = digitalPinToTimer(datapin);
    // if (_datatimer != NOT_ON_TIMER) turnOffPWM(_datatimer); TODO
    uint8_t _dataport   = digitalPinToPort(datapin);
    _dataout = portOutputRegister(_dataport);
    _databit = digitalPinToBitMask(datapin);

    // uint8_t _clocktimer = digitalPinToTimer(clockpin);
    // if (_clocktimer != NOT_ON_TIMER) turnOffPWM(_clocktimer);
    uint8_t _clockport  = digitalPinToPort(clockpin);
    _clockout = portOutputRegister(_clockport);
    _clockbit = digitalPinToBitMask(clockpin);
}

void hardHalfDuplexSerial::setDirPin(const uint8_t dirPin)
{
  _dirBitMask = digitalPinToBitMask(dirPin); // bit of dir pin
  _dirPortRegister = portOutputRegister(digitalPinToPort(dirPin)); // Output register of dir pin

  pinMode(dirPin, OUTPUT);
  *_dirPortRegister &= ~_dirBitMask; // digitalWrite(dirPin, LOW);  // should be put to High when writting
  _dirPinInitialized = true;

}

Adafruit_VS1053::Adafruit_VS1053(uint8_t mosi, uint8_t miso, uint8_t clk,
                                 uint8_t rst, uint8_t cs, uint8_t dcs, uint8_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 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);

}

/**
 * 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);
}

//
// Turn Arduino pin numbers into PORTx, DDRx, and PINx
//
void SoftI2CMaster::setPins(uint8_t sdaPin, uint8_t sclPin, uint8_t pullups)
{
    uint8_t port;
    
    usePullups = pullups;

    _sdaPin = sdaPin;
    _sclPin = sclPin;
    
    _sdaBitMask = digitalPinToBitMask(sdaPin);
    _sclBitMask = digitalPinToBitMask(sclPin);
    
    port = digitalPinToPort(sdaPin);
    _sdaPortReg  = portOutputRegister(port);
    _sdaDirReg   = portModeRegister(port);
    
    port = digitalPinToPort(sclPin);
    _sclPortReg  = portOutputRegister(port);
    _sclDirReg   = portModeRegister(port);
}