# имя: 'PTZ Tracer 3'
# описание: детектор слежения за целями в зоне
# тип триггера: 'EgsScheduled'
# создан: 2017.04.24 15.55.51, Сельченков Н.Ю.
# изменен: 2019.03.04 17.25.59, Белоусов А.И.
# изменен: '2019.06.14 12.45.25', Сельченков Н.Ю.
# подробности: https://redmine.integra-s.com:11000/projects/eilyacuario/wiki/PTZ_Tracer_2

use System.Uri
use System.Convert
use System.Math
use System.Threading.Tasks.Task
use System.Text.RegularExpressions.Regex
use Newtonsoft.Json.JsonConvert from Newtonsoft.Json
use System.Collections.Generic.Dictionary(string, object) as Dictionary
use System.Collections.ArrayList
use Newtonsoft.Json.Linq.JArray
use Newtonsoft.Json.Linq.JObject
use System.Environment

use acuario2.types.BaseObject from acuario2.types
use acuario2.types.SpatialObject from acuario2.types
use acuario2.types.MoveableObject from acuario2.types
use acuario2.types.VideoCamera from acuario2.types
use acuario2.types.Zone from acuario2.types
use acuario2.types.PTZDevice from acuario2.types
use acuario2.types.IntegraVideo7 from acuario2.types

use acuario2.utils.DateTimeExtension

use utils.GeoPoint from GeoUtils
use utils.GeoUtils from GeoUtils
use utils.Matrix3D from GeoUtils

use integravideo.client.Iv7Server from IntegraVideo7Client
use integravideo.client.MemoryMappedFrame from IntegraVideo7Client
use new { code = 0, user_code = 0, result = null as ArrayList } as CallMethod2Result
use new { pan = 0.0, tilt = 0.0, zoom = 0.0 } as PTZ
use new { ptzdev = nil as PTZDevice, task = nil as Task } as MyTask
use System.Collections.Generic.List(MyTask) as MyTaskList
use new { item = nil as MoveableObject, priority = 0.0, zones = nil as Zone[], zone = nil as Zone } as Target

use typedef
`
    <root>
        <param name="url">http://localhost:1986/axis2/services/Iv7Server</param>
        <param name="snapshot_width" type="int">320</param>
        <param name="snapshot_height" type="int">240</param>
        <param name="trace_highest_factor" type="real">10.0</param>
        <param name="trace_high_factor" type="real">2.0</param>
        <param name="trace_normal_factor" type="real">1.0</param>
        <param name="trace_low_factor" type="real">0.5</param>
        <param name="trace_lowest_factor" type="real">0.2</param>
        <param name="trace_disable_timeout" type="int">60</param>
        <param name="multi_trace_enabled" type="bool">false</param>
        <param name="max_adjust_iterations" type="int">10</param>
    </root>
` as Settings

const settings = Settings(trigger.settings)
const uri      = Uri(settings.url)

once tasks = MyTaskList()

######################################################

let intersected_with(seq1, seq2) = from seq1 intersect seq2 count all > 0

let is_one_of(target as MoveableObject, types as string[]) = (target.GetType().Name in types) or (target.Types is intersected_with types)

let allowed_to_trace(ptzdev as PTZDevice, target as MoveableObject) = (ptzdev.trace_types is empty) or is_one_of(target, ptzdev.trace_types)

######################################################

let iv7 = Iv7Server() 
iv7.Url = string(uri)

let sys_params = "" 

let сall_method2(name as string, args as object, log as bool) = 
    let request  = JsonConvert.SerializeObject(args)
    let response = iv7.CallMethod2(name, request, sys_params)
    let res      = JsonConvert.DeserializeObject(response, CallMethod2Result) as CallMethod2Result
    let ok       = (res.code is 0) and (res.user_code is 0)
    if (not ok) and log then
        print request
        print sys_params
        print response
    end
    if ok then res.result
end

let logon { login = "", password = "" } = 
    sys_params = JsonConvert.SerializeObject(new { session_id = "egs", timeout = 5, relay = string[](0), login = login, password = password })
    сall_method2("video-client:Logon", it, on) is not null
end

let get_ptz { key2 = "", QueryAll = true, login = "", password = "" } = 
    let res = сall_method2("ptzclient:Command", it, on)
    if res isnt null then 
        let ptz  = PTZ()
        let obj  = res[0] as JObject
        ptz.pan  = (obj["pan"] as double) / 100
        ptz.tilt = (obj["tilt"] as double) / 100
        ptz.zoom = (obj["zoom"] as double) / 10000
        ptz
    end
end

let _set_ptz { key2 = "", PanTo = -1, TiltTo = -1, ZoomTo = -1, login = "", password = "" } = сall_method2("ptzclient:Command", it, on) is not null
let set_ptz(login as string, password as string, key2 as string, ptz as PTZ) = _set_ptz { key2 = key2, PanTo = ptz.pan * 100 as int, TiltTo = ptz.tilt * 100 as int, ZoomTo = ptz.zoom * 10000 as int, login = login, password = password } 

