#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <webots/camera.h>
#include <webots/position_sensor.h>
#include <webots/distance_sensor.h>
#include <webots/motor.h>
#include <webots/robot.h>
#include <webots/utils/system.h>

#define TIME_STEP 32

int main(int argc, char **argv) {
    WbDeviceTag right_camera, left_camera, left_motor, right_motor, empilhadeira, frds, flds, ltds, lbds, pos;
    int width, height;
    int pause_counter = 0;
    int left_speed, right_speed;
    int i, j;
    int red, blue, green;
    wb_robot_init();

    frds = wb_robot_get_device("FrontRightDistanceSensor");
    flds = wb_robot_get_device("FrontLeftDistanceSensor");
    ltds = wb_robot_get_device("LeftTopDistanceSensor");
    lbds = wb_robot_get_device("LeftBottomDistanceSensor");
    wb_distance_sensor_enable(frds, TIME_STEP);
    wb_distance_sensor_enable(flds, TIME_STEP);
    wb_distance_sensor_enable(ltds, TIME_STEP);
    wb_distance_sensor_enable(lbds, TIME_STEP);
    pos = wb_robot_get_device("EMPPositionSensor");
    wb_position_sensor_enable(pos, TIME_STEP);
    right_camera = wb_robot_get_device("RightCamera");
    left_camera = wb_robot_get_device("LeftCamera");
    wb_camera_enable(right_camera, TIME_STEP);
    wb_camera_enable(left_camera, TIME_STEP);
    width = wb_camera_get_width(right_camera);
    height = wb_camera_get_height(right_camera);
    left_motor = wb_robot_get_device("LeftWheel");
    right_motor = wb_robot_get_device("RightWheel");
    empilhadeira = wb_robot_get_device("EMPLinearMotor");
    wb_motor_set_position(left_motor, INFINITY);
    wb_motor_set_position(right_motor, INFINITY);
    wb_motor_set_velocity(left_motor, 0.0);
    wb_motor_set_velocity(right_motor, 0.0);
    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);
    };

    wb_robot_cleanup();
    return 0;
}