void RH_RF95::setTxPower(int8_t power)
{
    if (power > 20)
	power = 20;
    if (power < 5)
	power = 5;
    // RFM95/96/97/98 does not have RFO pins connected to anything. ONly PA_BOOST
    // pin is connected, so must use PA_BOOST
    // Pout = 2 + OutputPower.
    // The documentation is pretty confusing on this topic: PaSelect says the max poer is 20dBm,
    // but OutputPower claims it would be 17dBm.
    // My measurements show 20dBm is correct
    spiWrite(RH_RF95_REG_09_PA_CONFIG, RH_RF95_PA_SELECT | (power-5));
//    spiWrite(RH_RF95_REG_09_PA_CONFIG, 0); // no power
}

bool RH_RF95::setFrequency(float centre)
{
	// Frf = FRF / FSTEP
	uint32_t frf = (centre * 1000000.0) / RH_RF95_FSTEP;

	ATOMIC_BLOCK_START;

	spiWrite(RH_RF95_REG_06_FRF_MSB, (frf >> 16) & 0xFF);
	spiWrite(RH_RF95_REG_07_FRF_MID, (frf >>  8) & 0xFF);
	spiWrite(RH_RF95_REG_08_FRF_LSB, (frf      ) & 0xFF);

	ATOMIC_BLOCK_END;

	return true;
}

/*
 * Class:     edu_wpi_first_wpilibj_hal_SPIJNI
 * Method:    spiWrite
 * Signature: (BLjava/nio/ByteBuffer;B)I
 */
JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiWrite
  (JNIEnv * env, jclass, jbyte port, jobject dataToSend, jbyte size)
{
	SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiWrite";
	SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port;
	uint8_t* dataToSendPtr = nullptr;
	if (dataToSend != 0) {
		dataToSendPtr = (uint8_t*)env->GetDirectBufferAddress(dataToSend);
	}
	SPIJNI_LOG(logDEBUG) << "Size = " << (jint)size;
	SPIJNI_LOG(logDEBUG) << "DataToSendPtr = " << dataToSendPtr;
	jint retVal = spiWrite(port, dataToSendPtr, size);
	SPIJNI_LOG(logDEBUG) << "ReturnValue = " << (jint)retVal;
	return retVal;
}

void Adafruit_PCD8544::display(void) {
  uint8_t col, maxcol, p;
  
  for(p = 0; p < 6; p++) {
#ifdef enablePartialUpdate
    // check if this page is part of update
    if ( yUpdateMin >= ((p+1)*8) ) {
      continue;   // nope, skip it!
    }
    if (yUpdateMax < p*8) {
      break;
    }
#endif

    command(PCD8544_SETYADDR | p);


#ifdef enablePartialUpdate
    col = xUpdateMin;
    maxcol = xUpdateMax;
#else
    // start at the beginning of the row
    col = 0;
    maxcol = LCDWIDTH-1;
#endif

    command(PCD8544_SETXADDR | col);

    digitalWrite(_dc, HIGH);
    if (_cs > 0)
      digitalWrite(_cs, LOW);
    for(; col <= maxcol; col++) {
      spiWrite(pcd8544_buffer[(LCDWIDTH*p)+col]);
    }
    if (_cs > 0)
      digitalWrite(_cs, HIGH);

  }

  command(PCD8544_SETYADDR );  // no idea why this is necessary but it is to finish the last byte?
#ifdef enablePartialUpdate
  xUpdateMin = LCDWIDTH - 1;
  xUpdateMax = 0;
  yUpdateMin = LCDHEIGHT-1;
  yUpdateMax = 0;
#endif

}

void ssd1306_send(uint8_t* data, uint16_t dataSize)
{
    uint16_t i;

    OLED_DC = 1;
    OLED_CS = 0;

    for (i = 0; i < dataSize; i++, data++)
    {
        spiWrite(*data);
    }

    Nop();
    Nop();
    OLED_CS = 1;
}

static void ssd1306_sendCMD(uint8_t* cmd, uint8_t cmdSize)
{
    uint8_t i;

    OLED_DC = 0;
    OLED_CS = 0;

    for (i = 0; i < cmdSize; i++, cmd++)
    {
        spiWrite(*cmd);
    }

    Nop();
    Nop();
    OLED_CS = 1;
}

