#include <lgpio.h>
#include <stdio.h>
#include <stdbool.h>
#include <stdlib.h>

#define I2C_PICO_ADDRESS 0x31
#define FLOATS_PER_PACKET 2

void i2c_send_floats(int i2c_handle, float* data);

int main(int argNum, char* args[]) {
	int chip_handle;
	int i2c_handle;

	puts("i2c com test");

	//INITIALIZATION
	chip_handle = lgGpiochipOpen(0);
	if(chip_handle < 0) {
		puts("Error in initialization, handle is invalid");
		return 0;
	}
	i2c_handle = lgI2cOpen(1, I2C_PICO_ADDRESS, 0);
	if( i2c_handle <= 0){
		puts("Error opening I2C Bus, handle is invalid");
		return 0;
	}
	if(argNum  < 3) {
		printf("correct syntax:\n   %s <on-delay-seconds> <off-delay-seconds>\n", args[0]);
		return 0;
	}

	float data[] = {atof(args[1]), atof(args[2])};
	printf("sending on_delay = %.6f   off_delay = %.6f\n", data[0], data[1]);
	i2c_send_floats(i2c_handle, data);

	return 0;
}


void i2c_send_floats(int i2c_handle, float* data) {
	unsigned int dataSize = sizeof(float)* FLOATS_PER_PACKET;
	lgI2cWriteDevice(i2c_handle,(char*)data, dataSize);
}

#include "pico/stdlib.h"
#include "hardware/i2c.h"

#define I2C_LINK_SDA_GP 16
#define I2C_LINK_SCL_GP 17
#define I2C_LINK_MODULE i2c0
#define I2C_SELF_ADDRESS 0x31
#define I2C_LINK_DATA_RATE 4000000

#define FLOATS_PER_PACKET 2

void i2c_link_init();
void i2c_link_read_floats_blocking(float data[FLOATS_PER_PACKET]);

#include "i2c-link.h"

void i2c_link_init(){
  gpio_pull_up(I2C_LINK_SDA_GP);
  gpio_pull_up(I2C_LINK_SCL_GP);
  i2c_init(I2C_LINK_MODULE, I2C_LINK_DATA_RATE);
  i2c_set_slave_mode(I2C_LINK_MODULE, true, I2C_SELF_ADDRESS);
  gpio_set_function(I2C_LINK_SDA_GP, GPIO_FUNC_I2C);
  gpio_set_function(I2C_LINK_SCL_GP, GPIO_FUNC_I2C);
}

void i2c_link_read_floats_blocking(float data[FLOATS_PER_PACKET]){
  float value;
  for(int i = 0; i < FLOATS_PER_PACKET; i++){
    uint8_t buffer[sizeof(float)];
    i2c_read_raw_blocking(I2C_LINK_MODULE, buffer, sizeof(float));
    value = 0;
    for(int k = 0; k < sizeof(float); k++){
      *(int*)&value |= ((int)buffer[k] << (8*k));
    }
    data[i] = value;
  }
}

#include "pico/stdlib.h"
#include "i2c-link.h"
#define LED_ONB 25

bool led_onb_state = false;
int64_t led_on_micros = 50000;
int64_t led_off_micros = 50000;

int64_t alarm_callback_blink(alarm_id_t id, void *user_data);

int main() {
	// setup &  initialize
	i2c_link_init();
	float rx_buffer[FLOATS_PER_PACKET];
	//onboard led
	gpio_init(LED_ONB);
	gpio_set_dir(LED_ONB, GPIO_OUT);
	add_alarm_in_ms(1000, alarm_callback_blink, NULL, true);
	// primary loop
		while (true) {
			i2c_link_read_floats_blocking(rx_buffer);
			if(rx_buffer[0] >= 0.000005f){
				led_on_micros  = (int64_t) (rx_buffer[0] * 1e6f);
			} else led_on_micros = 5;
			if(rx_buffer[1] >= 0.000005f){
				led_off_micros = (int64_t) (rx_buffer[1] * 1e6f);
			} else led_off_micros = 5;
		}
	}

int64_t alarm_callback_blink(alarm_id_t id, void *user_data) {
	led_onb_state = !led_onb_state;
	gpio_put(LED_ONB, led_onb_state);
	if(led_onb_state == true)	return led_on_micros;
	else return led_off_micros;
}