-- Problem: Program si? wiesza przy zapisywaniu zdj?? i wysy?aniu requesta do API
http = require('socket.http')
json = require('json')

COLOR_WHITE = {255, 255, 255}
COLOR_RED = {255, 0, 0}
COLOR_GREEN = {0, 255, 0}
COLOR_BLUE = {0, 0, 255}

OBSTACLE_AVOID_DISTANCE = 3
MOVING_VELOCITY = 20
TURNING_VELOCITY = 15
PROXIMITY_STOP_DISTANCE = 1
CONFIDENCE = 0
IMAGE_COUNTER = 0
CAMERA_FIELD_OF_VIEW = 60
PHOTO_WIDTH = 1920
BATERY_VALUE = 10

function sysCall_init()

    os.execute("rm -r robot")
    os.execute("mkdir robot")

    ModelHandle = sim.getObject(".")
    ModelName = sim.getObjectAlias(ModelHandle)

    RatHandles = {}  -- List of rats on the scene
    local i = 0
    while true do
        local rat_handle = sim.getObject("/Rat*", {index=i, noError=true})
        if rat_handle < 0 then
            break
        end
        table.insert(RatHandles, rat_handle)
        i = i + 1
    end

    DuckHandles = {}  -- List of ducks on the scene
    local i = 0
    while true do
        local duck_handle = sim.getObject("/Duck*", {index=i, noError=true})
        if duck_handle < 0 then
            break
        end
        table.insert(DuckHandles, duck_handle)
        i = i + 1
    end

    RightWheel = sim.getObject("./RightWheel")
    LeftWheel = sim.getObject("./LeftWheel")
    Camera = sim.getObject("./Camera")
    VisionSensor = sim.getObject("./Vision_sensor")
    SonarRight = sim.getObject("./Proximity_sensor_right")
    SonarLeft = sim.getObject("./Proximity_sensor_left")

    Diodes = { -- LED_strip has to be ungrouped
        D2 = sim.getObject("./diode_2"),
        D3 = sim.getObject("./diode_3"),
        D4 = sim.getObject("./diode_4"),
        D5 = sim.getObject("./diode_5"),
        D6 = sim.getObject("./diode_6"),
        D7 = sim.getObject("./diode_7")
    }

    sim.setShapeColor(sim.getObject("./diode_1"), nil, 0, COLOR_WHITE)
    sim.setShapeColor(sim.getObject("./diode_8"), nil, 0, COLOR_WHITE)

    sim.setObjectInt32Param(RightWheel,2000, 1) -- Enable velocity control
    sim.setObjectInt32Param(LeftWheel,2000, 1)
    sim.setObjectInt32Param(Camera,2000, 1)

    Set_wheel_velocity(5, 5)
    sim.setJointTargetVelocity(Camera, 0)

    sim.setJointTargetForce(LeftWheel, 5)
    sim.setJointTargetForce(RightWheel, 5)

    Last_Color_Change_Time = sim.getSimulationTime()
    Last_Camera_Rotation_Time = sim.getSimulationTime()
    Last_Photo_Time = sim.getSimulationTime()
    Last_Turn_Time=sim.getSimulationTime()
    Turning = false
    Seeing_Another_Rat = 0
    Close_To_Rat = false
    CameraAngle = Get_angle(VisionSensor)
    ModelAngle = Get_angle(ModelHandle)
    Rat_Degrees_Offset_From_Photo_Center = 0

    DEBUG_Sensor_Time = sim.getSimulationTime()

    --sim.addBanner("Bateria = 100", 2, sim_banner_fullyfacingcamera, nil, ModelHandle, nil, nil)

    Camera_Rotate_Counter = 0
    --IMAGE_COUNTER = 0
end



function sysCall_actuation()
    -------------------- photo test --------------------
    if (sim.getSimulationTime() - Last_Photo_Time) > 0.5 then
        if BATERY_VALUE > 0 then
            BATERY_VALUE = BATERY_VALUE - 1
            UpdateBatery(BATERY_VALUE)
        end
        if not Close_To_Rat then
            Take_photo()
        end
        Last_Photo_Time = sim.getSimulationTime()
    end

    -------------------- camera test -------------------
    if (sim.getSimulationTime() - Last_Camera_Rotation_Time) > 1.7 then
        if Camera_Rotate_Counter == 0 then
            sim.setJointTargetVelocity(Camera, 2)
            Camera_Rotate_Counter = 1
        else
            sim.setJointTargetVelocity(Camera, -2)
            Camera_Rotate_Counter = 0
        end
        Last_Camera_Rotation_Time = sim.getSimulationTime()
    end

    -------------------- moving test -------------------
    if Turning == true and Angle_difference(Get_angle(ModelHandle), Desired_angle) < 10 then
        Finish_turning()
        Stop_if_close(RatHandles)
    end

end