/**
 * It processing incoming data by the radio or serial connection. This function
 * has to be continously called to keep the node running. This function also
 * adds a delay which is specified as 100ms per unit. Therefore 1000 = 100 sec
 * @param countdown delay added to this function
 */
void awaitData(int countdown) {

    uint8_t rx_len, flags1, old_flags1 = 0x90;

    //Clear buffer
    data_temp[0] = '\0';

    RFM69_setMode(RFM69_MODE_RX);

    rx_restarts = 0;

    while(countdown > 0) {

        flags1 = spiRead(RFM69_REG_27_IRQ_FLAGS1);
#ifdef DEBUG
        if (flags1 != old_flags1) {
            printf("f1: %02x\r\n", flags1);
            old_flags1 = flags1;
        }
#endif
        if (flags1 & RF_IRQFLAGS1_TIMEOUT) {
            // restart the Rx process
            spiWrite(RFM69_REG_3D_PACKET_CONFIG2, spiRead(RFM69_REG_3D_PACKET_CONFIG2) | RF_PACKET2_RXRESTART);
            rx_restarts++;
            // reset the RSSI threshold
            floor_rssi = RFM69_sampleRssi();
#ifdef DEBUG
            // and print threshold
            printf("Restart Rx %d\r\n", RFM69_lastRssiThreshold());
#endif
        }
        // Check rx buffer
        if(RFM69_checkRx() == 1) {
            RFM69_recv(data_temp,  &rx_len);
            data_temp[rx_len - 1] = '\0';
            processData(rx_len);
        }

        countdown--;
        mrtDelay(100);
    }
}

/**
Sends a Module Synchronous Request (SREQ) message and retrieves the response. A SREQ is a message to 
the Module that is immediately followed by a Synchronous Response (SRSP) message from the Module. As 
opposed to an Asynchronous Request (AREQ) message, which does not have a SRSP. This is a private 
method that gets wrapped by sendMessage() and spiPoll().
@pre Module has been initialized
@pre zmBuf contains a properly formatted message. No validation is done.
@post received data is written to zmBuf
@return if FAST_PROCESSOR is defined then MODULE_SUCCESS, else an error code. If FAST_PROCESSOR is not defined, then MODULE_SUCCESS.
*/
moduleResult_t sendSreq()
{
#ifdef FAST_PROCESSOR                           //NOTE: only enable if using a processor with sufficient speed (25MHz+)
  uint32_t timeLeft1 = CHIP_SELECT_TO_SRDY_LOW_TIMEOUT;
  uint32_t timeLeft2 = WAIT_FOR_SRSP_TIMEOUT;
  
  SPI_SS_SET();                               // Assert SS
  while (SRDY_IS_HIGH() && (timeLeft1 != 0))  //wait until SRDY goes low
    timeLeft1--;
  if (timeLeft1 == 0)                         //SRDY did not go low in time, so return an error
    return ZM_PHY_CHIP_SELECT_TIMEOUT;
  timeFromChipSelectToSrdyLow = (CHIP_SELECT_TO_SRDY_LOW_TIMEOUT - timeLeft1);
  
  spiWrite(zmBuf, (*zmBuf + 3));              // *bytes (first byte) is length after the first 3 bytes, all frames have at least the first 3 bytes
  *zmBuf = 0; *(zmBuf+1) = 0; *(zmBuf+2) = 0; //poll message is 0,0,0
  //NOTE: MRDY must remain asserted here, but can de-assert SS if the two signals are separate
  
  /* Now: Data was sent, so we wait for Synchronous Response (SRSP) to be received.
  This will be indicated by SRDY transitioning to high */
  
  while (SRDY_IS_LOW() && (timeLeft2 != 0))    //wait for data
    timeLeft2--;
  if (timeLeft2 == 0)
    return ZM_PHY_SRSP_TIMEOUT;
  
  timeWaitingForSrsp = (WAIT_FOR_SRSP_TIMEOUT - timeLeft2);
  //NOTE: if SS & MRDY are separate signals then can re-assert SS here.
  spiWrite(zmBuf, 3);
  if (*zmBuf > 0)                             // *bytes (first byte) contains number of bytes to receive
    spiWrite(zmBuf+3, *zmBuf);              //write-to-read: read data into buffer
  SPI_SS_CLEAR();
  return 0;
#else                                           // In a slow processor there's not enough time to set up the timeout so there will be errors
  SPI_SS_SET();   
  while (SRDY_IS_HIGH()) ;                    //wait until SRDY goes low
  spiWrite(zmBuf, (*zmBuf + 3));              // *bytes (first byte) is length after the first 3 bytes, all frames have at least the first 3 bytes
  *zmBuf = 0; *(zmBuf+1) = 0; *(zmBuf+2) = 0; //poll message is 0,0,0
  //NOTE: MRDY must remain asserted here, but can de-assert SS if the two signals are separate
  
  //Now: Data was sent, wait for Synchronous Response (SRSP)
  while (SRDY_IS_LOW()) ;                     //wait for data
  //NOTE: if SS & MRDY are separate signals then can re-assert SS here.
  spiWrite(zmBuf, 3);
  if (*zmBuf > 0)                             // *bytes (first byte) contains number of bytes to receive
    spiWrite(zmBuf+3, *zmBuf);              //write-to-read: read data into buffer    
  SPI_SS_CLEAR();                             // re-assert MRDY and SS
  return MODULE_SUCCESS;  
#endif
}

