# имя: 'PTZ Tracer 2'
# описание: детектор слежения за целями в зоне
# тип триггера: 'EgsScheduled'
# создан: 2017.04.24 15.55.51, Сельченков Н.Ю.
# изменен: '2018.12.05 14.09.33', Сельченков Н.Ю.
# подробности: 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="max_zoom_distance" type="real">1500</param>
        <param name="max_zoom_limit_factor" type="real">1</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>
    </root>
` as Settings

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

once tasks = MyTaskList()

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

let iv7 = Iv7Server() 
iv7.Url = string(uri)
        
let сall_method2(name as string, args as object, log as bool) = 
    let request  = JsonConvert.SerializeObject(args)
    let response = iv7.CallMethod2(name, request, "")
    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 response
    end
    if ok then res.result
end

let logon { login = "", password = "" } = сall_method2("video-client:Logon", it, on) is not null

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 JArray)[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 recalibrate(point1 as GeoPoint, point2 as GeoPoint, matrix as Matrix3D) =  
    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 max_zoom_distance     = settings.max_zoom_distance ?? 1500
    let max_zoom_limit_factor = settings.max_zoom_limit_factor ?? 1
    ptz.zoom                  = if range > max_zoom_distance then max_zoom_limit_factor else 1 - (max_zoom_distance - range) / max_zoom_distance
    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
        )

        let ptz = recalibrate(camera_pos, target_pos, matrix)

        print("TRACE", ptzdev, target, zone, target_pos, login, password, ptz)
        
        logon { login = login, password = password }
        
        if false then # поправка на скорость
            let pantilt_speed = 180 / 5
            let zoom_time     = 2
            let min_dist      = 0.1
            let prev_dist     = 10000 
            let heading       = GeoUtils.toRad(target.heading) 
            let old           = get_ptz { key2 = key2, login = login, password = password }
            
            let min_time = ptzdev.focus_time + (if ptz.zoom <> old.zoom then zoom_time else 0)
            target_pos   = get_assumed_target_pos(target_pos, heading, target.speed * min_time)
            ptz          = recalibrate(camera_pos, target_pos, matrix)
            
            let continue = true
            from 1 to 10 while continue do
                let delta = Math.Max(Math.Abs(ptz.pan - old.pan), Math.Abs(ptz.tilt - old.tilt)) 
                let time  = delta / pantilt_speed + ptzdev.focus_time + (if ptz.zoom <> old.zoom then zoom_time else 0)
             
                let pos    = get_assumed_target_pos(target_pos, heading, target.speed * time)
                let dist   = target_pos.Distance(pos)
                
                continue = dist > min_dist && dist < prev_dist
                
                prev_dist  = dist
                target_pos = pos
                ptz        = recalibrate(camera_pos, target_pos, matrix)
                
                print(it, time, pos, dist, continue)
            now
        end
        
        set_ptz(login, password, key2, ptz)
        
        open_mapping { udp_id = udp_id, key2 = key2, unique = unique }
        
        await Task.Delay(int(Math.Round(ptzdev.focus_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 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