let open_mapping { udp_id = "", key2 = "", unique = "" } = сall_method2("video-client:OpenMapping", it, off) is not null
let close_mapping { unique = "" } = сall_method2("video-client:CloseMapping", it, off) is not null

let get_param_from_urls(obj as VideoCamera, pattern as string) = 
    let regex = Regex(pattern)
    from obj.url select regex.Match(it) where Success select Groups[1].Value first
end

let get_key2(obj as VideoCamera) = get_param_from_urls(obj, "key2=(.+)[&|;]?")
let get_udp_id(obj as VideoCamera) = get_param_from_urls(obj, "udp_id=([0-9]{1,})&")
let get_login(obj as VideoCamera) = get_param_from_urls(obj, "iv7://(.+):.+@")
let get_password(obj as VideoCamera) = get_param_from_urls(obj, "iv7://.+:(.+)@")

######################################################

let get_priority_factor(priority as Priority) = 
    switch priority 
        when "highest" then settings.trace_highest_factor ?? 10
        when "high" then settings.trace_high_factor ?? 2
        when "normal" then settings.trace_normal_factor ?? 1
        when "low" then settings.trace_low_factor ?? 0.5
        when "lowest" then settings.trace_lowest_factor ?? 0.2
        else 0
end    

let unix_now() = DateTimeExtension.ToUnixTime(DateTime.UtcNow)

let seconds_passed(datetime as DateTime) = unix_now() - DateTimeExtension.ToUnixTime(datetime)

let shorten_angle(angle as double) = 
    if      angle < -180 then angle + 360
    else if angle > 180  then angle - 360
    else                      angle
end

######################################################

let recalibrate(point1 as GeoPoint, point2 as GeoPoint, matrix as Matrix3D, ptzdev as PTZDevice) =  
    let ptz                   = PTZ()
    let arr                   = GeoUtils.recalibrate(point1, point2, matrix)
    ptz.tilt                  = Math.Round(arr[0], 2, "AwayFromZero")
    ptz.pan                   = Math.Round(arr[1], 2, "AwayFromZero")
    let range                 = arr[2]
    let min_zoom_distance     = if ptzdev.min_zoom_distance > 0 then ptzdev.min_zoom_distance else 0
    let min_zoom_limit_factor = if ptzdev.min_zoom_limit_factor > 0 then ptzdev.min_zoom_limit_factor else 0
    let max_zoom_distance     = if ptzdev.max_zoom_distance > 0 then ptzdev.max_zoom_distance else 1500
    let max_zoom_limit_factor = if ptzdev.max_zoom_limit_factor > 0 then ptzdev.max_zoom_limit_factor else 1
    #let in_zoom_limit_factor  = if ptzdev.in_zoom_limit_factor > 0 then ptzdev.in_zoom_limit_factor else 1
    ptz.zoom =  
                                if range < min_zoom_distance then min_zoom_limit_factor 
                                else if range > max_zoom_distance then max_zoom_limit_factor 
                                else ((range - min_zoom_distance)/(max_zoom_distance - min_zoom_distance))*(max_zoom_limit_factor - min_zoom_limit_factor) + min_zoom_limit_factor
    ptz.zoom                  = Math.Round(ptz.zoom, 2, "AwayFromZero")
    ptz
end

let get_assumed_target_pos(point as GeoPoint, heading as double, distance as double) =
    let WGS_84_RADIUS_EQUATOR = 6378137.0
    let result                = point.Destination(heading, distance, WGS_84_RADIUS_EQUATOR)
    result.Lattitude          = GeoUtils.toDeg(result.Lattitude) 
    result.Longitude          = GeoUtils.toDeg(result.Longitude) 
    result
end

