/**
 ****************************************************************************************
 * @brief Read n byte data form i2c device
 * @param[in]  saddr         slave device address(7bits, without R/W bit)
 * @param[in]  reg_addr      device register address
 * @param[in]  buffer        Pointer to read data buffer
 * @param[in]  len           read data length
 * @description
 * Read n byte data from slave device, read start address is 16 bits and the data will be
 * stored in buffer, n is the specified length
 *****************************************************************************************
 */
void I2C_nBYTE_READ2(uint8_t saddr, uint16_t reg_addr, uint8_t *buffer, uint16_t len)
{
    uint32_t i;

    //Store environment parameters
    i2c_env.i2cOpFsm = I2C_OP_IDLE;
    i2c_env.i2cIndex = 0;
    i2c_env.i2cTxCount = 2;
    i2c_env.i2cRxCount = len;
    i2c_env.i2cBuffer[0] = (reg_addr >> 8) & 0xFF;
    i2c_env.i2cBuffer[1] = reg_addr & 0xFF;
    for (i = 0; i < len; i++) {
        i2c_env.i2cBuffer[i2c_env.i2cTxCount + i] = 0;  // clear result buffer
    }
#if I2C_CALLBACK_EN==TRUE
    i2c_env.callback = NULL;
#endif

    i2c_read(saddr);
    for (i = 0; i < len; i++) {
        buffer[i] = i2c_env.i2cBuffer[i2c_env.i2cTxCount + i];
    }
}

static int w83782d_hwmon_init(void)
{
	u8 buf;

	if (i2c_read(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_CFG, 1, &buf, 1))
		return -1;

	i2c_reg_write(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_CFG, 0x80);
	i2c_reg_write(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_BANK_SEL, 0);
	i2c_reg_write(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_ADCCLK, 0x40);

	buf = i2c_reg_read(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_BEEP_CTRL);
	i2c_reg_write(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_BEEP_CTRL,
		      buf | 0x80);
	i2c_reg_write(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_BEEP_CTRL2, 0);
	i2c_reg_write(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_PWMOUT1, 0x47);
	i2c_reg_write(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_VBAT, 0x01);

	buf = i2c_reg_read(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_CFG);
	i2c_reg_write(CONFIG_SYS_I2C_W83782G_ADDR, W83782D_REG_CFG,
		      (buf & 0xf4) | 0x01);
	return 0;
}

unsigned int8 RTCRead(unsigned int8 address)
{
    int1 nack;
    unsigned int8 value = 0;

	i2c_start();
    nack = i2c_write(PCF8583_WRITE);
    if (nack) {
     i2c_stop();
     printf("rtcread nack 1\n\r");
    } else {
     nack = i2c_write(address);
     if (!nack) {
      i2c_start();
      nack = i2c_write(PCF8583_READ);
      if (!nack) {
       value  = i2c_read();
      } else      printf("rtcread nack 3\n\r");
     } else      printf("rtcread nack 2\n\r");
     i2c_stop();
    }
    return value;
}

int main(void)
{
  volatile uint8_t data;

  NRF_TWIM0->PSEL.SCL = PIN_SCL;
  NRF_TWIM0->PSEL.SDA = PIN_SDA;

  NRF_TWIM0->ADDRESS = DEVICE_ADDRESS;
  NRF_TWIM0->FREQUENCY = TWIM_FREQUENCY_FREQUENCY_K400 << TWIM_FREQUENCY_FREQUENCY_Pos;
  NRF_TWIM0->SHORTS = 0;

  NRF_TWIM0->ENABLE = TWIM_ENABLE_ENABLE_Enabled << TWIM_ENABLE_ENABLE_Pos;

  i2c_write(0x0, 0xFF);

  data = i2c_read(0x0);
  data; // to get rid of set but unused warning

  while (1)
  {
    __WFE();
  }
}

/***************************************************************************//**
 * @brief Reads the value of a register.
 *
 * @param dev              - The device structure.
 * @param register_address - Address of the register.
 * @param bytes_number     - Number of bytes.
 *
 * @return registerValue  - Value of the register.
*******************************************************************************/
uint32_t ad5933_get_register_value(struct ad5933_dev *dev,
				   uint8_t register_address,
				   uint8_t bytes_number)
{
	uint32_t register_value = 0;
	uint8_t byte = 0;
	uint8_t write_data[2]   = {0, 0};
	uint8_t read_data[2]    = {0, 0};

	for(byte = 0; byte < bytes_number; byte ++) {
		/* Set the register pointer. */
		write_data[0] = AD5933_ADDR_POINTER;
		write_data[1] = register_address + byte;
		i2c_write(dev->i2c_desc, write_data, 2, 1);
		/* Read Register Data. */
		read_data[0] = 0xFF;
		i2c_read(dev->i2c_desc, read_data, 1, 1);
		register_value = register_value << 8;
		register_value += read_data[0];
	}

	return register_value;
}

