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

boolean WatchPin::pinIsAnalogic(uint8_t _pin){
  uint8_t bit = digitalPinToBitMask(_pin);			//bitmask of pin requested
  uint8_t port = digitalPinToPort(_pin);			//port of pin requested

//if pin requested is I/O return false else return if analogic pin requested is working as analog or digital
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  if (_pin < 54){
    return false;
  }else{
    return ((*portModeRegister(port) & bit)==0) ? true:false;
  }
#elif defined(__AVR_ATmega32U4__)
  if (_pin < 18){
    return false;
  }else{
    return ((*portModeRegister(port) & bit)==0) ? true:false;
  }
#elif defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__)
  if (_pin < 24){
    return false;
  }else{
    return ((*portModeRegister(port) & bit)==0) ? true:false;
  }
#else
  if (_pin < 14){
    return false;
  }else{
    return ((*portModeRegister(port) & bit)==0) ? true:false;
  }
#endif
}

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

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

MeRGBLed::MeRGBLed(uint8_t port,uint8_t slot):MePort(port){
	if(slot==1){
		pinMask = digitalPinToBitMask(s2);
		ws2812_port = portOutputRegister(digitalPinToPort(s2));
		ws2812_port_reg = portModeRegister(digitalPinToPort(s2));
                pinMode(s2,OUTPUT);
	}else{
		pinMask = digitalPinToBitMask(s1);
		ws2812_port = portOutputRegister(digitalPinToPort(s1));
		ws2812_port_reg = portModeRegister(digitalPinToPort(s1));
                pinMode(s1,OUTPUT);
	}
	//*ws2812_port_reg |= pinMask; // set pinMode OUTPUT
	setNumber(4);
}

void TimerFreeTone(uint8_t pin, unsigned long frequency, unsigned int duration) {
  if (!frequency) { delay(duration); return; } // Frequency is false (zero), nothing to play, just delay for duration and return.
  unsigned long endtime = millis() + duration; // Calculate when the note will end.
  unsigned long notch = 500000 / frequency;    // Calculate how fast to toggle the pin.
#ifdef __AVR__
  uint8_t pinBit = digitalPinToBitMask(pin);                                           // Get the bitmask for the pin.
  volatile uint8_t *pinOutput = (uint8_t *) portOutputRegister(digitalPinToPort(pin)); // Get the port register for the pin.
  uint8_t *portMode = (uint8_t *) portModeRegister(digitalPinToPort(pin));             // Get the port mode register for the pin.
  *portMode |= pinBit;                                                                 // Set pin to output mode.
#else
  pinMode(pin, OUTPUT);                                                                // Set pin to output mode.
#endif
  while (millis() < endtime) { // Loop for the desired duration.
#ifdef __AVR__
    *pinOutput |= pinBit;      // Set pin high.
    delayMicroseconds(notch);  // Square wave duration (how long to leave pin high).
    *pinOutput &= ~pinBit;     // Set pin low.
#else
    digitalWrite(pin,HIGH);    // Set pin high.
    delayMicroseconds(notch);  // Square wave duration (how long to leave pin high).
    digitalWrite(pin,LOW);     // Set pin low.
#endif
    delayMicroseconds(notch);  // Square wave duration (how long to leave pin low).
  }
}

void gpio_pin_mode(const uint8_t pin_no, pin_mode_t pin_mode)
{
	const uint8_t port = digitalPinToPort(pin_no);
	if (!port) return;

	const uint8_t mask = digitalPinToBitMask(pin_no);
	volatile uint8_t *ptr_ddr = portModeRegister(port);
	volatile uint8_t *ptr_out = portOutputRegister(port);

	uint8_t oldSREG = SREG;
	cli();
	switch (pin_mode)
	{
	case INPUT:
		*ptr_ddr &= ~mask;
		*ptr_out &= ~mask;
		break;
	case INPUT_PULLUP:
		*ptr_ddr &= ~mask;
		*ptr_out |= mask;
		break;
	case OUTPUT:
		*ptr_ddr |= mask;
		break;
	};
	SREG = oldSREG; // restore interrupts, if enabled
}

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

void pinMode(uint8_t pin, uint8_t mode)
{
	if( (pin >= 0) && (pin <= 7) ) //port A outputs!
		return; //do nothing!
	if( (pin >= 16) && (pin <= 23) ) //port C inputs!
		return; //do nothing!
		
	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) { 
		uint8_t oldSREG = SREG;
                cli();
		*reg &= ~bit;
		SREG = oldSREG;
	} else {
		uint8_t oldSREG = SREG;
                cli();
		*reg |= bit;
		SREG = oldSREG;
	}
}

