#define THERMOMETER_ADDR    0x5B     // thermometer I2C address
#define GET_RAM_OBJECT_TEMP 0        // get object temperature from RAM command
#define TWI_READ_BIT        (0x01)   // If this bit set in address field, transfer direction from slave to master

//---------------
// Main Function
//---------------
int main(void)
{
  twi_master_init();
  gpio_config();                // configure LED0,LED1,SCL,SDA as outputs
  
  NVIC_EnableIRQ(TIMER0_IRQn);  // Enable timer interrupt
  __enable_irq();

  nrf_gpio_pin_set(TWI_MASTER_CONFIG_POWER_PIN_NUMBER);     // turn on sensor power (sensor powered by IO pin)
  for (i = 0; i < 10000; i++);                              // wait for power to settle

  while(1)
  {
    send_twi_command(THERMOMETER_ADDR, GET_RAM_OBJECT_TEMP);  // send command to twi (get 4-byte data from slave)
  }
}

//-----------------------
// Initialize TWI Master
//-----------------------
bool twi_master_init(void)
{
    NRF_GPIO->PIN_CNF[TWI_MASTER_CONFIG_CLOCK_PIN_NUMBER] = 
        (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos)
      | (GPIO_PIN_CNF_DRIVE_S0D1     << GPIO_PIN_CNF_DRIVE_Pos)
      | (GPIO_PIN_CNF_PULL_Pullup    << GPIO_PIN_CNF_PULL_Pos)
      | (GPIO_PIN_CNF_INPUT_Connect  << GPIO_PIN_CNF_INPUT_Pos)
      | (GPIO_PIN_CNF_DIR_Input      << GPIO_PIN_CNF_DIR_Pos);    

    NRF_GPIO->PIN_CNF[TWI_MASTER_CONFIG_DATA_PIN_NUMBER] = 
        (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos)
      | (GPIO_PIN_CNF_DRIVE_S0D1     << GPIO_PIN_CNF_DRIVE_Pos)
      | (GPIO_PIN_CNF_PULL_Pullup    << GPIO_PIN_CNF_PULL_Pos)
      | (GPIO_PIN_CNF_INPUT_Connect  << GPIO_PIN_CNF_INPUT_Pos)
      | (GPIO_PIN_CNF_DIR_Input      << GPIO_PIN_CNF_DIR_Pos);    

    NRF_TWI1->EVENTS_RXDREADY = 0;
    NRF_TWI1->EVENTS_TXDSENT = 0;
    NRF_TWI1->PSELSCL = TWI_MASTER_CONFIG_CLOCK_PIN_NUMBER;
    NRF_TWI1->PSELSDA = TWI_MASTER_CONFIG_DATA_PIN_NUMBER;
    NRF_TWI1->FREQUENCY = TWI_FREQUENCY_FREQUENCY_K100 << TWI_FREQUENCY_FREQUENCY_Pos;
    NRF_PPI->CH[0].EEP = (uint32_t)&NRF_TWI1->EVENTS_BB;
    NRF_PPI->CH[0].TEP = (uint32_t)&NRF_TWI1->TASKS_SUSPEND;
    NRF_PPI->CHENCLR = PPI_CHENCLR_CH0_Msk;
    NRF_TWI1->ENABLE = TWI_ENABLE_ENABLE_Enabled << TWI_ENABLE_ENABLE_Pos;

    return twi_master_clear_bus();
}

//------------------------
// Configure GPIO for TWI
//------------------------
static void gpio_config(void)
{
  nrf_gpio_pin_dir_set(EVAL_BOARD_LED_0, NRF_GPIO_PIN_DIR_OUTPUT);
  nrf_gpio_pin_dir_set(EVAL_BOARD_LED_1, NRF_GPIO_PIN_DIR_OUTPUT);

  nrf_gpio_pin_dir_set(TWI_MASTER_CONFIG_POWER_PIN_NUMBER, NRF_GPIO_PIN_DIR_OUTPUT);
  nrf_gpio_pin_dir_set(TWI_MASTER_CONFIG_CLOCK_PIN_NUMBER, NRF_GPIO_PIN_DIR_OUTPUT);
  nrf_gpio_pin_dir_set(TWI_MASTER_CONFIG_DATA_PIN_NUMBER, NRF_GPIO_PIN_DIR_OUTPUT);
}

