//----------------------------------------------------------------------------
void 
spiEepromWriteSR(spiEepromDriver * sedp, uint8_t sr)
{
	spiStart(sedp->spip, &sedp->cfgp->spicfg);
	spiSelect(sedp->spip);
	spiPolledExchange(sedp->spip, OP_WRSR);
	spiPolledExchange(sedp->spip, sr);
	spiUnselect(sedp->spip);
}

static int SPISendData(SPIDriver *SPIPtr, uint8_t *TxBuf, size_t Size)
  {
    spiStart(SPIPtr, &HSSpiConfig); /* Setup transfer parameters.       */
    spiSelect(SPIPtr); /* Slave Select assertion.          */
    spiSend(SPIPtr, Size, TxBuf); /* Send command                     */
    spiUnselect(SPIPtr); /* Slave Select de-assertion.       */
    spiStop(SPIPtr);
    return 0;
  }

void _readBuffer(uint16_t len, uint8_t *data)
{
  uint8_t tx[] = { READ_BUF_MEM };

  spiSelect(_spip);
  spiSend(_spip, 1, tx);
  spiReceive(_spip, len, data);
  spiUnselect(_spip);
}

/**
 * @brief   Reads a register value.
 * @pre     The SPI interface must be initialized and the driver started.
 *
 * @param[in] spip      pointer to the SPI initerface
 * @param[in] reg       register number
 * @return              The register value.
 */
uint8_t lis302dlReadRegister(SPIDriver *spip, uint8_t reg) {

  spiSelect(spip);
  txbuf[0] = 0x80 | reg;
  txbuf[1] = 0xff;
  spiExchange(spip, 2, txbuf, rxbuf);
  spiUnselect(spip);
  return rxbuf[1];
}

/**
 * @brief   Writes a value into a generic register using SPI.
 * @pre     The SPI interface must be initialized and the driver started.
 *
 * @param[in] spip      pointer to the SPI interface
 * @param[in] reg       starting register address
 * @param[in] n         number of adjacent registers to write
 * @param[in] b         pointer to a buffer of values.
 */
static void lis302dlSPIWriteRegister(SPIDriver *spip, uint8_t reg, size_t n,
                                     uint8_t* b) {
  uint8_t cmd;
  (n == 1) ? (cmd = reg) : (cmd = reg | LIS302DL_MS);
  spiSelect(spip);
  spiSend(spip, 1, &cmd);
  spiSend(spip, n, b);
  spiUnselect(spip);
}

void ad5504Stop(void) {
  /* All DAC channels are in the power down mode. */
  txBuf[0] = AD5504_CONTROL;
  txBuf[1] = AD5504_DAC_A; /* Set DAC channel A to 0. */

  spiSelect(&SPID1);
  spiSend(&SPID1, 2 * sizeof(uint16_t), (uint8_t *)txBuf);
  spiUnselect(&SPID1);
}

static void
xflash_txn_begin()
{
#if SPI_USE_MUTUAL_EXCLUSION
  spiAcquireBus(SPI_FLASH);
#endif
  spiStart(SPI_FLASH, &flash_spi_cfg);
  spiSelect(SPI_FLASH);
}

void ad5504Init(void) {
  /* Only DAC channel A is in the power up mode. */
  txBuf[0] = AD5504_CONTROL | AD5504_CR_DAC_A_POWER_UP;
  txBuf[1] = AD5504_DAC_A; /* Set DAC channel A to 0. */

  spiSelect(&SPID1);
  spiSend(&SPID1, 2 * sizeof(uint16_t), (uint8_t *)txBuf);
  spiUnselect(&SPID1);
}

/**
 * @brief   Sends a command an returns a single byte response.
 *
 * @param[in] mmcp      pointer to the @p MMCDriver object
 * @param[in] cmd       the command id
 * @param[in] arg       the command argument
 * @return              The response as an @p uint8_t value.
 * @retval 0xFF         timed out.
 *
 * @notapi
 */
static uint8_t send_command_R1(MMCDriver *mmcp, uint8_t cmd, uint32_t arg) {
  uint8_t r1;

  spiSelect(mmcp->config->spip);
  send_hdr(mmcp, cmd, arg);
  r1 = recvr1(mmcp);
  spiUnselect(mmcp->config->spip);
  return r1;
}

/**
 * @brief   Reads a generic register value using SPI.
 * @pre     The SPI interface must be initialized and the driver started.
 *
 * @param[in] spip      pointer to the SPI interface
 * @param[in] reg       starting register address
 * @param[in] n         number of consecutive registers to read
 * @param[in] b         pointer to an output buffer.
 */
static void l3gd20SPIReadRegister(SPIDriver *spip, uint8_t reg,  size_t n,
                                     uint8_t* b) {
  uint8_t cmd;
  (n == 1) ? (cmd = reg | L3GD20_RW) : (cmd = reg | L3GD20_RW | L3GD20_MS);
  spiSelect(spip);
  spiSend(spip, 1, &cmd);
  spiReceive(spip, n, b);
  spiUnselect(spip);
}

/*
 * Read the current acceleration values from the ADXL on SPI driver `SPID`.
 * The values are stored in `accels` as three int16s.
 */
static void adxl3x5_read_accel(SPIDriver* SPID, int16_t* accels)
{
    uint8_t adr = 0x32 | (1<<6) | (1<<7);

    spiSelect(SPID);
    spiSend(SPID, 1, (void*)&adr);
    spiReceive(SPID, 6, (void*)accels);
    spiUnselect(SPID);
}