static int tps6586x_read(int reg)
{
	int	i;
	uchar	data;
	int	retval = -1;

	for (i = 0; i < MAX_I2C_RETRY; ++i) {
		if (!i2c_read(tps6586x_dev, reg,  &data, 1)) {
			retval = (int)data;
			goto exit;
		}

		/* i2c access failed, retry */
		udelay(100);
	}

exit:
	debug("pmu_read %x=%x\n", reg, retval);
	if (retval < 0)
		debug("%s: failed to read register %#x: %d\n", __func__, reg,
		      retval);
	return retval;
}

uint8_t i2c_ReadBuffer(uint8_t nAddress, uint8_t *pByte, uint8_t nBufSize, uint8_t *pLen)
{
    uint8_t stat = 0;

    *pLen = 0;

    stat = i2c_start( nAddress, TW_READ );

    while (!stat && nBufSize) {
        
        stat = i2c_read( pByte, nBufSize != 1 );

        if ( !stat || stat == TW_MR_DATA_NACK) {
            nBufSize--;
            pByte++;
            (*pLen)++;
            stat = 0;
        }
    }
    i2c_stop();

    return stat;
}

bool I2C_MultipleRead(alt_u32 clk_base, alt_u32 data_base, alt_8 DeviceAddr, alt_u8 szData[], alt_u16 len){
    int i;
    bool bSuccess = TRUE;
    //alt_u8 DeviceAddr, 
    alt_u8 ControlAddr = 0;
    
   
    // device id
    //DeviceAddr = HMB_E2_I2C_ID;

    i2c_start(clk_base, data_base);
    if (!i2c_write(clk_base, data_base, DeviceAddr)){  // send ID
        bSuccess = FALSE;
        I2C_DEBUG(("I2C HMB_E2 Fail: Address NACK!\n"));
    }
    if (bSuccess && !i2c_write(clk_base, data_base, ControlAddr)){ // send sub-address
        bSuccess = FALSE;
        I2C_DEBUG(("I2C HMB_E2 Fail: SubAddress NACK!\n"));
    }    
    if (bSuccess)        
        i2c_start(clk_base, data_base);  // restart
    DeviceAddr |= 1; // Read
    if (bSuccess && !i2c_write(clk_base, data_base, DeviceAddr)){  // send id
        bSuccess = FALSE;
        I2C_DEBUG(("I2C HMB_E2 Fail: Address+1 NACK!\n"));
    }
    
    if (bSuccess){
        for(i=0;i<len && bSuccess;i++){
            i2c_read(clk_base, data_base, &szData[i], (i==(len-1))?FALSE:TRUE);  // read
        }            
    }        
    i2c_stop(clk_base, data_base);
    
    return bSuccess;    
    
}

int altitude_changes_lookup(i2c_can_trans_t* trans, long* nextAlt) {
	short i;
   char dataBuf[8];
   char plop=0x00;
   short test;
   unsigned char hasChanged;
   unsigned int oldPosition;
   unsigned int newPosition;
   int difference;

   hasChanged=0;

   i2c_write(trans->i2cNodes[0].nodeAddr, &plop, 1);
   test=i2c_read(trans->i2cNodes[0].nodeAddr, dataBuf, trans->i2cNodes[0].dataLength);
   if(test != 0) {
   // If there is no new value, copy the old one.
   	memcpy(dataBuf, trans->oldData,
      trans->i2cNodes[0].dataLength);
   }

   hasChanged = memcmp(dataBuf, trans->oldData, trans->dataLength);

   if(hasChanged!=0){
     //	printf("has changed = %d \nread new value=  0x%4x\n",hasChanged,*(unsigned int*)dataBuf);
      oldPosition=((unsigned int)(trans->oldData[1])&0x00FF)|(((unsigned int)(trans->oldData[0])<<8)&0xFF00);
      newPosition=((unsigned int)(dataBuf[1])&0x00FF)|(((unsigned int)(dataBuf[0])<<8)&0xFF00);

      difference=newPosition-oldPosition;
      *nextAlt=*nextAlt+((long)(30.4799*difference));
      if(*nextAlt<0) *nextAlt=0;
      printf("\naltitude=%lu\n", (*nextAlt)*3.28084);
   }

	memcpy(trans->oldData, dataBuf, trans->dataLength);
   return (hasChanged != 0 ? 0 : -1);
}