void _portMode(uint8_t port, uint8_t mode)
{
  volatile uint8_t *moderegister;

  moderegister = portModeRegister(port);

  if(moderegister == NULL) 
    return;

  uint8_t oldSREG = SREG;
  cli();

  if(mode == OUTPUT)
    *moderegister = 0xff;
  else if(mode == INPUT_PULLUP)
  {
    *moderegister = 0x00;
    volatile uint8_t *portregister = portOutputRegister(port);
    *portregister = 0xff;
  }  
  else // INPUT
    *moderegister = 0x00;

  SREG = oldSREG;
}

int8_t PinoccioScout::getRegisterPinMode(uint8_t pin) {
  if (isPinReserved(pin)) {
    return -1;
  }
  if ((~(*portModeRegister(digitalPinToPort(pin))) & digitalPinToBitMask(pin)) &&
      (~(*portOutputRegister(digitalPinToPort(pin))) & digitalPinToBitMask(pin))) {
    return INPUT; // 0
  }
  if ((*portModeRegister(digitalPinToPort(pin)) & digitalPinToBitMask(pin))) {
    return OUTPUT; // 1
  }
  if ((~(*portModeRegister(digitalPinToPort(pin))) & digitalPinToBitMask(pin)) &&
      (*portOutputRegister(digitalPinToPort(pin)) & digitalPinToBitMask(pin))) {
    return INPUT_PULLUP; // 2
  }
}

void WeatherStation::disableBeforeSleep()
{
    volatile uint8_t *out, *ddr;
    uint8_t i;

    setCurrentDate();

    lcd.noBacklight();

    sensorMgt.stop();

    overTime = 0;

    delay(100);

    /**close ports**/
    for( i = 0; i < PALETTE_PORT_NUM; i++ )
    {
        ddr = portModeRegister(ioPort[i]);
        out = portOutputRegister(ioPort[i]);

        ioPreDDRValue[i] = *ddr;
        ioPrePORTValue[i] = *out;

        (*ddr) &= ioDisableMask[i];
        (*out) &= ioDisableMask[i];
    }


}

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

void TimerFreeTone(uint8_t pin, unsigned int frequency, unsigned int duration) {
  if (!frequency) { delay(duration); return; } // Frequency is false (zero), nothing to play, just delay for duration and return.
  uint16_t notch = 500000 / frequency;       // Calculate how fast to toggle the pin.
  uint32_t loops = (duration * 500L) / notch; // Calculate the number of loops to run.
#ifdef __AVR__
  uint8_t pinBit = digitalPinToBitMask(pin);                                           // Get the bitmask for the pin.
  uint8_t *pinOutput = (uint8_t *) portOutputRegister(digitalPinToPort(pin)); // Get the port register for the pin.
  uint8_t *portMode = (uint8_t *) portModeRegister(digitalPinToPort(pin));             // Get the port mode register for the pin.
  *portMode |= pinBit;                                                                 // Set pin to output mode.
#else
  pinMode(pin, OUTPUT);                                                                // Set pin to output mode.
#endif
  for (uint32_t i=0; i<loops; i++) { // Loop for the desired number of loops.
#ifdef __AVR__
    *pinOutput |= pinBit;      // Set pin high.
    delayMicroseconds(notch);  // Square wave duration (how long to leave pin high).
    *pinOutput &= ~pinBit;     // Set pin low.
#else
    digitalWrite(pin,HIGH);    // Set pin high.
    delayMicroseconds(notch);  // Square wave duration (how long to leave pin high).
    digitalWrite(pin,LOW);     // Set pin low.
#endif
    delayMicroseconds(notch);  // Square wave duration (how long to leave pin low).
  }
}

void pinMode ( int pin, pinmode_e 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;
  }

  return;
}

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

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
	
}

/*******************************************************************************
 *  Configure pin 17 on Teensy LC for OUTPUT LOW
 *******************************************************************************/
void Snoozelc5vBuffer::enableDriver( void ) {
#if defined(KINETISL)
    *portModeRegister( 17 ) &= ~digitalPinToBitMask( 17 );
    volatile uint32_t *config;
    config = portConfigRegister( 17 );
    *config = return_core_pin_config;
    pinMode( 17, OUTPUT );
    digitalWriteFast( 17, LOW );
#endif
}

//op is port data info (3 bytes) index, avr ports compatible
void I2CServerBranch::dispatch(char op) {
	//Serial.print("i2c dispatch ");
	//Serial.println(op,BIN);
  Wire.beginTransmission(serverId);
  Wire.write((hostPort<<2)|op);//codify operation on lower 2 bits
  for(int n=0;n<size;n++) {
  	Wire.write(*(portModeRegister(localPort+n)+op));
  }
  Wire.endTransmission(serverId);
}