#include "kilolib.h"

message_t msg;
uint32_t next_flash_time;

#define FLASH_PERIOD 30
#define FLASH_LENGTH 5

void setup()
{
  next_flash_time = kilo_ticks + (rand_hard() % FLASH_PERIOD);
  // initialize message
  msg.type = NORMAL;
}

void loop()
{
  if (kilo_ticks > next_flash_time)
    next_flash_time = kilo_ticks + FLASH_PERIOD;

  if (kilo_ticks + FLASH_PERIOD < next_flash_time + FLASH_LENGTH)
    set_color(RGB(1,1,1));
  //    set_color(RGB(1,0,0));
  else
    set_color(RGB(0,0,0));
}

message_t *transmit_message()
{
  // Send ticks until next flash.
  msg.data[0] = (next_flash_time >= kilo_ticks ? next_flash_time - kilo_ticks : 0);
  // msg.data[0] = (rand_soft() % FLASH_PERIOD);  // See how bad signals disrupt synchronization
  msg.crc = message_crc(&msg);
  return &msg;
}

void receive_message(message_t *m, distance_measurement_t *d)
{
  uint32_t t = kilo_ticks + m->data[0];
  if (next_flash_time < t && next_flash_time + FLASH_PERIOD/2 > t)
    next_flash_time += 1;
  else if (next_flash_time > t && next_flash_time < t + FLASH_PERIOD/2)
    next_flash_time -= 1;
}

int main()
{
    kilo_init();
    kilo_message_tx = transmit_message;
    kilo_message_rx = receive_message;
    kilo_start(setup, loop);

    return 0;
}