int16_t ds1338_ram_read(uint16_t addr,uint8_t * data,int16_t len)
{
	if(addr >= DS1338_RAM_SIZE || len < 0)
	{
		return -1; 
	}
	
	int16_t ret = 0;
	
	if(addr + len > DS1338_RAM_SIZE)
	{
		len = DS1338_RAM_SIZE - addr;
		ret = len;
	}
	
	addr = DS1338_RAM_OFFSET + (addr);
	
	uint8_t a = (uint8_t)(addr&0xff);
	
	i2c_write(DS1338_I2C_ADDR,&a,1);
	i2c_read(DS1338_I2C_ADDR,data,len);
	
	return ret;
}

int I2C_DATA_READ(uint8_t saddr,uint8_t *buffer, uint16_t len)
{
    int ret = 0;
		uint32_t i;

    //Store environment parameters
    i2c_env.i2cOpFsm = I2C_OP_IDLE;
    i2c_env.i2cIndex = 0;
    i2c_env.i2cTxCount = 0;
    i2c_env.i2cRxCount = len;
    //i2c_env.i2cBuffer[0] = reg_addr;
    for (i = 0; i < len; i++) {
        i2c_env.i2cBuffer[i2c_env.i2cTxCount + i] = 0;  // clear result buffer
    }
#if I2C_CALLBACK_EN==TRUE
    i2c_env.callback = NULL;
#endif

    ret = i2c_read(saddr);
    for (i = 0; i < len; i++) {
        buffer[i] = i2c_env.i2cBuffer[i2c_env.i2cTxCount + i];
    }
		return ret;
}

static u32 omap5evm_get_board_rev(void)
{
	u32 ret = 0;
	hal_i2c i2c_id = HAL_I2C1;

	u32 clk32;
	u16 slave;
	u16 reg_addr;
	u16 cmd[7];

	ret = i2c_init(i2c_id);
	if (ret != 0) {
		printf("Failed to init I2C-%d\n", i2c_id);
		return ret;
	}

	slave = 0x50; reg_addr = 0x8;
	cmd[0] = (reg_addr & 0xFF);
	clk32 = readl(CLK32K_COUNTER_REGISTER);
	ret = i2c_read(i2c_id, slave, 12, &cmd[0], clk32, 0xFF);
	if (ret != 0) {
		printf("I2C read failed, ret = %d\n", ret);
		return ret;
	}

	ret = i2c_close(i2c_id);
	if (ret != 0) {
		printf("i2c close for bus %d failed, ret = %d\n",
							i2c_id, ret);
		return ret;
	}

	ret  = crc_board_rev((u8 *) cmd);

	return ret;
}

int cmd_i2cread(int ac, char *av[])
{
	int i, len = 1;
	unsigned char addr[16] = { 0xa3, 0x02, 0x02, 0x02, 0x02, 0x02 };
	unsigned char slave = 0xa3;
	unsigned char regoff = 0x02;
	unsigned char buf[16];

	for (i = 0; i < ac; i++)
		printf("av[%d] : %s\n", i, av[i]);

	slave = strtoul(av[1], NULL, 0);
	printf("slave = %08x\n ", slave);
	if ((slave & 0x1) == 0)	// Fix bad input
		slave = 0xa3;

	if (ac > 2)
		regoff = strtol(av[2], NULL, 0);
	printf("regoff = %08x\n", regoff);

	printf("%08x\n", i2c_read(slave, regoff));

	return 0;
}

/*
 * Routine: get_board_revision
 * Description: Returns the board revision
 */
int get_board_revision(void)
{
	int revision;

#ifdef CONFIG_DRIVER_OMAP34XX_I2C
	unsigned char data;

	/* board revisions <= R2410 connect 4030 irq_1 to gpio112             */
	/* these boards should return a revision number of 0                  */
	/* the code below forces a 4030 RTC irq to ensure that gpio112 is low */
	i2c_set_bus_num(TWL4030_I2C_BUS);
	data = 0x01;
	i2c_write(0x4B, 0x29, 1, &data, 1);
	data = 0x0c;
	i2c_write(0x4B, 0x2b, 1, &data, 1);
	i2c_read(0x4B, 0x2a, 1, &data, 1);
#endif

	if (!gpio_request(112, "") &&
	    !gpio_request(113, "") &&
	    !gpio_request(115, "")) {

		gpio_direction_input(112);
		gpio_direction_input(113);
		gpio_direction_input(115);

		revision = gpio_get_value(115) << 2 |
			   gpio_get_value(113) << 1 |
			   gpio_get_value(112);
	} else {
		puts("Error: unable to acquire board revision GPIOs\n");
		revision = -1;
	}

	return revision;
}