function sysCall_sensing()
    local _, _, _, detected_handle_left, _ = sim.readProximitySensor(SonarLeft)
    local _, _, _, detected_handle_right, _ = sim.readProximitySensor(SonarRight)
    if Has_Value(RatHandles, detected_handle_left) or Has_Value(RatHandles, detected_handle_right) then
        if detected_handle_left > 0 and Has_Value(RatHandles, detected_handle_left) then
            Handle = detected_handle_left
        else
            Handle = detected_handle_right
        end
        Close_To_Rat = true
        Change_strip_color(COLOR_RED)
        SensorMovement(Handle)
    elseif BATERY_VALUE > 0 then
        Close_To_Rat = false
        VisionMovement()
    else
        Set_wheel_velocity(0, 0)
    end
end



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





------------------------------ non-sysCall functions below ------------------------------
function VisionMovement()
    if Seeing_Another_Rat == 1 then
        local adjusted_angle = Calculate_desired_angle(CameraAngle, Rat_Degrees_Offset_From_Photo_Center)
        local angle_diff = Angle_difference(ModelAngle, adjusted_angle)
        if angle_diff > 5 then
            if  math.abs(ModelAngle - adjusted_angle) < 180 then
                if ModelAngle < adjusted_angle then
                    print(string.format("[%s] Rat detected on the left", ModelName))
                else
                    print(string.format("[%s] Rat detected on the right", ModelName))
                    angle_diff = -angle_diff
                end
            else
                if ModelAngle > adjusted_angle then
                    print(string.format("[%s] Rat detected on the left", ModelName))
                else
                    print(string.format("[%s] Rat detected on the right", ModelName))
                    angle_diff = -angle_diff
                end
            end
            DEGREES_TO_TURN = angle_diff
            Start_turning(angle_diff)
            ModelAngle = 0
            CameraAngle = 0
            Rat_Degrees_Offset_From_Photo_Center = 0
        end
        if not Turning then
            Go_Forward()
        end
    elseif not Turning then
        Random_Walk()
    end
    if not Turning then
        Stop_if_close(RatHandles)
    end
end

function SensorMovement(handle)
    Follow(handle)
end


function RatCallPredictionAPI(filename)
    local response = http.request(string.format("http://127.0.0.1:5000/detectRat1?img=%s", filename))
    return json.decode(response)
end

function RobotCallPredictionAPI(filename)
    local response = http.request(string.format("http://127.0.0.1:5000/detectRobot1?img=%s", filename))
    return json.decode(response)
end

function UpdateBatery(batery_value)
    sim.addBanner(batery_value, 0.4, sim_banner_fullyfacingcamera, {1.0, 0, 0, 0, 0, 0}, ModelHandle, nil, {1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0})
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 Change_strip_color(rgb_vals)
    for _, v in pairs(Diodes) do
        sim.setShapeColor(v, nil, 0, rgb_vals)
    end
end


function Set_wheel_velocity(left_vel, right_vel)
    sim.setJointTargetVelocity(LeftWheel, left_vel)
    sim.setJointTargetVelocity(RightWheel, right_vel)
end


function Take_photo()
    CameraAngle = Get_angle(VisionSensor)
    ModelAngle = Get_angle(ModelHandle)
    local img = sim.getVisionSensorImg(VisionSensor)
    local filename = "img"..IMAGE_COUNTER..".png"
    local path = "robot1/"..filename
    sim.saveImage(img, {1920,1080}, 0, path, -1)
    print(string.format("[%s] Photo taken 1", ModelName))
    local res = RatCallPredictionAPI(filename)
    local res_robot = RobotCallPredictionAPI(filename)
    print(CONFIDENCE)
    if BATERY_VALUE > 0 then
        if table.concat(res['0']) == 'None' then
            --print(CONFIDENCE)
            if CONFIDENCE > 0 then
                CONFIDENCE = CONFIDENCE - 1
            end
            Seeing_Another_Rat = 0
            if not Close_To_Rat and CONFIDENCE == 0 then
                Change_strip_color(COLOR_GREEN)
                print(string.format("[%s] No rats detected!", ModelName))
            else
                print(string.format("[%s] Chase the rat!", ModelName))
            end

        else
            local frame_width = res['0'][1]
            local rat_pos = res['0'][2] + frame_width / 2
            Rat_Degrees_Offset_From_Photo_Center = (rat_pos - (PHOTO_WIDTH / 2)) / (PHOTO_WIDTH/CAMERA_FIELD_OF_VIEW)
            if table.concat(res_robot['0']) == 'None' then
                Seeing_Another_Rat = 1
                Change_strip_color(COLOR_RED)
                CONFIDENCE = 3
                print(string.format("[%s] Another rat detected on camera", ModelName))
            elseif res_robot['0'][1] == 'red' then
                Seeing_Another_Rat = 0
                print(string.format("[%s] I'm looking for another rat. This robot doesn't need my help", ModelName))
                if CONFIDENCE == 0 then
                    Change_strip_color(COLOR_GREEN)
                end

            elseif res_robot['0'][1] == 'white' then
                Seeing_Another_Rat = 1
                print(string.format("[%s] Robot need my help. I'm on my way!", ModelName))
                Change_strip_color(COLOR_RED)
                CONFIDENCE = 3
            end
        end
        IMAGE_COUNTER = IMAGE_COUNTER + 1
    else
        Change_strip_color(COLOR_WHITE)
        print(string.format("[%s] I have to charge my battery", ModelName))
    end