uint8_t RFM69_init()
{
    mrtDelay(12);
    
    //Configure SPI
    spiInit(LPC_SPI0,24,0);
    
    mrtDelay(100);
    
    // Set up device
    uint8_t i;
    for (i = 0; CONFIG[i][0] != 255; i++)
        spiWrite(CONFIG[i][0], CONFIG[i][1]);
    
    RFM69_setMode(_mode);
    
    // Clear TX/RX Buffer
    _bufLen = 0;
    
    return 1;
}

float RFM69::readTemp()
{
    // Store current transceiver mode
    uint8_t oldMode = _mode;
    // Set mode into Standby (required for temperature measurement)
    setMode(RFM69_MODE_STDBY);
	
    // Trigger Temperature Measurement
    spiWrite(RFM69_REG_4E_TEMP1, RF_TEMP1_MEAS_START);
    // Check Temperature Measurement has started
    if(!(RF_TEMP1_MEAS_RUNNING && spiRead(RFM69_REG_4E_TEMP1))){
        return 255.0;
    }
    // Wait for Measurement to complete
    while(RF_TEMP1_MEAS_RUNNING && spiRead(RFM69_REG_4E_TEMP1)) { };
    // Read raw ADC value
    uint8_t rawTemp = spiRead(RFM69_REG_4F_TEMP2);
	
    // Set transceiver back to original mode
    setMode(oldMode);
    // Return processed temperature value
    return 168.3-float(rawTemp);
}

uint16_t SpiFlash_ReadMidDid(void)
{
    uint8_t u8RxData[2];

    // /CS: active
    spiIoctl(0, SPI_IOC_ENABLE_SS, SPI_SS_SS0, 0);

    // send Command: 0x90, Read Manufacturer/Device ID
    spiWrite(0, 0, 0x90);
    spiIoctl(0, SPI_IOC_TRIGGER, 0, 0);
    while(spiGetBusyStatus(0));

    // send 24-bit '0', dummy
    spiWrite(0, 0, 0x00);
    spiIoctl(0, SPI_IOC_TRIGGER, 0, 0);
    while(spiGetBusyStatus(0));

    spiWrite(0, 0, 0x00);
    spiIoctl(0, SPI_IOC_TRIGGER, 0, 0);
    while(spiGetBusyStatus(0));

    spiWrite(0, 0, 0x00);
    spiIoctl(0, SPI_IOC_TRIGGER, 0, 0);
    while(spiGetBusyStatus(0));

    // receive 16-bit
    spiWrite(0, 0, 0x00);
    spiIoctl(0, SPI_IOC_TRIGGER, 0, 0);
    while(spiGetBusyStatus(0));
    u8RxData[0] = spiRead(0, 0);

    spiWrite(0, 0, 0x00);
    spiIoctl(0, SPI_IOC_TRIGGER, 0, 0);
    while(spiGetBusyStatus(0));
    u8RxData[1] = spiRead(0, 0);

    // /CS: de-active
    spiIoctl(0, SPI_IOC_DISABLE_SS, SPI_SS_SS0, 0);

    return ( (u8RxData[0]<<8) | u8RxData[1] );
}

