#include <Arduino.h>
#include <micro_ros_platformio.h>

#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/int32.h>

#include <Wire.h>


rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

// ADAM CODE

extern "C"
{
  bool keplie_transport_i2c_open(struct uxrCustomTransport * transport);
  bool keplie_transport_i2c_close(struct uxrCustomTransport * transport);
  size_t kelpie_transport_i2c_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
  size_t kelpie_transport_ic2_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);

  typedef struct
  {
    uint8_t masterAddress;
    uint8_t myAddress;
  } kelpie_i2c_args;

}

// ADAM CODE

// Error handle loop
void error_loop() {
  while(1) {
    delay(100);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
  }
}

void setup() {
  // Configure serial transport
  Serial.begin(115200);
  //set_microros_serial_transports(Serial);

  // ADAM CODE
  kelpie_i2c_args args = {0x0, 0x1};
  uxrCustomTransport transport;

  rmw_uros_set_custom_transport(
    true,
    &args,
    keplie_transport_i2c_open,
    keplie_transport_i2c_close,
    kelpie_transport_i2c_write,
    kelpie_transport_ic2_read
  );

  uxr_init_custom_transport(&transport, (void *) &args);
  // ADAM CODE

  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_platformio_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_platformio_node_publisher"));

  // create timer,
  const unsigned int timer_timeout = 1000;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  msg.data = 0;
}

void loop() {
  delay(100);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

// ADAM CODE
bool keplie_transport_i2c_open(struct uxrCustomTransport * transport)
{
  kelpie_i2c_args *args = (kelpie_i2c_args*)(transport->args);
  Wire.begin(args->myAddress);
  return true;
}

bool keplie_transport_i2c_close(struct uxrCustomTransport * transport)
{
  Wire.end();
  return true;
}

size_t kelpie_transport_i2c_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err)
{
  kelpie_i2c_args *args = (kelpie_i2c_args*)(transport->args);
  size_t bytesWritten;
  Wire.beginTransmission(args->masterAddress);
  bytesWritten = Wire.write(buf, len);
  Wire.endTransmission();

  return bytesWritten;
}

size_t kelpie_transport_ic2_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err)
{
  kelpie_i2c_args *args = (kelpie_i2c_args*)(transport->args);
  //Wire.requestFrom(args->masterAddress, len); pico isnt the master on the bus, it shouldn't be requesting
  int i = 0;
  while(Wire.available())
  {
    buf[i] = Wire.read();
    i++;
  }

  return i;
}

// ADAM CODE