local veclib = require("opensteer.os-vec")
local osmath, osvec, Vec3 = veclib.osmath, veclib.osvec, veclib.vec3

local SphericalObstacle = require("opensteer.os-obstacle").SphericalObstacle

-- // ----------------------------------------------------------------------------
-- // PolylinePathway: a simple implementation of the Pathway protocol.  The path
-- // is a "polyline" a series of line segments between specified points.  A
-- // radius defines a volume for the path which is the union of a sphere at each
-- // point and a cylinder along each segment.
local pathway = {

    PolylinePathway = function() 

        local self = {}
        -- // xxx shouldn't these 5 just be local variables?
        -- // xxx or are they used to pass secret messages between calls?
        -- // xxx seems like a bad design
        self.segmentLength = 0.0
        self.segmentProjection = 0.0
        self.localspace = Vec3()
        self.chosen = Vec3()
        self.segmentNormal = Vec3()

        self.lengths = {}
        self.normals = {}
        self.totalPathLength = 0.0

        -- // is the given point inside the path tube?
        self.isInsidePath = function(point) 
            local outside 
            local tangent = Vec3()
            local res = self.mapPointToPath(point, tangent, outside)
            return res.outside < 0.0
        end

        -- // how far outside path tube is the given point?  (negative is inside)
        self.howFarOutsidePath = function(point) 
            local outside 
            local tangent = Vec3()
            local res = self.mapPointToPath(point, tangent, outside)
            return res.outside
        end

        self.pointCount = 0
        self.points = {}
        self.radius = 0.0
        self.cyclic = false


        -- // utility for constructors in derived classes
        self.initialize = function( _pointCount, _points, _radius, _cyclic) 
            -- // set data members, allocate arrays
            self.radius = _radius
            self.cyclic = _cyclic
            self.pointCount = _pointCount
            self.totalPathLength = 0.0
            if (self.cyclic) then self.pointCount = self.pointCount + 1 end
            self.lengths = {}
            self.points  = {}
            self.normals = {}

            -- // loop over all points
            for i = 1, self.pointCount  do
            
                -- // copy in point locations, closing cycle when appropriate
                local closeCycle = self.cyclic and (i == self.pointCount)
                local j = i
                if closeCycle then j = 1 end
                self.points[i] = _points[j]

                -- // for the end of each segment
                if (i > 1) then 
                    -- // compute the segment length
                    self.normals[i] = self.points[i].sub( self.points[i-1] )
                    self.lengths[i] = self.normals[i].length()

                    -- // find the normalized vector parallel to the segment
                    self.normals[i] = self.normals[i].mult(1.0 / self.lengths[i])

                    -- // keep running total of segment lengths
                    self.totalPathLength = self.totalPathLength + self.lengths[i]
                end
            end
        end

        -- // Given an arbitrary point ("A"), returns the nearest point ("P") on
        -- // this path.  Also returns, via output arguments, the path tangent at
        -- // P and a measure of how far A is outside the Pathway's "tube".  Note
        -- // that a negative distance indicates A is inside the Pathway.
        self.mapPointToPath = function( point, tangent, outside) 

            local d
            local minDistance = math.huge
            local onPath = Vec3()
        
            -- // loop over all segments, find the one nearest to the given point
            for i = 2, self.pointCount do 
                self.segmentLength = self.lengths[i]
                self.segmentNormal = self.normals[i]
                d = self.pointToSegmentDistance(point, self.points[i-1], self.points[i])
                if (d < minDistance) then
                    minDistance = d
                    onPath = self.chosen
                    tangent = self.segmentNormal
                end
            end
        
            -- // measure how far original point is outside the Pathway's "tube"
            outside = osvec.Vec3_distance(onPath, point) - self.radius
            local res = { onPath=onPath, tangent=tangent, outside=outside }
            -- // return point on path
            return res
        end


        -- // given an arbitrary point, convert it to a distance along the path
        self.mapPointToPathDistance = function(point) 
            local d
            local minDistance = math.huge
            local segmentLengthTotal = 0.0
            local pathDistance = 0.0
        
            for i = 2, self.pointCount do
                self.segmentLength = self.lengths[i]
                self.segmentNormal = self.normals[i]
                d = self.pointToSegmentDistance(point, self.points[i-1], self.points[i])
                if (d < minDistance) then 
                    minDistance = d
                    pathDistance = segmentLengthTotal + self.segmentProjection
                end
                segmentLengthTotal = segmentLengthTotal + self.segmentLength
            end
        
            -- // return distance along path of onPath point
            return pathDistance
        end

        -- // given a distance along the path, convert it to a point on the path
        self.mapPathDistanceToPoint = function(pathDistance) 
            
            -- // clip or wrap given path distance according to cyclic flag
            local remaining = pathDistance
            if (self.cyclic) then 
                remaining = (pathDistance % self.totalPathLength)
            else
                if (pathDistance < 0.0) then return self.points[1] end
                if (pathDistance >= self.totalPathLength) then return self.points[self.pointCount] end
            end

            -- // step through segments, subtracting off segment lengths until
            -- // locating the segment that contains the original pathDistance.
            -- // Interpolate along that segment to find 3d point value to return.
            local result = Vec3()
            for i = 2, self.pointCount do
                self.segmentLength = self.lengths[i]
                if (self.segmentLength < remaining) then 
                    remaining = remaining - self.segmentLength
                else 
                    local ratio = remaining / self.segmentLength
                    result = osmath.interpolateV(ratio, self.points[i-1], self.points[i])
                    break
                end
            end
            return result
        end

        -- // utility methods

        -- // compute minimum distance from a point to a line segment
        self.pointToSegmentDistance = function( point, ep0, ep1) 

            -- // convert the test point to be "local" to ep0
            self.localspace = point.sub( ep0 )

            -- // find the projection of "local" onto "segmentNormal"
            self.segmentProjection = self.segmentNormal.dot(self.localspace)

            -- // handle boundary cases: when projection is not on segment, the
            -- // nearest point is one of the endpoints of the segment
            if (self.segmentProjection < 0.0)  then 
                self.chosen = ep0
                self.segmentProjection = 0
                return osvec.Vec3_distance(point, ep0)
            end
            if (self.segmentProjection > self.segmentLength) then 
                self.chosen = ep1
                self.segmentProjection = self.segmentLength
                return osvec.Vec3_distance(point, ep1)
            end

            -- // otherwise nearest point is projection point on segment
            self.chosen = self.segmentNormal.mult( self.segmentProjection)
            self.chosen = self.chosen.add( ep0 )
            return osvec.Vec3_distance (point, self.chosen)
        end

        -- // assessor for total path length
        self.getTotalPathLength = function() return self.totalPathLength end
        return self
    end,
}

-- // construct a PolylinePathway given the number of points (vertices),
-- // an array of points, and a path radius.
pathway.PolylinePathway1 = function(_pointCount, _points, _radius, _cyclic) 
    local pw = pathway.PolylinePathway()
    pw.initialize (_pointCount, _points, _radius, _cyclic)
    return pw
end

pathway.BuildPathway = function(data, scale)

    data.pathPoints = {}
    for k, pt in ipairs(data.points) do 
        data.pathPoints[k] = osvec.Vec3Set(pt[1], pt[2], pt[3])
    end

    data.obstacles = {}
    for k, objs in ipairs(data.staticObjects) do
        
        data.obstacles[k] = SphericalObstacle()
        local vpos = go.get_world_position(objs.url)
        data.obstacles[k].center = osvec.Vec3Set(vpos.x / scale, vpos.y / scale, vpos.z / scale)
        data.obstacles[k].radius = objs.radius
    end

    data.pathway = pathway.PolylinePathway1(data.pathPointCount, data.pathPoints, data.pathRadius, false)
    data.pathway.endpoint0 = data.pathway.points[1]
    data.pathway.endpoint1 = data.pathway.points[data.pathPointCount]

    return data
end

return pathway