//---------------------
// Send command to TWI
//---------------------
bool send_twi_command(uint8_t address, uint8_t command)
{              //+++ ADD SWITCH COMMAND HERE TO PARSE COMMANDS
  if (!twi_master_transfer(address << 1, data_buffer, 1, TWI_ISSUE_STOP))  // send slave address, command
    return false;                                                        // shift address to make room for TWI_READ_BIT
  return twi_master_transfer(address << 1 | TWI_READ_BIT, data_buffer, 4, TWI_ISSUE_STOP);  // get 4-byte data
}

//---------------------
// TWI Master Transfer
//---------------------
bool twi_master_transfer(uint8_t address, uint8_t *data, uint8_t data_length, bool issue_stop_condition)
{
    bool transfer_succeeded = true;

    if (data_length > 0 && twi_master_clear_bus())
    {
      if (data_length > 0)
      {
          NRF_TWI1->ADDRESS = (address >> 1);     // reset address (remove TWI_READ_BIT)

          if ((address & TWI_READ_BIT))
              transfer_succeeded = twi_master_read(data, data_length, issue_stop_condition);
          else
              transfer_succeeded = twi_master_write(data, data_length, issue_stop_condition);
      }
    }
    return transfer_succeeded;
}

//------------------
// TWI Master Write
//------------------
static bool twi_master_write(uint8_t *data, uint8_t data_length, bool issue_stop_condition)
{
    uint32_t timeout = MAX_TIMEOUT_LOOPS;   // max loops to wait for EVENTS_TXDSENT event

    if (data_length == 0)
        return false;   // gently return false for requesting data of size 0

    NRF_TWI1->TXD = *data++;
    NRF_TWI1->TASKS_STARTTX = 1;

    while (true)
    {
      while(NRF_TWI1->EVENTS_TXDSENT == 0 && (--timeout));
      
        if (timeout == 0)
        {
            NRF_TWI1->EVENTS_STOPPED = 0; 
            NRF_TWI1->TASKS_STOP = 1; 

            while(NRF_TWI1->EVENTS_STOPPED == 0);  // wait until stop sequence is sent and clear the EVENTS_STOPPED
            /* timeout before receiving event*/
            return false;
        }

        NRF_TWI1->EVENTS_TXDSENT = 0;
        if (--data_length == 0)
            break;

        NRF_TWI1->TXD = *data++;
    }
    
    if (issue_stop_condition) 
    { 
        NRF_TWI1->EVENTS_STOPPED = 0; 
        NRF_TWI1->TASKS_STOP = 1; 
        while(NRF_TWI1->EVENTS_STOPPED == 0); // wait until stop sequence is sent and clear the EVENTS_STOPPED
    }                                         //+++ HANGS HERE
    return true;
}

//-----------------
// TWI Master Read
//-----------------
static bool twi_master_read(uint8_t *data, uint8_t data_length, bool issue_stop_condition)
{
    uint32_t timeout = MAX_TIMEOUT_LOOPS;   /* max loops to wait for RXDREADY event*/

    if(data_length == 0)
        return false;   // gently return false for requesting data of size 0


    if (data_length == 1)
        NRF_PPI->CH[0].TEP = (uint32_t)&NRF_TWI1->TASKS_STOP;
    else
        NRF_PPI->CH[0].TEP = (uint32_t)&NRF_TWI1->TASKS_SUSPEND;

    NRF_PPI->CHENSET = PPI_CHENSET_CH0_Msk;
    NRF_TWI1->TASKS_STARTRX = 1;
    while(true)
    {
        while((NRF_TWI1->EVENTS_RXDREADY == 0) && (--timeout));

        if(timeout == 0)
            return false;  // timeout before receiving event

        NRF_TWI1->EVENTS_RXDREADY = 0;
        *data++ = NRF_TWI1->RXD;

        if (--data_length == 1)   // configure PPI to stop TWI master before we get last BB event
            NRF_PPI->CH[0].TEP = (uint32_t)&NRF_TWI1->TASKS_STOP;

        if (data_length == 0)         // all data read
            break;

        NRF_TWI1->TASKS_RESUME = 1;
    }

    while(NRF_TWI1->EVENTS_STOPPED == 0)  // wait until stop sequence is sent and clear the EVENTS_STOPPED
    {
    }
    NRF_TWI1->EVENTS_STOPPED = 0;

    NRF_PPI->CHENCLR = PPI_CHENCLR_CH0_Msk;
    return true;
}