import com.cyberbotics.webots.controller.*;
import java.util.ArrayList;

public class sheepController extends Robot {
    private static final int TIME_STEP = 64; // milliseconds
    private static final int NUMBER_OF_CAMERAS = 3;
    private static final int MAGIC = 5;
    private static final double SPEED = 0.5;
    private String[] cameras_name = {"camera_front", "camera_left", "camera_right", "camera_rear"};
    private String[] wheels_name = {"wheel1", "wheel2", "wheel3", "wheel4"};
    private String[] ds_names = {"ds_left_front", "ds_right_front", "ds_left_corner", "ds_right_corner", "ds_left_rear", "ds_right_rear", "ds_left_back", "ds_right_back" };
    private int[][] image = new int[NUMBER_OF_CAMERAS][];
    private int width;
    private int height;
    private DistanceSensor[] distanceSensors = new DistanceSensor[8];
    private Motor[] motors = new Motor[4];
    private Camera[] cameras = new Camera[NUMBER_OF_CAMERAS];
    private ArrayList<Double> previousDirections = new ArrayList<Double>();

    public sheepController() {
        super();
        for (int i =0; i<distanceSensors.length; i++){
            distanceSensors[i] = new DistanceSensor(ds_names[i]);
            distanceSensors[i].enable(TIME_STEP);
        }
        for (int i =0; i<motors.length; i++){
            motors[i] = new Motor(wheels_name[i]);
            motors[i].setPosition(Double.POSITIVE_INFINITY);
        }
        for (int i =0; i<NUMBER_OF_CAMERAS; i++){
            cameras[i] = new Camera(cameras_name[i]);
            cameras[i].enable(TIME_STEP);
        }
        width = cameras[0].getWidth();
        height = cameras[0].getHeight();
    }

    public void run() {
        while (step(TIME_STEP) != -1) {
            for (int i = 0; i< NUMBER_OF_CAMERAS; i++)
                image[i] = cameras[i].getImage();

            int[] WhatIsOnTheImage = new int[NUMBER_OF_CAMERAS];
            for (int c=0; c<NUMBER_OF_CAMERAS; c++)
                WhatIsOnTheImage[c] = whatIsThat(image[c]);

            AddCurrentDirection(calculateDirection(WhatIsOnTheImage));
            setWheelsVelocity();
        }
    }

    private double calculateDirection(int[] whatIsHappeningOutside) {
        // camery: 0 -przod, 2 - prawo, 1- lewo, 3?-tył
        // wynik: 0- cos, 1- owca, 2- pies lub wilk
        // ujemny return w lewo, dodatni w prawo
        double direction = 0;
        if (whatIsHappeningOutside[0] == 1) direction += Math.random()*10 - 5;
        else if (whatIsHappeningOutside[0] == 2) {
            direction += 180;
        }
        else {
            double[] ds_values = new double[2];
            for (int i=0; i<2 ; i++)
                ds_values[i] = distanceSensors[i].getValue();
            if (ds_values[0] < 990.0 || ds_values[1] < 990.0) direction += 180;
        }
        for (int i = 1; i<NUMBER_OF_CAMERAS ;i++) {
            if (whatIsHappeningOutside[i] == 2) {
                direction -= Math.signum(i-1.5)*(-50); //jeżeli wilk lub pies, to uciekaj
                break;
            }
            if (whatIsHappeningOutside[i] == 1) {
                direction += Math.signum(i-1.5)*20; //jeżeli owca, to się zbliż
                continue;
            }
        }
        return direction;
    }

    private void AddCurrentDirection(double direction) {
        previousDirections.add(direction);
        if (previousDirections.size() > MAGIC) previousDirections.remove(previousDirections.get(0));
        //bo po co miec wiecej niz 100? to i tak duzo
    }

    private void setWheelsVelocity() {
        double weightedSumOfDirections = 0;
        int sumOfWeights = 0;
        for (int i = 1; i<=previousDirections.size(); i++) {
            weightedSumOfDirections += i*(double)previousDirections.get(i-1);
            sumOfWeights += i;
        }
        double direction = weightedSumOfDirections/(sumOfWeights*180);

        double left_speed = SPEED - direction*SPEED;
        double right_speed = SPEED + direction*SPEED;

        motors[0].setVelocity(left_speed);
        motors[1].setVelocity(right_speed);
        motors[2].setVelocity(left_speed);
        motors[3].setVelocity(right_speed);
    }

    private int whatIsThat(int[] image) {
        double red = 0;
        double blue = 0;
        double green = 0;
        int temp = 35;
        double numberOfPixels = 0;
        for (int i= 3*width/9; i<6*width/9;i++)
            for (int j = 4*height/9; j< 5*height/9; j++) {
                red += Camera.imageGetRed(image, width, i,j);
                green += Camera.imageGetGreen(image, width, i,j);
                blue += Camera.imageGetBlue(image, width, i,j);
                numberOfPixels ++;
        }
        red /= numberOfPixels;
        green /= numberOfPixels;
        blue /= numberOfPixels;
        if (red < temp && green <temp && blue < temp) {
            return 2;
        }
        if(red < 75 && green <75 && blue < 75){
            return 1;
        }
        return 0;
    }

    public static void main(String[] args) {
        sheepController robot = new sheepController();
        robot.run();
    }
}