bool RH_RF95::send(const uint8_t* data, uint8_t len)
{
    if (len > RH_RF95_MAX_MESSAGE_LEN)
	return false;

    waitPacketSent(); // Make sure we dont interrupt an outgoing message
    setModeIdle();

    // Position at the beginning of the FIFO
    spiWrite(RH_RF95_REG_0D_FIFO_ADDR_PTR, 0);
    // The headers
    spiWrite(RH_RF95_REG_00_FIFO, _txHeaderTo);
    spiWrite(RH_RF95_REG_00_FIFO, _txHeaderFrom);
    spiWrite(RH_RF95_REG_00_FIFO, _txHeaderId);
    spiWrite(RH_RF95_REG_00_FIFO, _txHeaderFlags);
    // The message data
    spiBurstWrite(RH_RF95_REG_00_FIFO, data, len);
    spiWrite(RH_RF95_REG_22_PAYLOAD_LENGTH, len + RH_RF95_HEADER_LEN);

    setModeTx(); // Start the transmitter
    // when Tx is done, interruptHandler will fire and radio mode will return to STANDBY
    return true;
}

//------------------------------------------------------------------------
// Adds fhch * fhs to centre frequency
// Returns true if centre + (fhch * fhs) is within limits
bool RF22_setFHChannel(uint8_t fhch)
{
    spiWrite(RF22_REG_79_FREQUENCY_HOPPING_CHANNEL_SELECT, fhch);
    return !(RF22_statusRead() & RF22_FREQERR);
}

void Adafruit_VS1053::sineTest(uint8_t n, uint16_t ms) {
    reset();

    uint16_t mode = sciRead(VS1053_REG_MODE);
    mode |= 0x0020;
    sciWrite(VS1053_REG_MODE, mode);

    while (!digitalRead(_dreq));
    //  delay(10);

#ifdef SPI_HAS_TRANSACTION
    if (useHardwareSPI) SPI.beginTransaction(VS1053_DATA_SPI_SETTING);
#endif
    digitalWrite(_dcs, LOW);
    spiWrite(0x53);
    spiWrite(0xEF);
    spiWrite(0x6E);
    spiWrite(n);
    spiWrite(0x00);
    spiWrite(0x00);
    spiWrite(0x00);
    spiWrite(0x00);
    digitalWrite(_dcs, HIGH);
#ifdef SPI_HAS_TRANSACTION
    if (useHardwareSPI) SPI.endTransaction();
#endif

    delay(ms);

#ifdef SPI_HAS_TRANSACTION
    if (useHardwareSPI) SPI.beginTransaction(VS1053_DATA_SPI_SETTING);
#endif
    digitalWrite(_dcs, LOW);
    spiWrite(0x45);
    spiWrite(0x78);
    spiWrite(0x69);
    spiWrite(0x74);
    spiWrite(0x00);
    spiWrite(0x00);
    spiWrite(0x00);
    spiWrite(0x00);
    digitalWrite(_dcs, HIGH);
#ifdef SPI_HAS_TRANSACTION
    if (useHardwareSPI) SPI.endTransaction();
#endif
}