void _writeBuffer(uint16_t len, uint8_t *data)
{
  uint8_t tx[] = { WRITE_BUF_MEM };

  spiSelect(_spip);
  spiSend(_spip, 1, tx);
  spiSend(_spip, len, data);
  spiUnselect(_spip);
}

/*
 * Application entry point.
 */
int main(void) {
  uint8_t i;

  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /*
   * Activates the SD1 and SPI1 drivers.
   */
  sdStart(&SD1, NULL);                  /* Default: 38400,8,N,1.            */
  spiStart(&SPID1, &spicfg);

  /*
   * Creates the blinker threads.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
  chThdCreateStatic(waThread2, sizeof(waThread2), NORMALPRIO, Thread2, NULL);

  /*
   * Normal main() thread activity, in this demo it updates the 7-segments
   * display on the LPCXpresso main board using the SPI driver.
   */
  i = 0;
  while (TRUE) {
    if (!palReadPad(GPIO0, GPIO0_SW3))
      TestThread(&SD1);
    spiSelect(&SPID1);
    spiSend(&SPID1, 1, &digits[i]);                 /* Non polled method.   */
    spiUnselect(&SPID1);
    chThdSleepMilliseconds(500);
    spiSelect(&SPID1);
    spiPolledExchange(&SPID1, digits[i | 0x10]);    /* Polled method.       */
    spiUnselect(&SPID1);
    chThdSleepMilliseconds(500);
    i = (i + 1) & 15;
  }
}

void adxl362_write_register (uint16_t address, uint8_t data) {
    uint8_t command = 0x0A;

    spiStart(&SPID1, &adxl362_cfg);      /* Setup transfer parameters. */
    spiSelect(&SPID1);                   /* Slave Select assertion.    */
    spiSend(&SPID1, 1, &command);        /* Write Command              */
    spiSend(&SPID1, 1, &address);        /* Address                    */
    spiSend(&SPID1, 1, &data);           /* Data                       */
    spiUnselect(&SPID1);                 /* Slave Select de-assertion. */
}

static void sendToPot(Mcp42010Driver *driver, int channel, int value) {
	lockSpi(SPI_NONE);
	spiStart(driver->spi, &driver->spiConfig);
	spiSelect(driver->spi);
	int word = (17 + channel) * 256 + value;
	spiSend(driver->spi, 1, &word);
	spiUnselect(driver->spi);
	spiStop(driver->spi);
	unlockSpi();
}

/*
 * Write a register at address `adr` with value `val` on the ADXL3x5 on SPI
 * driver `SPID`.
 */
static void adxl3x5_write_u8(SPIDriver* SPID, uint8_t adr, uint8_t val)
{
    uint8_t tx[2];
    tx[0] = adr & ~(1<<7 | 1<<6);
    tx[1] = val;

    spiSelect(SPID);
    spiSend(SPID, 2, (void*)tx);
    spiUnselect(SPID);
}

//----------------------------------------------------------------------------
uint8_t 
spiEepromReadSR(spiEepromDriver * sedp)
{
	spiStart(sedp->spip, &sedp->cfgp->spicfg);
	spiSelect(sedp->spip);
	spiPolledExchange(sedp->spip, OP_RDSR);
	uint8_t sr = spiPolledExchange(sedp->spip, 0x00);
	spiUnselect(sedp->spip);
	return sr;
}

/**
 * @brief   Sends a command which returns a five bytes response (R3).
 *
 * @param[in] mmcp      pointer to the @p MMCDriver object
 * @param[in] cmd       the command id
 * @param[in] arg       the command argument
 * @param[out] response pointer to four bytes wide uint8_t buffer
 * @return              The first byte of the response (R1) as an @p
 *                      uint8_t value.
 * @retval 0xFF         timed out.
 *
 * @notapi
 */
static uint8_t send_command_R3(MMCDriver *mmcp, uint8_t cmd, uint32_t arg,
                               uint8_t *response) {
  uint8_t r1;
  
  spiSelect(mmcp->spip);
  send_hdr(mmcp, cmd, arg);
  r1 = recvr3(mmcp, response);
  spiUnselect(mmcp->spip);
  return r1;
}

static uint16_t internal_adis16405_read_u16(uint8_t addr_in) {
    uint16_t addr_out = (uint16_t)(addr_in & 0x7F) << 8;
    uint16_t data_rx;

    spiSelect(&ADIS16405_SPID);
    spiSend(&ADIS16405_SPID, 1, (void*)&addr_out);
    spiReceive(&ADIS16405_SPID, 1, (void*)&data_rx);
    spiUnselect(&ADIS16405_SPID);
    return data_rx;
}

void gyro_write_register (uint8_t address, uint8_t data) {
  address = address & (~0x80);         /* Clear the write bit (bit 7)      */
  spiAcquireBus(&SPID1);               /* Acquire ownership of the bus.    */
  spiStart(&SPID1, &gyro_cfg);         /* Setup transfer parameters.       */
  spiSelect(&SPID1);                   /* Slave Select assertion.          */
  spiSend(&SPID1, 1, &address);        /* Send the address byte            */
  spiSend(&SPID1, 1, &data); 
  spiUnselect(&SPID1);                 /* Slave Select de-assertion.       */
  spiReleaseBus(&SPID1);               /* Ownership release.               */
}