#include <webots/robot.h>
#include <webots/supervisor.h>

#include <math.h>

#define TIME_STEP 16
#define FREQ_0 0.20
#define FREQ_1 2.0

int main(int argc, const char* argv[])
{
  wb_robot_init();

  // Get light nodes
  WbNodeRef light_node[2];
  light_node[0] = wb_supervisor_node_get_from_def("light_0");
  light_node[1] = wb_supervisor_node_get_from_def("light_1");

  // Get intensity fields
  WbFieldRef light_intensity_field[2];
  light_intensity_field[0] = wb_supervisor_node_get_field(light_node[0], "intensity");
  light_intensity_field[1] = wb_supervisor_node_get_field(light_node[1], "intensity");

  // Get maximum intensity
  double max_intensity[2];
  max_intensity[0] = wb_supervisor_field_get_sf_float(light_intensity_field[0]);
  max_intensity[1] = wb_supervisor_field_get_sf_float(light_intensity_field[1]);
  
  // Main loop
  float elapsed = 0.f;
  while (wb_robot_step(TIME_STEP) != -1) {
    wb_supervisor_field_set_sf_float(light_intensity_field[0], max_intensity[0] * (0.5 * sin(FREQ_0 * M_PI * elapsed) + 0.5f));
    wb_supervisor_field_set_sf_float(light_intensity_field[1], max_intensity[1] * (0.5 * sin(FREQ_1 * M_PI * elapsed) + 0.5f));
    elapsed += TIME_STEP / 1000.f;
  }
  /* if needed we can now write e.g., the intensity to a log file for future analysis.
   * it is also possible to access the robot's positions
   * For all details regarding available supervisor functions
   * please refer to: https://www.cyberbotics.com/doc/reference/supervisor
   */
  wb_robot_cleanup();
  return 0;
}