bool RH_RF95::init()
{
    if (!RHSPIDriver::init())
	return false;

    // Determine the interrupt number that corresponds to the interruptPin
    int interruptNumber = digitalPinToInterrupt(_interruptPin);
    if (interruptNumber == NOT_AN_INTERRUPT)
	return false;

    // No way to check the device type :-(
    
    // Set sleep mode, so we can also set LORA mode:
    spiWrite(RH_RF95_REG_01_OP_MODE, RH_RF95_MODE_SLEEP | RH_RF95_LONG_RANGE_MODE);
    delay(10); // Wait for sleep mode to take over from say, CAD
    // Check we are in sleep mode, with LORA set
    if (spiRead(RH_RF95_REG_01_OP_MODE) != (RH_RF95_MODE_SLEEP | RH_RF95_LONG_RANGE_MODE))
    {
//	Serial.println(spiRead(RH_RF95_REG_01_OP_MODE), HEX);
	return false; // No device present?
    }
    // Set up interrupt handler
    // Since there are a limited number of interrupt glue functions isr*() available,
    // we can only support a limited number of devices simultaneously
    // ON some devices, notably most Arduinos, the interrupt pin passed in is actuallt the 
    // interrupt number. You have to figure out the interruptnumber-to-interruptpin mapping
    // yourself based on knwledge of what Arduino board you are running on.
    _deviceForInterrupt[_interruptCount] = this;
    if (_interruptCount == 0)
	attachInterrupt(interruptNumber, isr0, RISING);
    else if (_interruptCount == 1)
	attachInterrupt(interruptNumber, isr1, RISING);
    else if (_interruptCount == 2)
	attachInterrupt(interruptNumber, isr2, RISING);
    else
	return false; // Too many devices, not enough interrupt vectors
    _interruptCount++;

    // Set up FIFO
    // We configure so that we can use the entire 256 byte FIFO for either receive
    // or transmit, but not both at the same time
    spiWrite(RH_RF95_REG_0E_FIFO_TX_BASE_ADDR, 0);
    spiWrite(RH_RF95_REG_0F_FIFO_RX_BASE_ADDR, 0);

    // Packet format is preamble + explicit-header + payload + crc
    // Explicit Header Mode
    // payload is TO + FROM + ID + FLAGS + message data
    // RX mode is implmented with RXCONTINUOUS
    // max message data length is 255 - 4 = 251 octets

    // Add by Adrien van den Bossche <[email protected]> for Teensy
    // ARM M4 requires the below. else pin interrupt doesn't work properly.
    // On all other platforms, its innocuous, belt and braces
    pinMode(_interruptPin, INPUT); 

    setModeIdle();

    // Set up default configuration
    // No Sync Words in LORA mode.
//    setModemConfig(Bw125Cr45Sf128); // Radio default
    setModemConfig(Bw125Cr48Sf4096);
    setPreambleLength(8); // Default is 8
    // An innocuous ISM frequency, same as RF22's
    setFrequency(434.0);
    // Lowish power
//    setTxPower(13);
    setTxPower(20);

    return true;
}

//------------------------------------------------------------------------
void RF22_setMode(uint8_t mode)
{
    spiWrite(RF22_REG_07_OPERATING_MODE1, mode);
}

//------------------------------------------------------------------------
void RF22_startTransmit()
{
	RF22_sendNextFragment(); // Actually the first fragment
    spiWrite(RF22_REG_3E_PACKET_LENGTH, _bufLen); // Total length that will be sent
    RF22_setModeTx(); // Start the transmitter, turns off the receiver
}

