#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/f1/i2c.h>

#include <libLHP_I2C.h>

void setup_I2C1_GPIO(){

  gpio_set_mode(GPIOB,
		GPIO_MODE_OUTPUT_50_MHZ,
		GPIO_CNF_OUTPUT_ALTFN_OPENDRAIN,
		GPIO_I2C1_SCL | GPIO_I2C1_SDA
		);
  
}

void setup_I2C2_GPIO(){

  gpio_set_mode(GPIOB,
		GPIO_MODE_OUTPUT_50_MHZ,
		GPIO_CNF_OUTPUT_ALTFN_OPENDRAIN,
		GPIO_I2C2_SCL | GPIO_I2C2_SDA
		);
  
}

void setup_I2C1(){
  rcc_periph_clock_disable(RCC_I2C1);
  rcc_periph_clock_enable(RCC_I2C1);
  rcc_periph_clock_enable(RCC_GPIOB);
  setup_I2C1_GPIO();
  i2c_reset(I2C1);
  i2c_peripheral_disable(I2C1);
  //configure ANFOFF DNF[3:0] in CR1
  i2c_set_clock_frequency(I2C1,I2C_CR2_FREQ_2MHZ);
  i2c_set_dutycycle(I2C1,I2C_CCR_DUTY_DIV2);
  i2c_set_speed(I2C1, i2c_speed_sm_100k, 8);
  i2c_peripheral_enable(I2C1);
  // enable interrupts
  
}

void setup_I2C2(){
  rcc_periph_clock_disable(RCC_I2C2);
  rcc_periph_clock_enable(RCC_I2C2);
  rcc_periph_clock_enable(RCC_GPIOB);
  setup_I2C2_GPIO();
  i2c_reset(I2C2);
  i2c_peripheral_disable(I2C2);
  //configure ANFOFF DNF[3:0] in CR1
  i2c_set_clock_frequency(I2C2,I2C_CR2_FREQ_2MHZ);
  i2c_set_dutycycle(I2C2,I2C_CCR_DUTY_DIV2);
  i2c_set_speed(I2C2, i2c_speed_sm_100k, 8);
  i2c_peripheral_enable(I2C2);
  // enable interrupts
  
}

void i2c_setup_slave(uint32_t i2c,uint32_t addr){
  i2c_peripheral_disable(i2c);
  i2c_set_own_7bit_slave_address(i2c,addr);
  if(i2c == I2C1)
    nvic_enable_irq(NVIC_I2C1_EV_IRQ);
  else
    nvic_enable_irq(NVIC_I2C2_EV_IRQ);
  i2c_enable_interrupt(i2c, I2C_CR2_ITEVTEN);
 
  i2c_peripheral_enable(i2c);
  i2c_enable_ack(i2c);
}
	
void i2c_read7(uint32_t i2c, int addr, uint8_t *res, size_t n)
 {
         i2c_send_start(i2c);
         i2c_enable_ack(i2c);
 
         /* Wait for master mode selected */
         while (!((I2C_SR1(i2c) & I2C_SR1_SB)
                 & (I2C_SR2(i2c) & (I2C_SR2_MSL | I2C_SR2_BUSY))));
 
         i2c_send_7bit_address(i2c, addr, I2C_READ);
 
         /* Waiting for address is transferred. */
         while (!(I2C_SR1(i2c) & I2C_SR1_ADDR));
         /* Clearing ADDR condition sequence. */
         (void)I2C_SR2(i2c);
 
         for (size_t i = 0; i < n; ++i) {
                 if (i == n - 1) {
                         i2c_disable_ack(i2c);
                 }
                 while (!(I2C_SR1(i2c) & I2C_SR1_RxNE));
                 res[i] = i2c_get_data(i2c);
         }
         
 
         return;
 }

void i2c_write7(uint32_t i2c, int addr, uint8_t *res, size_t n){
  i2c_send_start(i2c);
  i2c_enable_ack(i2c);
  /* Wait for master mode selected */
         while (!((I2C_SR1(i2c) & I2C_SR1_SB)
                 & (I2C_SR2(i2c) & (I2C_SR2_MSL | I2C_SR2_BUSY))));
 
         i2c_send_7bit_address(i2c, addr, I2C_WRITE);
	 while (!(I2C_SR1(i2c) & I2C_SR1_ADDR));
	 (void)I2C_SR2(i2c);
	 for (size_t i = 0; i < n; i++) {
		i2c_send_data(i2c, res[i]);
		while (!(I2C_SR1(i2c) & (I2C_SR1_BTF)));
	}
}