let trace_target(ptzdev as PTZDevice, zone as Zone, target as MoveableObject) =

    let camera_pos = with ptzdev as SpatialObject do 
        let p    = GeoPoint(geo_position)
        p.Height = geo_height
        p
    end
    
    let polygon    = GeoUtils.ParsePolygon(zone.area, zone.area_heights)
    let target_pos = with target as BaseObject do
        let p    = GeoPoint(position)
        p.Height = GeoUtils.GetZoneApproxHeight(polygon, p)
        p
    end
    
    let v = ptzdev.calibrationMatrix
    if (v isnt null) and camera_pos.Valid and target_pos.Valid then
    
        let key2   = get_key2(ptzdev as VideoCamera)
        let udp_id = get_udp_id(ptzdev as VideoCamera)
        let unique = ptzdev.Id..".mmap"
        
        let videoServer = from ptzdev.GetLinkedItems(IntegraVideo7) try single
        let login       = (videoServer?.login) ??? get_login(ptzdev as VideoCamera) 
        let password    = (videoServer?.password) ??? get_password(ptzdev as VideoCamera) 
        
        let matrix = Matrix3D
        (
            v[0], v[1], v[2], 0, 
            v[3], v[4], v[5], 0, 
            v[6], v[7], v[8], 0, 
               0,    0,    0, 1
        )

        logon { login = login, password = password }
        
        let max_iterations = settings.max_adjust_iterations ?? 10
        let focus_time     = if ptzdev.focus_time > 0 then ptzdev.focus_time else 5
        let zoom_time      = if ptzdev.zoom_time > 0 then ptzdev.zoom_speed else 5
        let zoom_speed     = 1 / zoom_time  
        let pan_speed      = if ptzdev.pan_speed > 0  then ptzdev.pan_speed  else 36
        let tilt_speed     = if ptzdev.tilt_speed > 0 then ptzdev.tilt_speed else 36
        let parallel_ptz   = ptzdev.parallel_ptz
        let heading        = GeoUtils.toRad(target.heading) 
        let speed          = target.speed
        let old            = get_ptz { key2 = key2, login = login, password = password }
        let old            = get_ptz { key2 = key2, login = login, password = password }
        let prev_pos       = target_pos
        let iter ()        = 
            let ptz       = recalibrate(camera_pos, prev_pos, matrix, ptzdev)
            let pan_time  = Math.Abs(shorten_angle(ptz.pan  - old.pan)) / pan_speed 
            let tilt_time = Math.Abs(shorten_angle(ptz.tilt - old.tilt)) / tilt_speed
            let zoom_time = Math.Abs(ptz.zoom - old.zoom) / zoom_speed
            let times     = new [ focus_time, pan_time, tilt_time, zoom_time ]
            let time      = if parallel_ptz then (from times max) else (from times sum)
            let pos       = get_assumed_target_pos(target_pos, heading, speed * time)
            let err       = prev_pos.Distance(pos)
            prev_pos      = pos
            #print(ptz, pos, err)
            new { ptz = ptz, pos = pos, time = time, err = err } 
        end

        let result = from 1 to max_iterations select iter () order by err try first

        print("TRACE", ptzdev, target, zone, target_pos, target.heading, target.speed, result.pos, login, password, result.ptz)
        
        set_ptz(login, password, key2, result.ptz)
        
        open_mapping { udp_id = udp_id, key2 = key2, unique = unique }
        
        await Task.Delay(int(Math.Round(result.time * 1000)))
        
        let frame = try MemoryMappedFrame(unique)

        if (frame isnt null) and frame.Valid then 
            let jpg               = frame.ToJPEG(settings.snapshot_width ?? 320, settings.snapshot_height ?? 240)
            target.trace_snapshot = Convert.ToBase64String(jpg)
        else
            target["trace_snapshot"].Set(null, "invalid", null)
        end
    end
end

let completed = from tasks where task.IsCompleted to array 
from completed do tasks.Remove(it) now

let busy(ptzdev as PTZDevice) = from tasks any it.ptzdev is ptzdev

let ptzdevs = from trigger.Module.Graphs
              select many (from Values of type PTZDevice) 
              where (calibrationMatrix isnt empty) 
                and (it["trace_enabled"].Value isnt null)
                and (trace_enabled or seconds_passed(it["trace_enabled"].DateTime) > (settings.trace_disable_timeout ?? 60))
                and (it isnt busy)
              do trace_enabled = true
              to list
              
let zones = from ptzdevs select many GetLinkedItems(Zone) distinct bind string(Id) to it

let makeTarget(item as MoveableObject) =
    let target   = Target()
    target.item  = item
    target.zones = from item.trace_zones where it in zones select zones[it] 
                      order by get_priority_factor(trace_priority) desc to array
    target.zone     = from target.zones try first 
    target.priority = seconds_passed(item["trace_snapshot"].DateTime) 
                    * get_priority_factor(item.trace_priority)
                    * get_priority_factor(target.zone?.trace_priority)
    target                
end

let runTasks(target as Target) =
    let continue = true
    from target.zones while continue do
        let zone = it
        from GetLinkedItems(PTZDevice) where (it in ptzdevs) and (it is allowed_to_trace target.item) while continue do
            ptzdevs.Remove(it)
            let task    = MyTask()
            task.ptzdev = it 
            task.task   = async trace_target(it, zone, target.item)
            tasks.Add(task)
            continue = settings.multi_trace_enabled ?? false
        end now
    end now
end

from graph.Values of type MoveableObject 
    where (trace_zones isnt empty) and (trace_priority isnt "ignore") 
    select makeTarget(it) where zone isnt nil order by priority desc
    while ptzdevs.Count > 0 and priority > 0
    do runTasks(it) now