boolean RF22::init()
{
    // Wait for RF22 POR (up to 16msec)
    delay(16);

    // Initialise the slave select pin
    pinMode(_slaveSelectPin, OUTPUT);
    digitalWrite(_slaveSelectPin, HIGH);

    // start the SPI library:
    // Note the RF22 wants mode 0, MSB first and default to 1 Mbps
    _spi->begin();
    _spi->setDataMode(SPI_MODE0);
    _spi->setBitOrder(MSBFIRST);
    _spi->setClockDivider(SPI_CLOCK_DIV16);  // (16 Mhz / 16) = 1 MHz
    delay(100);

    // Software reset the device
    reset();

    // Get the device type and check it
    // This also tests whether we are really connected to a device
    _deviceType = spiRead(RF22_REG_00_DEVICE_TYPE);
    if (   _deviceType != RF22_DEVICE_TYPE_RX_TRX
        && _deviceType != RF22_DEVICE_TYPE_TX)
	return false;

	// Set up interrupt handler
    if (_interrupt == 0)
    {
	_RF22ForInterrupt[0] = this;
	attachInterrupt(0, RF22::isr0, LOW);  
    }
    else if (_interrupt == 1)
    {
	_RF22ForInterrupt[1] = this;
	attachInterrupt(1, RF22::isr1, LOW);  
    }
    else
	return false;
 
    clearTxBuf();
    clearRxBuf();
  
    // Most of these are the POR default
    spiWrite(RF22_REG_7D_TX_FIFO_CONTROL2, RF22_TXFFAEM_THRESHOLD);
    spiWrite(RF22_REG_7E_RX_FIFO_CONTROL,  RF22_RXFFAFULL_THRESHOLD);
    spiWrite(RF22_REG_30_DATA_ACCESS_CONTROL, RF22_ENPACRX | RF22_ENPACTX | RF22_ENCRC | RF22_CRC_CRC_16_IBM);
    // Configure the message headers
    // Here we set up the standard packet format for use by the RF22 library
    // 8 nibbles preamble
    // 2 SYNC words 2d, d4
    // Header length 4 (to, from, id, flags)
    // 1 octet of data length (0 to 255)
    // 0 to 255 octets data
    // 2 CRC octets as CRC16(IBM), computed on the header, length and data
    // On reception the to address is check for validity against RF22_REG_3F_CHECK_HEADER3
    // or the broadcast address of 0xff
    // If no changes are made after this, the transmitted
    // to address will be 0xff, the from address will be 0xff
    // and all such messages will be accepted. This permits the out-of the box
    // RF22 config to act as an unaddresed, unreliable datagram service
    spiWrite(RF22_REG_32_HEADER_CONTROL1, RF22_BCEN_HEADER3 | RF22_HDCH_HEADER3);
    spiWrite(RF22_REG_33_HEADER_CONTROL2, RF22_HDLEN_4 | RF22_SYNCLEN_2);
    setPreambleLength(8);
    uint8_t syncwords[] = { 0x2d, 0xd4 };
    setSyncWords(syncwords, sizeof(syncwords));
    setPromiscuous(false); 
    // Check the TO header against RF22_DEFAULT_NODE_ADDRESS
    spiWrite(RF22_REG_3F_CHECK_HEADER3, RF22_DEFAULT_NODE_ADDRESS);
    // Set the default transmit header values
    setHeaderTo(RF22_DEFAULT_NODE_ADDRESS);
    setHeaderFrom(RF22_DEFAULT_NODE_ADDRESS);
    setHeaderId(0);
    setHeaderFlags(0);

    // Ensure the antenna can be switched automatically according to transmit and receive
    // This assumes GPIO0(out) is connected to TX_ANT(in) to enable tx antenna during transmit
    // This assumes GPIO1(out) is connected to RX_ANT(in) to enable rx antenna during receive
    spiWrite (RF22_REG_0B_GPIO_CONFIGURATION0, 0x12) ; // TX state
    spiWrite (RF22_REG_0C_GPIO_CONFIGURATION1, 0x15) ; // RX state

    // Enable interrupts
    spiWrite(RF22_REG_05_INTERRUPT_ENABLE1, RF22_ENTXFFAEM | RF22_ENRXFFAFULL | RF22_ENPKSENT | RF22_ENPKVALID | RF22_ENCRCERROR | RF22_ENFFERR);
    spiWrite(RF22_REG_06_INTERRUPT_ENABLE2, RF22_ENPREAVAL);

    // Set some defaults. An innocuous ISM frequency, and reasonable pull-in
    setFrequency(434.0, 0.05);
//    setFrequency(900.0);
    // Some slow, reliable default speed and modulation
    setModemConfig(FSK_Rb2_4Fd36);
//    setModemConfig(FSK_Rb125Fd125);
    // Minimum power
    setTxPower(RF22_TXPOW_8DBM);
//    setTxPower(RF22_TXPOW_17DBM);

    return true;
}

/** Send data to the LCD
 */
static void sendData(uint8_t data) {
  pinWrite(g_dc, true);
  spiWrite(&data, 1);
  }

//------------------------------------------------------------------------
void RF22_setHeaderFlags(uint8_t flags)
{
    spiWrite(RF22_REG_3D_TRANSMIT_HEADER0, flags);
}