static int bq2425x_getreg8(FAR struct bq2425x_dev_s *priv, uint8_t regaddr,
                           FAR uint8_t *regval)
{
  struct i2c_config_s config;
  uint8_t val;
  int ret;

  /* Set up the I2C configuration */

  config.frequency = priv->frequency;
  config.address   = priv->addr;
  config.addrlen   = 7;

  /* Write the register address */

  ret = i2c_write(priv->i2c, &config, &regaddr, 1);
  if (ret < 0)
    {
      batdbg("i2c_write failed: %d\n", ret);
      return ret;
    }

  /* Restart and read 8-bits from the register */

  ret = i2c_read(priv->i2c, &config, &val, 1);
  if (ret < 0)
    {
      batdbg("i2c_read failed: %d\n", ret);
      return ret;
    }

  /* Copy 8-bit value to be returned */

  *regval = val;
  return OK;
}

/*
 * Modify masked bits in register
 */
static int tca642x_reg_write(uchar chip, uint8_t addr,
		uint8_t reg_bit, uint8_t data)
{
	uint8_t valw;
	int org_bus_num;
	int ret;

	org_bus_num = i2c_get_bus_num();
	i2c_set_bus_num(CONFIG_SYS_I2C_TCA642X_BUS_NUM);

	if (i2c_read(chip, addr, 1, (uint8_t *)&valw, 1)) {
		printf("Could not read before writing\n");
		ret = -1;
		goto error;
	}
	valw &= ~reg_bit;
	valw |= data;

	ret = i2c_write(chip, addr, 1, (u8 *)&valw, 1);

error:
	i2c_set_bus_num(org_bus_num);
	return ret;
}

int platform_i2c_transfer(unsigned bus, struct i2c_seg *segments, int seg_count)
{
	struct i2c_seg *seg = segments;
	int ret = 0;

	if (i2c_init(bus))
		return 1;

	while (!ret && seg_count--) {
		if (seg->read)
			ret = i2c_read(bus, seg->chip, seg->buf, seg->len);
		else
			ret = i2c_write(bus, seg->chip, seg->buf, seg->len,
					(seg_count ? 0 : 1));
		seg++;
	}

	if (ret) {
		qup_set_state(bus, QUP_STATE_RESET);
		return 1;
	}

	return 0;
}

static void send_command_to_pic(const unsigned char in, unsigned char* out,
                                const unsigned int length)
{
    /* Disable GIO0 interrupt */
    IO_INTC_EINT1 &= ~INTR_EINT1_EXT0;
    /* Clear EXT0 interrupt */
    IO_INTC_IRQ1 = INTR_IRQ1_EXT0;
    /* Write command to I²C */
    restart:
    i2c_write(PIC_ADR, &in, 1);
    /* Wait for PIC */
    int i = 0;
    while(!(IO_INTC_IRQ1 & INTR_IRQ1_EXT0))
    {
        sleep(0);
        i++;
        if(i > 50)
            goto restart;
    }
    /* Read return from I²C */
    i2c_read(PIC_ADR, out, length);
    /* Re-enable GIO0 interrupt */
    IO_INTC_EINT1 |= INTR_EINT1_EXT0;
}

void read_1307()
{
    // set read address
    i2c_start();
    i2c_write(0b11010000);
    i2c_write(0x0);
    i2c_stop();


    // read first 8 bytes of eprom
    i2c_start();
    
    i2c_write(0b11010001);
    i2c_read();
    i2c_read();
    i2c_read();
    i2c_read();

    i2c_read();
    i2c_read();
    i2c_read();
    i2c_read_with_nack();
    i2c_stop();
}

void GyroReadData(int *gyroX, int *gyroY, int *gyroZ) {

    i2c_write(GYRO_SLAVE_ADDRESS, 0x1D);
    gyroX = i2c_read(GYRO_SLAVE_ADDRESS);

    i2c_write(GYRO_SLAVE_ADDRESS, 0x1E);
    gyroX = (gyroX<<8)|i2c_read(GYRO_SLAVE_ADDRESS);

    i2c_write(GYRO_SLAVE_ADDRESS, 0x1F);
    gyroY = i2c_read(GYRO_SLAVE_ADDRESS);

    i2c_write(GYRO_SLAVE_ADDRESS, 0x20);
    gyroY = (gyroY<<8)|i2c_read(GYRO_SLAVE_ADDRESS);

    i2c_write(GYRO_SLAVE_ADDRESS, 0x21);
    gyroZ = i2c_read(GYRO_SLAVE_ADDRESS);

    i2c_write(GYRO_SLAVE_ADDRESS, 0x22);
    gyroZ = (gyroZ<<8)|i2c_read(GYRO_SLAVE_ADDRESS);

}