WALL_AVOID_DISTANCE = 0.5
MOVING_VELOCITY = -20
TURNING_VELOCITY = 2.5
ROBOT_PROXIMITY_STOP_DISTANCE = 1.2



function sysCall_init()

    grains1 = sim.getObject("../Bump[2]/shape")
    WallHandle = sim.getObject("../ExternalWall/Cuboid")

    rightWheel= sim.getObject("./RightWheelRat")
    leftWheel= sim.getObject("./LeftWheelRat")
    sim.setObjectInt32Param(rightWheel,2000,1)
    sim.setObjectInt32Param(leftWheel,2000,1)

    Set_wheel_velocity(-20, -20)

    sim.setJointTargetForce(leftWheel, 5)
    sim.setJointTargetForce(rightWheel, 5)

    last_time=sim.getSimulationTime()
    turn_time=sim.getSimulationTime()
    rotate = false

    sonarRight = sim.getObject("./Proximity_sensor_right_rat");
    sonarLeft = sim.getObject("./Proximity_sensor_left_rat");
    sonarCenter = sim.getObject("./Proximity_sensor_center_rat");
    max_dist = 3.0
    speed = 10


     RobotHandles = {}  -- List of our robots on the scene
    local i = 0
    while true do
        local robot_handle = sim.getObject("/Bump*/shape", {index=i, noError=true})
        if robot_handle < 0 then
            break
        end
        table.insert(RobotHandles, robot_handle)
        i = i + 1
    end

end


function sysCall_actuation()
end

function sysCall_sensing()
    -- put your sensing code here

    local _, _, _, detected_handle_left, _ = sim.readProximitySensor(sonarLeft)
    local _, _, _, detected_handle_right, _ = sim.readProximitySensor(sonarRight)
    if Has_Value(RobotHandles, detected_handle_left) then   -- For demonstration purposes this robot will follow any robot of the same type
        Follow(detected_handle_left)
    elseif Has_Value(RobotHandles, detected_handle_right) then
        Follow(detected_handle_right)
    else
        Random_Walk()
    end
end



function sysCall_cleanup()
    -- do some clean-up here
end

function Has_Value (tab, val)
    for index, value in ipairs(tab) do
        if value == val then
            return true
        end
    end
    return false
end

function Set_wheel_velocity(left_vel, right_vel)
    sim.setJointTargetVelocity(leftWheel, left_vel)
    sim.setJointTargetVelocity(rightWheel, right_vel)
end

function Turn_Left()
    Set_wheel_velocity(TURNING_VELOCITY, -TURNING_VELOCITY)
end


function Turn_Right()
    Set_wheel_velocity(-TURNING_VELOCITY, TURNING_VELOCITY)
end


function Go_Forward()
    Set_wheel_velocity(MOVING_VELOCITY, MOVING_VELOCITY)
end


function Stop_Moving()
    Set_wheel_velocity(0, 0)
end

function Random_Walk()
    -- put your sensing code here
    distSonarRight = getDistance(sonarRight, max_dist)
    distSonarLeft = getDistance(sonarLeft, max_dist)
    distSonarCenter = getDistance(sonarCenter, max_dist)

    if distSonarRight < max_dist and distSonarRight < distSonarLeft and rotate == false then
        Set_wheel_velocity(0, -40)
    elseif distSonarRight < max_dist and distSonarLeft < max_dist and distSonarCenter < max_dist and distSonarRight < distSonarLeft and rotate == false then
        Set_wheel_velocity(0, -40)
    elseif distSonarRight < max_dist and distSonarCenter < max_dist and distSonarRight < distSonarLeft and rotate == false then
        Set_wheel_velocity(0, -40)
    elseif distSonarLeft < max_dist and distSonarCenter < max_dist and distSonarLeft < distSonarRight and rotate == false then
        Set_wheel_velocity(-40, 0)
    elseif distSonarLeft < max_dist and distSonarLeft < distSonarRight and rotate == false then
        Set_wheel_velocity(-40, 0)
    elseif distSonarCenter < max_dist and rotate == false then
        Set_wheel_velocity(0, -40)
        turn_time = sim.getSimulationTime()
        rotate = true
    elseif rotate == false then
        Set_wheel_velocity(-20, -20)
    end

    if rotate == true and (sim.getSimulationTime() - turn_time) > 0.5 then
        Set_wheel_velocity(-20, -20)
        rotate = false
    end
end

function Follow(object_handle) -- Use with flat sensors with no blind zone in the middle
    local left_detected, left_distance, _, _, _ = sim.checkProximitySensor(sonarLeft, object_handle)
    local right_detected, right_distance, _, _, _ = sim.checkProximitySensor(sonarRight, object_handle)

    if left_detected == 1 and right_detected == 1 then
        Go_Forward()
    elseif left_detected == 1 then
        Turn_Left()
    elseif right_detected == 1 then
        Turn_Right()
    else
        Go_Forward()
    end

    if (left_distance > 0 and left_distance < ROBOT_PROXIMITY_STOP_DISTANCE) or (right_distance > 0 and right_distance < ROBOT_PROXIMITY_STOP_DISTANCE) then
        Stop_Moving()
    end

end

function getDistance(sensor, max_dist)
    local detected, distance
    detected, distance = sim.readProximitySensor(sensor)
    if (detected < 1) then
        distance = max_dist
    end
    return distance
end

function DEBUG_What_Do_I_See(sensor_handle)
    local detected, distance, detected_point, detected_object_handle, detected_surface_normal_vector = sim.readProximitySensor(sensor_handle)
    if detected == 1 then
        print(string.format("[%s] Object detected by %s; Handle: %d, Name: %s, Distance: %.2f",
                        ModelName,
                        sim.getObjectAlias(sensor_handle),
                        detected_object_handle,
                        sim.getObjectAlias(detected_object_handle, 1),
                        distance))
    end
end