/*
 * File:          asdasss.c
 * Date:
 * Description:
 * Author:
 * Modifications:
 */

/*
 * You may need to add include files like <webots/distance_sensor.h> or
 * <webots/motor.h>, etc.
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <webots/camera.h>
#include <webots/motor.h>
#include <webots/robot.h>
#include <webots/utils/system.h>

/*
 * You may want to add macros here.
 */
#define TIME_STEP 32

/*
 * This is the main program.
 * The arguments of the main function can be specified by the
 * "controllerArgs" field of the Robot node
 */
int main(int argc, char **argv) {
  /* necessary to initialize webots stuff */
  WbDeviceTag rc, lm, rm;
  int width, height;
  int pause_counter = 0;
  int left_speed, right_speed;
  int i, j;
  int red, blue, green;
  wb_robot_init();

  /*
   * You should declare here WbDeviceTag variables for storing
   * robot devices like this:yguviugoyiouyg
   *  WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
   *  WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
   */
   rc = wb_robot_get_device("RightCamera");
   wb_camera_enable(rc, TIME_STEP);
   width = wb_camera_get_width(rc);
   height = wb_camera_get_height(rc);
   lm = wb_robot_get_device("LeftWheel");
   rm = wb_robot_get_device("RightWheel");
   wb_motor_set_position(lm, INFINITY);
   wb_motor_set_position(rm, INFINITY);
   wb_motor_set_velocity(lm, 0.0);
   wb_motor_set_velocity(rm, 0.0);

  /* main loop
   * Perform simulation steps of TIME_STEP milliseconds
   * and leave the loop when the simulation is over
   */
  while (wb_robot_step(TIME_STEP) != -1) {
    wb_motor_set_velocity(lm, 2);
    wb_motor_set_velocity(rm, 2);
    const unsigned char *image = wb_camera_get_image(rc);
    red = 0, green = 0, blue = 0;
    for(int i = 0; i < 8; i++) {
        for(int j = 0; j < 8; j++) {
          red += wb_camera_image_get_red(image, width, i, j);
          blue += wb_camera_image_get_blue(image, width, i, j);
          green += wb_camera_image_get_green(image, width, i, j);
        }
      }
    printf("%d %d %d\n", red, green, blue);
  };

  /* Enter your cleanup code here */

  /* This is necessary to cleanup webots resources */
  wb_robot_cleanup();

  return 0;
}