end



function Orientation_to_angle(orientation)
    -- returns a value between [0, 360]
    local angle = math.deg(orientation[2]) + 90
    if orientation[1] < 0 then
        angle = 360 - angle
    end
    return angle
end


function Get_angle(handle)
    local orientation = sim.getObjectOrientation(handle, sim.handle_world)
    return Orientation_to_angle(orientation)
end


function Calculate_desired_angle(old_angle, angle_delta)
    local desired = old_angle + angle_delta
    if desired > 360 then
        return desired % 360
    elseif
        desired < 0 then
            return desired + 360
    else
        return desired
    end
end


function Angle_difference(actual_angle, desired_angle)
    -- returns a value between [0, 180]
   local diff = math.abs(desired_angle - actual_angle)
   return diff < 180 and diff or 360 - diff
end


function Turn_Left()
    Set_wheel_velocity(0, TURNING_VELOCITY)
end


function Turn_Right()
    Set_wheel_velocity(TURNING_VELOCITY,0)
end


function Go_Forward()
    Set_wheel_velocity(MOVING_VELOCITY, MOVING_VELOCITY)
end


function Stop_Moving()
    Set_wheel_velocity(0, 0)
end


function Start_turning(degrees)
        FULL_CIRCLE_PERIOD = 2.8 -- at 2.5 speed
        print(string.format("[%s] Starting to turn %.2f degrees", ModelName, degrees))
        Turning = true
        Desired_angle = Calculate_desired_angle(Get_angle(ModelHandle), degrees)
        if degrees > 0 then
            Turn_Left()
        else
            Turn_Right()
        end
        Last_Turn_Time = sim.getSimulationTime()
end


function Finish_turning()
    Go_Forward()
    Last_Turn_Time = sim.getSimulationTime()
    Turning = false
    After_turning = Get_angle(ModelHandle)
    print(string.format("[%s] Finished turning; Angle deviation from desired: %.2f (Desired: %.2f vs Actual: %.2f)",
                        ModelName,
                        Angle_difference(After_turning, Desired_angle),
                        Desired_angle,
                        After_turning))
end


function Random_Walk()
    local _, obstacle_left_distance, _, _, _ = sim.readProximitySensor(SonarLeft)
    if obstacle_left_distance == 0 then
        obstacle_left_distance = math.huge
    end

    local _, obstacle_right_distance, _, _, _ = sim.readProximitySensor(SonarRight)
    if obstacle_right_distance == 0 then
        obstacle_right_distance = math.huge
    end

    if obstacle_right_distance < obstacle_left_distance and obstacle_right_distance < OBSTACLE_AVOID_DISTANCE then
        Turn_Left()
    elseif obstacle_left_distance < obstacle_right_distance and obstacle_left_distance < OBSTACLE_AVOID_DISTANCE then
        Turn_Right()
    else
        Go_Forward()
    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 (sim.getSimulationTime() - Last_Photo_Time) > 0.5 then
        if not Close_To_Rat then
            local img = sim.getVisionSensorImg(VisionSensor)
            local filename = "img"..IMAGE_COUNTER..".png"
            local path = "robot1/"..filename
            sim.saveImage(img, {1920,1080}, 0, path, -1)
        end
        Last_Photo_Time = sim.getSimulationTime()
        IMAGE_COUNTER = IMAGE_COUNTER + 1
    end

    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 < PROXIMITY_STOP_DISTANCE) or (right_distance > 0 and right_distance < PROXIMITY_STOP_DISTANCE) then
        Stop_Moving()
    end
end


function Stop_if_close(handle_table)
    -- Przy szczurach jest ok przy robotach z jakiegos powodu nie wykrywa jesli patrzy na kolo zamiast chassisa (kolo pewnie ma inny objecthandle?)
    local left_detected, left_distance, _, left_detected_object_handle, _ = sim.readProximitySensor(SonarLeft)
    local right_detected, right_distance, _, right_detected_object_handle, _ = sim.readProximitySensor(SonarRight)
    if Has_Value(handle_table, left_detected_object_handle) or Has_Value(handle_table, right_detected_object_handle) then
        if (left_distance > 0 and left_distance < PROXIMITY_STOP_DISTANCE) or (right_distance > 0 and right_distance < PROXIMITY_STOP_DISTANCE) then
            Stop_Moving()
        end
    end
end


------------------------------ DEBUG functions below ------------------------------
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