#=--------------------------------------------------------------------

projective - Functions supporting projective geometry for computer vision.

Part of the ImageProjectiveGeometry Module

Copyright (c) 2016 Peter Kovesi
pk@peterkovesi.com

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

The Software is provided "as is", without warranty of any kind.

PK February 2016

---------------------------------------------------------------------=#

export Camera
export cameraproject, camera2projmatrix, imagept2plane, decomposecamera, rq3
export makehomogeneous, makeinhomogeneous, hnormalise, hnormalise!
export homography1d, homography2d, normalise1dpts, normalise2dpts
export skew, hcross
export fundmatrix, affinefundmatrix, fundfromcameras
export idealimagepts, solvestereopt, undistortimage
export hline, plotcamera

using Interpolations
using PyPlot

#=
imTrans.m
imTransD.m
equalAngleConstraint.m
knownAngleConstraint.m
lengthRatioConstraint
homoTrans 2D homogeneous transformation of points/lines.

=#

#------------------------------------------------------------------------
"""
Camera - Structure defining parameters of a camera
```
      fx::Real      # Focal length.
      fy::Real
     ppx::Real      # Principal point.
     ppy::Real
      k1::Real      # Radial lens distortion parameters.
      k2::Real
      k3::Real
      p1::Real      # Tangential lens distortion parameters.
      p2::Real
    skew::Real
    rows::Integer   # Optional, providing rows and columns allows detection
    cols::Integer   # of points being projected out of image bounds.
       P::Array     # Camera position in world coordinates.
    Rc_w::Array     # Rotation matrix defining world orientation with
                    # respect to the camera frame.
```

Note the ordering of the tangential distortion parameters p1 and p2 is
not always consistent in the literature.  Within cameraproject() they
are used in the following order, where x and y refer to column and
row coordinates repectively.

```
   dx = 2*p1*x*y          +  p2*(r^2 + 2*x^2)
   dy = p1*(r^2 + 2*y^2)  +  2*p2*x*y

```
Constructors:
```
      Camera(keyword = value, ....)

      Camera(P)     # Where P is a 3x4 projection matrix.
                    # In this case only fx, fy, skew, P and Rc_w can be defined,
                    # all other parameters are set to 0.
```

"""

type Camera{T<:AbstractFloat}
    fx::T           # Focal length.
    fy::T
    ppx::T          # Principal point.
    ppy::T
    k1::T           # Radial lens distortion.
    k2::T
    k3::T
    p1::T           # Tangential lens distortion.
    p2::T
    skew::T
    rows::Int       # Optional, providing rows and columns allows detection
    cols::Int       # of points being projected out of image bounds.
    P::Vector{T}    # Camera position in world coordinates.
    Rc_w::Matrix{T} # Rotation matrix defining world orientation with
                    # respect to the camera frame.
end

# Keyword-value constructor
function Camera(;fx=1.0, fy=1.0, ppx=0.0, ppy=0.0,
                k1=0.0, k2=0.0, k3=0.0, p1=0.0, p2=0.0, skew=0.0,
                rows=0, cols=0,
                P=[0.0, 0.0, 0.0], Rc_w=eye(3))
    if size(P) != (3,)
        error("Camera position must be a 3x1 array")
    end

    if size(Rc_w) != (3,3)
        error("Camera orientation must be a 3x3 rotation matrix")
    end

     # Get the promotion type
     args = promote(fx,fy,ppx,ppy,k1,k2,k3,p1,p2,skew,P[1],Rc_w[1,1])
     T = eltype(args)

     return Camera{T}(fx, fy, ppx, ppy, k1, k2, k3, p1, p2, skew, rows, cols, P, Rc_w)
end

# Contructor that takes a projection matrix
function Camera{T<:AbstractFloat}(P::Matrix{T})
    if size(P) != (3,4)
        error("Projection matrix must be 3x4")
    end

    (K, Rc_w, Pc, pp, pv) = decomposecamera(P)
    fx = K[1,1]
    fy = K[2,2]
    skew = K[1,2]
    k1 = 0.0
    k2 = 0.0
    k3 = 0.0
    p1 = 0.0
    p2 = 0.0
    rows = 0
    cols = 0
    Camera{T}(fx, fy, pp[1], pp[2], k1, k2, k3, p1, p2, skew, rows, cols, Pc, Rc_w)
end

#-------------------------------------------------------------------------
"""
cameraproject - Projects 3D points into camera image
```
Usage:  xy = cameraproject(P, pt)

Arguments:
             P - 3x4 camera projection matrix.
            pt - 3xN matrix of 3D points to project into the image.

Returns:
            xy - 2xN matrix of projected image positions.
```
See also: Camera(), camstruct2projmatrix()

"""
#=
PK September 2008
        June 2015    - Ported to Julia
=#
# Projection matrix version

function cameraproject(P::Array, pt::Array)

    if size(P) != (3,4)
        error("Projection matrix must be 3x4")
    end

    if size(pt, 1) != 3
        error("Points must be in a 3xN array")
    end

    xy = P*makehomogeneous(pt)
    xy = makeinhomogeneous(xy)

    return xy
end

#--------------------------------------------------------------------------
"""
cameraproject - Projects 3D points into camera image
```
Usage:             xy = cameraproject(C, pt, computevisibility = false)
        (xy, visible) = cameraproject(C, pt, computevisibility = true)

Arguments:
             C - Camera structure.
            pt - 3xN matrix of 3D points to project into the image.

Keyword Argument:
 computevisibility - If set to true point visibility is returned
                     The default value is false.

Returns:
       xy      - 2xN matrix of projected image positions
       visible - Boolean array indicating whether the point is
                 within the field of view.  This is only evaluated if
                 'computevisibility is true and the camera structure has
                 non zero values for its 'rows' and 'cols' fields.

```
See also: Camera(), camstruct2projmatrix()
"""

# Camera structure version

function cameraproject(C::Camera, pta::Array; computevisibility = false)

    pt = copy(pta) # Make local copy
    rows = size(pt,1)

    if rows != 3
        error("Points must be in a 3xN array")
    end

    # Transformation of a ground point from world coordinates to camera coords
    # can be thought of as a translation, then rotation as follows
    #
    #   Gc =  Tc_p *  Tp_w * Gw
    #
    #  | . . .   | | 1     -x | |Gx|
    #  | Rc_w    | |   1   -y | |Gy|
    #  | . . .   | |     1 -z | |Gz|
    #  |       1 | |        1 | | 1|
    #
    # Subscripts:
    #   w - world frame
    #   p - world frame translated to camera origin
    #   c - camera frame

    # First translate world frame origin to match camera origin.  This is
    # the Tp_w bit.
    for n = 1:3
        pt[n,:] = pt[n,:] - C.P[n]
    end

    # Then rotate to camera frame using Rc_w
    pt = C.Rc_w*pt

    # Follow classical projection process
    # Generate normalized coords
    x_n = pt[1:1,:]./pt[3:3,:]
    y_n = pt[2:2,:]./pt[3:3,:]
    rsqrd = x_n.^2 + y_n.^2  # radius squared from centre

    # Radial distortion factor
    r_d = 1.0 + C.k1*rsqrd + C.k2*rsqrd.^2 + C.k3*rsqrd.^3

    # Tangential distortion component
    dtx = 2*C.p1*x_n.*y_n         + C.p2*(rsqrd + 2*x_n.^2)
    dty = C.p1*(rsqrd + 2*y_n.^2) + 2*C.p2*x_n.*y_n

    # Obtain lens distorted coordinates
    x_d = r_d.*x_n + dtx
    y_d = r_d.*y_n + dty

    # Finally project to pixel coordinates
    # Note skew represents the 2D shearing coefficient times fx
    x_p = C.ppx + x_d*C.fx + y_d*C.skew
    y_p = C.ppy + y_d*C.fy

    xy = [x_p
	  y_p]

    # If C.rows and C.cols not empty determine points that are within image bounds
    if computevisibility
        if C.rows !=0 && C.cols != 0
            visible = (x_p .>= 1.0) & (x_p .<= convert(Float64,C.cols)) &
                      (y_p .>= 1.0) & (y_p .<= convert(Float64,C.rows))
        else
            warn("Point visibility requested but Camera structure has no image size data")
            visible = []
        end

        return xy, visible
    else
        return xy
    end

end

#--------------------------------------------------------------------------
"""
imagept2plane - Project image points to a plane and return their 3D locations
```
Usage:  pt = imagept2plane(C, xy, planeP, planeN)

Arguments:
         C - Camera structure, Alternatively
             C can be a 3x4 camera projection matrix.
        xy - Image points specified as 2 x N array (x,y) / (col,row)
    planeP - Some point on the plane.
    planeN - Plane normal vector.

Returns:
        pt - 3xN array of 3D points on the plane that the image points
             correspond to.
```
Note that the plane is specified in terms of the world frame by defining a
3D point on the plane, planeP, and a surface normal, planeN.

Lens distortion is handled by using the standard lens distortion
parameters assuming locally that the distortion is constant, computing
the forward distortion and then subtracting the distortion.

See also Camera(), cameraproject()
"""
#=
PK May 2015
=#

function imagept2plane(C::Camera, xy::Array, planeP::Vector, planeN::Vector)

    (dim, N) = size(xy, 1, 2)
    if dim != 2
        error("Image xy data must be a 2 x N array")
    end

    # Reverse the projection process as used by cameraproject()

    # Subtract principal point and divide by focal length to get normalised,
    # distorted image coordinates.  Note skew represents the 2D shearing
    # coefficient times fx
    y_d = (xy[2:2,:] - C.ppy)/C.fy
    x_d = (xy[1:1,:] - C.ppx - y_d*C.skew)/C.fx

    # Radial distortion factor.  Here the squared radius is computed from the
    # already distorted coordinates.  The approximation we are making here is to
    # assume that the distortion is locally constant.
    rsqrd = x_d.^2 + y_d.^2
    r_d = 1 + C.k1*rsqrd + C.k2*rsqrd.^2 + C.k3*rsqrd.^3

    # Tangential distortion component, again computed from the already distorted
    # coords.
    dtx = 2*C.p1*x_d.*y_d         + C.p2*(rsqrd + 2*x_d.^2)
    dty = C.p1*(rsqrd + 2*y_d.^2) + 2*C.p2*x_d.*y_d

    # Subtract the tangential distortion components and divide by the radial
    # distortion factor to get an approximation of the undistorted normalised
    # image coordinates (with no skew)
    x_n = (x_d - dtx)./r_d
    y_n = (y_d - dty)./r_d

    # Define a set of points at the normalised distance of z = 1 from the
    # principal point, these define the viewing rays in terms of the camera
    # frame.
    ray = [x_n; y_n; ones(1,N)]

    # Rotate to get the viewing rays in the world frame
    ray = C.Rc_w'*ray

    # The point of intersection of each ray with the plane will be
    #   pt = C.P + k*ray    where k is to be determined
    #
    # Noting that the vector planeP -> pt will be perpendicular to planeN.
    # Hence the dot product between these two vectors will be 0.
    #  dot((( C.P + k*ray ) - planeP) , planeN)  = 0
    # Rearranging this equation allows k to be solved
    pt = zeros(3,N)
    for n = 1:N
        k = (dot(planeP, planeN) - dot(C.P, planeN)) / dot(ray[:,n], planeN)
        pt[:,n] = C.P + k*ray[:,n]
    end

    return pt
end

# Case when the camera is represented by a projection matrix

function imagept2plane(P::Array, xy::Array, planeP::Vector, planeN::Vector)

    if size(P) != (3,4)
        error("Projection matrix must be 3 x 4")
    end

    return pt = imagept2plane(Camera(P), xy, planeP, planeN)
end

#--------------------------------------------------------------------------
"""
camera2projmatrix - Generate a camera projection matrix from a Camera structure
```
Usage:     P = camera2projmatrix(C)

Argument:  C - Camera structure

Returns:   P - 3x4 camera projection matrix that maps homogeneous 3D world
           coordinates to homogeneous image coordinates.
```
Function takes a camera structure and returns its equivalent projection matrix
ignoring lens distortion parameters etc.

See also: Camera(), projmatrix2camera(), cameraproject()
"""
function camera2projmatrix(C::Camera)

    K = [C.fx  C.skew   C.ppx  0
          0    C.fy     C.ppy  0
          0     0         1    0]

    T = [ C.Rc_w  -C.Rc_w*C.P
          0  0  0       1     ]

    return P = K*T
end

#--------------------------------------------------------------------------
"""
decomposecamera -  Decomposition of a camera projection matrix
```
Usage:  K, Rc_w, Pc, pp, pv = decomposecamera(P)

   P is decomposed into the form P = K*(R -R*Pc)

Argument:  P - 3 x 4 camera projection matrix
Returns:
           K - Calibration matrix of the form
                 |  ax   s   ppx |
                 |   0   ay  ppy |
                 |   0   0    1  |

               Where:
               ax = f/pixel_width and ay = f/pixel_height,
               ppx and ppy define the principal point in pixels,
               s is the camera skew.
        Rc_w - 3 x 3 rotation matrix defining the world coordinate frame
               in terms of the camera frame. Columns of R transposed define
               the directions of the camera X, Y and Z axes in world
               coordinates.
          Pc - Camera centre position in world coordinates.
          pp - Image principal point.
          pv - Principal vector  from the camera centre C through pp
               pointing out from the camera.  This may not be the same as
               R'(:,3) if the principal point is not at the centre of the
               image, but it should be similar.
```
See also: rq3()
"""
#=
Reference: Hartley and Zisserman 2nd Ed. pp 155-164

October  2010  Original version
November 2013  Description of rotation matrix R corrected (transposed)
=#

function decomposecamera{T1<:Real}(P::Array{T1,2})

    if size(P) != (3,4)
        error("Projection matrix must be 3 x 4")
    end

    # Convenience variables for the columns of P
    p1 = P[:,1]
    p2 = P[:,2]
    p3 = P[:,3]
    p4 = P[:,4]

    M = [p1 p2 p3];
    m3 = M[3:3,:]'

    # Camera centre, analytic solution
    X =  det([p2 p3 p4])
    Y = -det([p1 p3 p4])
    Z =  det([p1 p2 p4])
    T = -det([p1 p2 p3])

    Pc = [X;Y;Z;T]
    Pc = Pc/Pc[4]
    Pc = Pc[1:3]     # Make inhomogeneous

    # Pc = null(P,'r'); # numerical way of computing C

    # Principal point
    pp = M*m3
    pp = pp/pp[3]
    pp = pp[1:2]   # Make inhomogeneous

    # Principal ray pointing out of camera
    pv = det(M)*m3
    pv = pv/norm(pv)

    # Perform RQ decomposition of M matrix. Note that rq3 returns K with +ve
    # diagonal elements, as required for the calibration marix.
    (K, Rc_w) = rq3(M)

    # Check that R is right handed, if not give warning
    if dot(cross(Rc_w[:,1], Rc_w[:,2]), Rc_w[:,3]) < 0.0
        info("Warning: rotation matrix is left handed")
    end

    return K, Rc_w, Pc, pp, pv
end

#--------------------------------------------------------------------------
"""
rq3 - RQ decomposition of 3x3 matrix
```
Usage:  R, Q = rq3(A)

Argument:  A - 3 x 3 matrix.
Returns:   R - Upper triangular 3 x 3 matrix
           Q - 3 x 3 orthonormal rotation matrix
               Such that  R*Q = A.
```
The signs of the rows and columns of R and Q are chosen so that the diagonal
elements of R are +ve.

See also: decomposecamera()
"""
#=
Follows algorithm given by Hartley and Zisserman 2nd Ed. A4.1 p 579

October  2010
February 2014  Incorporated modifications suggested by Mathias Rothermel to
               avoid potential division by zero problems
=#

function rq3{T<:Real}(A::Array{T,2})

    if size(A) != (3,3)
        error("A must be 3x3")
    end

    # Find rotation Qx required to set A[3,2] to 0
    A33 = A[3,3] + eps(1e6)  # Avoid side effects when we add eps(1e6)
    c = -A33/sqrt(A33^2+A[3,2]^2)
    s =  A[3,2]/sqrt(A33^2+A[3,2]^2)
    Qx = [1 0 0; 0 c -s; 0 s c]
    R = A*Qx

    # Find rotation Qy required to set A[3,1] to 0
    R[3,3] = R[3,3] + eps(1e6)
    c = R[3,3]/sqrt(R[3,3]^2+R[3,1]^2)
    s = R[3,1]/sqrt(R[3,3]^2+R[3,1]^2)
    Qy = [c 0 s; 0 1 0;-s 0 c]
    R = R*Qy

    # Find rotation Qz required to set A[2,1] to 0
    R[2,2] = R[2,2] + eps(1e6)
    c = -R[2,2]/sqrt(R[2,2]^2+R[2,1]^2)
    s =  R[2,1]/sqrt(R[2,2]^2+R[2,1]^2)
    Qz = [c -s 0; s c 0; 0 0 1]
    R = R*Qz

    Q = Qz'*Qy'*Qx'

    # Adjust R and Q so that the diagonal elements of R are +ve
    for n = 1:3
        if R[n,n] < 0
            R[:,n] = -R[:,n]
            Q[n,:] = -Q[n,:]
        end
    end

    return R, Q
end

#--------------------------------------------------------------------------
"""
makehomogeneous - Appends a scale of 1 to array inhomogeneous coordinates
```
Usage:  hx = makehomogeneous(x)

Argument:
        x  - an N x npts array of inhomogeneous coordinates.

Returns:
        hx - an (N+1) x npts array of homogeneous coordinates with the
             homogeneous scale set to 1
```
See also: makeinhomogeneous(), hnormalise()
"""
# April 2010

function makehomogeneous{T<:Real}(x::Array{T,2})
    return [x; ones(1,size(x,2))]
end

function makehomogeneous{T<:Real}(x::Vector{T})
    return [x;1]
end

#--------------------------------------------------------------------------
"""
makeinhomogeneous - Converts homogeneous coords to inhomogeneous coordinates
```
Usage:  x = makehomogeneous(hx)

Argument:
        hx  - an N x npts array of homogeneous coordinates.

Returns:
        x - an (N-1) x npts array of inhomogeneous coordinates
```
Warning:  If there are any points at infinity (scale = 0) the coordinates
of these points are simply returned minus their scale coordinate.

See also: makehomogeneous(), hnormalise()
"""
function makeinhomogeneous{T<:Real}(hx::Array{T,2})

    x = hnormalise(hx)  # Normalise to scale of one
    x = x[1:end-1,:]    # Extract all but the last row
    return x
end

function makeinhomogeneous{T<:Real}(hx::Vector{T})

    x = hnormalise(hx)  # Normalise to scale of one
    x = x[1:end-1]      # Extract all but the last row
    return x
end

#---------------------------------------------------------------------------
"""
hnormalise - Normalises array of homogeneous coordinates to a scale of 1
```
Usage:  nx = hnormalise(x)

Argument:
        x  - an Nxnpts array of homogeneous coordinates.

Returns:
        nx - an Nxnpts array of homogeneous coordinates rescaled so
             that the scale values nx[end,:] are all 1.
```
Note that any homogeneous coordinates at infinity (having a scale value of
0) are left unchanged.

See also: hnormalise!()
"""

function hnormalise{T<:Real}(x::Union{Array{T,2},Vector{T}})
    hnormalise!(copy(x))
end

#---------------------------------------------------------------------------
"""
hnormalise! - In-place normalisation of homogeneous coordinates to a scale of 1
```
Usage:   hnormalise!(x)

Argument:
        x - An Nxnpts array of homogeneous coordinates.

Return value and side effect:
        x - The homogeneous coordinates in x are rescaled so
            that the scale values x[end,:] are all 1.
```
Note that any homogeneous coordinates at infinity (having a scale value of
0) are left unchanged.

See also: hnormalise()

"""

# Inplace version
function hnormalise!{T<:Real}(x::Union{Array{T,2},Vector{T}})

    (rows, npts) = size(x, 1, 2)

    for n = 1:npts
        if abs(x[rows, n]) > eps(T)   # point not at infinity
            for r = 1:rows-1
                x[r, n] /= x[rows, n]
            end
            x[rows, n] = 1
        end
    end

    return x
end

#---------------------------------------------------------------------------
"""
homography1d - Computes 1D homography
```
Usage:   H = homography1d(x1, x2)

Arguments:
         x1  - 2xN set of homogeneous points
         x2  - 2xN set of homogeneous points such that x1<->x2
Returns:
          H - the 2x2 homography such that x2 = H*x1
```
This code is modelled after the normalised direct linear transformation
algorithm for the 2D homography given by Hartley and Zisserman p92.
"""

# May 2003
# February 2016 ported to Julia

function homography1d{T1<:Real,T2<:Real}(x1::Array{T1,2}, x2::Array{T2,2})

    if size(x1) != size(x2)
        error("x1 and x2 must have same dimensions")
    end

    (dim, Npts) = size(x1, 1, 2)

    # Attempt to normalise each set of points so that the origin
    # is at centroid and mean distance from origin is 1.
    (x1n, Tx1) = normalise1dpts(x1)
    (x2n, Tx2) = normalise1dpts(x2)

    # Note that it may have not been possible to normalise
    # the points if one was at infinity so the following does not
    # assume that scale parameter w = 1.
    A = zeros(2*Npts,4)

    for n = 1:Npts
        X = x1n[:,n]'
        x = x2n[1,n]
        w = x2n[2,n]
        A[n,:] = [-w*X x*X]
    end

    (U,S,V) = svd(A)

    # Extract homography
    H = reshape(V[:,4],2,2)'

    # Denormalise
    return H = Tx2\H*Tx1
end

#---------------------------------------------------------------------------
"""
homography2d - Computes 2D homography
```
Usage 1:   H = homography2d(x1, x2)

Arguments:
         x1  - 3xN set of homogeneous points
         x2  - 3xN set of homogeneous points such that x1<->x2

Usage 2:    H = homography2d(x)
Argument:
          x  - If a single argument is supplied it is assumed that it
               is in the form x = [x1; x2]

Returns:
         H - the 3x3 homography such that x2 = H*x1
```
Usage 2 is intended for use with ransac()

This code follows the normalised direct linear transformation
algorithm given by Hartley and Zisserman "Multiple View Geometry in
Computer Vision" p92.
"""
#=
May 2003  - Original version.
Feb 2004  - Single argument allowed for to enable use with RANSAC.
June 2015 - Ported to Julia
=#

function homography2d{T1<:Real,T2<:Real}(x1::Array{T1,2}, x2::Array{T2,2})

    if size(x1) != size(x2)
        error("x1 and x2 must have same dimensions")
    end

    (dim, Npts) = size(x1, 1, 2)

    if dim != 3
        error("pts must be 3xN")
    end

    if Npts < 4
        error("Number of input points must match and be >= 4")
    end

    # Attempt to normalise each set of points so that the origin
    # is at centroid and mean distance from origin is sqrt(2).
    (x1n, Tx1) = normalise2dpts(x1)
    (x2n, Tx2) = normalise2dpts(x2)

    # Note that it may have not been possible to normalise
    # the points if one was at infinity so the following does not
    # assume that the scale parameter w = 1.
    A = zeros(3*Npts, 9)

    O = [0. 0. 0.]
    for n = 1:Npts
	X = x1n[:,n]'
	x = x2n[1,n]
        y = x2n[2,n]
        w = x2n[3,n]
	A[3*n-2,:] = [  O  -w*X  y*X]
	A[3*n-1,:] = [ w*X   O  -x*X]
	A[3*n  ,:] = [-y*X  x*X   O ]
    end

    (U,S,V) = svd(A)

    H = reshape(V[:,9],3,3)'  # Extract homography
    return H = Tx2\H*Tx1               # and denormalise
end

# Version with a single argument for use with ransac()
function homography2d{T<:Real}(x::Array{T,2})

    (dim, npts) = size(x, 1, 2)

    if dim != 6
        error("pts must be a 6xN array")
    end

    if npts < 4
        error("Number of input points must match and be >= 4")
    end

    return homography2d(x[1:3,:], x[4:6,:])
end

#---------------------------------------------------------------------------
"""
normalise1dpts - Normalises 1D homogeneous points

Function translates and normalises a set of 1D homogeneous points
so that their centroid is at the origin and their mean distance from
the origin is 1.
```
Usage:   (newpts, T) = normalise1dpts(pts)

Argument:
  pts -  2xN array of 2D homogeneous coordinates

Returns:
  newpts -  2xN array of transformed 1D homogeneous coordinates
  T      -  The 2x2 transformation matrix, newpts = T*pts
```
Note that if one of the points is at infinity no normalisation
is possible.  In this case a warning is printed and pts is
returned as newpts and T is the identity matrix.

See also: normalise2dpts()
"""

# ? Can this be unified with normalise2dpts ?

function normalise1dpts{T1<:Real}(ptsa::Array{T1,2})

    pts = copy(ptsa) # Copy because we alter ptsa (should be able to fix this)

    if size(pts,1) != 2
        error("pts must be 2xN")
    end

    if any(pts[2,:] .== 0)
        warning("Attempt to normalise a point at infinity")
        return pts, eye(2)
    end

    # Ensure homogeneous coords have scale of 1
    pts[1,:] = pts[1,:]./pts[2,:]

    c = mean(pts[1,:])        # Centroid.
    newp = pts[1,:] - c       # Shift origin to centroid.

    meandist = mean(abs(newp))
    scale = 1/meandist

    T = [scale    -scale*c
         0         1      ]

    newpts = T*pts

    return newpts, T
end

#---------------------------------------------------------------------------
"""
normalise2dpts - Normalises 2D homogeneous points
```
Usage:   (newpts, T) = normalise2dpts(pts)

Argument:
     pts -  3xN array of 2D homogeneous coordinates.

Returns:
  newpts -  3xN array of transformed 2D homogeneous coordinates.  The
            scaling parameter is normalised to 1 unless the point is at
            infinity.
  T      -  The 3x3 transformation matrix, newpts = T*pts.
```
Function translates and normalises a set of 2D homogeneous points
so that their centroid is at the origin and their mean distance from
the origin is sqrt(2).  This process typically improves the
conditioning of any equations used to solve homographies, fundamental
matrices etc.

If there are some points at infinity the normalisation transform
is calculated using just the finite points.  Being a scaling and
translating transform this will not affect the points at infinity.
"""
#=
May 2003      - Original version
February 2004 - Modified to deal with points at infinity.
June 2015     - Ported to Julia
=#

function normalise2dpts{Ty<:Real}(ptsa::Array{Ty,2})

    pts = copy(ptsa) # Copy because we alter ptsa (should be able to fix this)
    newp = zeros(pts)

    if size(pts,1) != 3
        error("pts must be 3xN")
    end

    # Find the indices of the points that are not at infinity
    finiteind = find(abs(pts[3,:]) .> eps())

    # Should this worning be made?
    if length(finiteind) != size(pts,2)
        warning("Some points are at infinity")
    end

    # For the finite points ensure homogeneous coords have scale of 1
    pts[1,finiteind] = pts[1,finiteind]./pts[3,finiteind]
    pts[2,finiteind] = pts[2,finiteind]./pts[3,finiteind]
    pts[3,finiteind] = 1.0

    c = mean(pts[1:2,finiteind],2)            # Centroid of finite points
    newp[1,finiteind] = pts[1,finiteind]-c[1] # Shift origin to centroid.
    newp[2,finiteind] = pts[2,finiteind]-c[2]

    dist = sqrt(newp[1,finiteind].^2 + newp[2,finiteind].^2)
    meandist = mean(dist)

    scale = sqrt(2.0)/meandist

    T = [scale   0.  -scale*c[1]
         0.    scale -scale*c[2]
         0.      0.     1.      ]

    newpts = T*pts

    return newpts, T
end

#------------------------------------------------------------------------
"""
skew - Constructs 3x3 skew-symmetric matrix from 3-vector
```
Usage:  s = skew(v)

Argument:  v - 3-vector
Returns:   s - 3x3 skew-symmetric matrix
```
The cross product between two vectors, a x b can be implemented as a matrix
product  skew(a)*b
"""
function skew{T<:Real}(v::Array{T})

    @assert length(v) == 3  "Input must be a 3x1 array or vector"

    s = [ 0.  -v[3]  v[2]
         v[3]   0.  -v[1]
        -v[2]  v[1]   0. ]
end

#------------------------------------------------------------------------
"""
hcross - Homogeneous cross product, result normalised to s = 1.

Function to form cross product between two points, or lines,
in homogeneous coodinates.  The result is normalised to lie
in the scale = 1 plane.
```
Usage: c = hcross(a,b)

Arguments:  a, b  - 3x1 arrays or vectors
Returns:       c  - 3-vector
```
"""
function hcross{T1<:Real, T2<:Real}(a::Array{T1}, b::Array{T2})
    @assert length(a) == 3 && length(b) == 3  "Input must be a 3x1 arrays or vectors"
    c = cross(vec(a), vec(b))
    c = c/c[3]
end

#------------------------------------------------------------------------
"""
fundmatrix - Computes fundamental matrix from 8 or more points

Function computes the fundamental matrix from 8 or more matching points in
a stereo pair of images.  The normalised 8 point algorithm given by
Hartley and Zisserman p265 is used.  To achieve accurate results it is
recommended that 12 or more points are used
```
Usage:   (F, e1, e2) = fundmatrix(x1, x2)
         (F, e1, e2) = fundmatrix(x)

Arguments:
         x1, x2 - Two sets of corresponding 3xN set of homogeneous
         points.

         x      - If a single argument is supplied it is assumed that it
                  is in the form x = [x1; x2]
Returns:
         F      - The 3x3 fundamental matrix such that x2'*F*x1 = 0.
         e1     - The epipole in image 1 such that F*e1 = 0
         e2     - The epipole in image 2 such that F'*e2 = 0
```

Usage with a single argument is intended for the use of this function
with ransac()

See also: affinefundmatrix()
"""
#=
Feb 2002  - Original version.
May 2003  - Tidied up and numerically improved.
Feb 2004  - Single argument allowed to enable use with RANSAC.
Mar 2005  - Epipole calculation added, 'economy' SVD used.
Feb 2016  - Ported to Julia
=#

function  fundmatrix{T1<:Real,T2<:Real}(x1i::Array{T1,2}, x2i::Array{T2,2}; epipoles=false)

    if size(x1i) != size(x2i)
        error("x1 and x2 must have same dimensions")
    end

    (dim, npts) = size(x1i, 1, 2)

    if dim != 3
        error("pts must be a 3xN array of homogeneous coords")
    end

    if npts < 8
        error("Number of input points must match and be >= 8")
    end

    # Normalise each set of points so that the origin
    # is at centroid and mean distance from origin is sqrt(2).
    # normalise2dpts also ensures the scale parameter is 1.
    (x1, Tx1) = normalise2dpts(x1i)
    (x2, Tx2) = normalise2dpts(x2i)

    # Build the constraint matrix
    # The following hopefully works for both v0.4 and v0.5
    A = [x2[1:1,:]'.*x1[1:1,:]'   x2[1:1,:]'.*x1[2:2,:]'  x2[1:1,:]'  x2[2:2,:]'.*x1[1:1,:]'   x2[2:2,:]'.*x1[2:2,:]'  x2[2:2,:]'   x1[1:1,:]'   x1[2:2,:]'  ones(npts,1) ]

    # Build A transposed so that we can format it in code, and then transpose it
#=
    A = [x2[1:1,:].*x1[1:1,:]
         x2[1:1,:].*x1[2:2,:]
         x2[1:1,:]
         x2[2:2,:].*x1[1:1,:]
         x2[2:2,:].*x1[2:2,:]
         x2[2:2,:]
         x1[1:1,:]
         x1[2:2,:]
         ones(1,npts) ]'

println(A)
=#
    (U,D,V) = svd(A,thin=false)

    # Extract fundamental matrix from the column of V corresponding to
    # smallest singular value.
    F = reshape(V[:,9], 3, 3)'

    # Enforce constraint that fundamental matrix has rank 2 by performing
    # a svd and then reconstructing with the two largest singular values.
    (U,D,V) = svd(F)
    F = U*diagm([D[1], D[2], 0])*V'

    # Denormalise
    F = Tx2'*F*Tx1

    if epipoles
        # Solve for epipoles
        (U,D,V) = svd(F)
        e1 = hnormalise(V[:,3])
        e2 = hnormalise(U[:,3])

        return F, e1, e2
    else
        return F
    end
end

# Version with a single argument for use with ransac()
function fundmatrix{T<:Real}(x::Array{T,2})

    (dim, npts) = size(x, 1, 2)

    if dim != 6
        error("pts must be a 6xN array")
    end

    if npts < 8
        error("Number of input points must match and be >= 8")
    end

    return fundmatrix(x[1:3,:], x[4:6,:], epipoles=false)
end

#--------------------------------------------------------------------------
"""
affinefundmatrix - Computes affine fundamental matrix from 4 or more points

Function computes the affine fundamental matrix from 4 or more matching
points in a stereo pair of images.
```
Usage:   (F, e1, e2) = affinefundmatrix(x1, x2)
         (F, e1, e2) = affinefundmatrix(x)

Arguments:
         x1, x2 - Two sets of corresponding points defined as
                  2xN arrays of inhomogeneous image coordinates.

         x      - If a single argument is supplied it is assumed that it
                  is in the form x = [x1; x2]
Returns:
         F      - The 3x3 fundamental matrix such that x2'*F*x1 = 0.
         e1     - The epipole in image 1 such that F*e1 = 0
         e2     - The epipole in image 2 such that F'*e2 = 0
```
Usage with a single argument is intended for the use of this function
with ransac()

The Gold Standard algorithm given by Hartley and Zisserman p351 (2nd
Ed.) is used.

See also: fundmatrix()
"""

# ** Inconsistent in that arguments are inhomogeneous but to use them
# ** with the fundamental matrix you have to make then homogeneous **

# Feb 2005  Original version
# Feb 2016  Ported to Julia

function affinefundmatrix{T1<:Real,T2<:Real}(x1::Array{T1,2}, x2::Array{T2,2})

    if size(x1) != size(x2)
        error("x1 and x2 must have same dimensions")
    end

    (dim, npts) = size(x1, 1, 2)

    if dim != 2
        error("pts must be a 2xN array of inhomogeneous image coords")
    end

    if npts < 4
        error("Number of input points must match and be >= 4")
    end

    X = [x2; x1]       # Form vectors of correspondences
    Xmean = mean(X,2)  # Mean

    deltaX = zeros(size(X))
    for k = 1:4
	deltaX[k,:] = X[k,:] - Xmean[k]
    end

    (U,D,V) = svd(deltaX')

    # Form  fundamental matrix from the column of V corresponding to
    # smallest singular value.
    v = V[:,4]
    F = [ 0    0    v[1]
	  0    0    v[2]
	 v[3] v[4] -v'*Xmean]

    # Solve for epipoles
    (U,D,V) = svd(F)
    e1 = V[:,3]
    e2 = U[:,3]

    return F, e1, e2
end

# Version with a single argument for use with ransac()
function affinefundmatrix{T<:Real}(x::Array{T,2})

    (dim, npts) = size(x)

    if dim != 4
        error("pts must be a 4xN array")
    end

    if npts < 4
        error("Number of input points must match and be >= 4")
    end

    return affinefundmatrix(x[1:2,:], x[3:4,:])
end

#--------------------------------------------------------------------
"""
fundfromcameras - Fundamental matrix from camera matrices or structures
```
Usage: F = fundfromcameras(P1, P2)
       F = fundfromcameras(C1, C2)

Arguments:  P1, P2 - Two 3x4 camera projection matrices or
            C1, C2 - Two Camera structres

Returns:    F      - Fundamental matrix relating the two camera views.
```
See also: fundmatrix(), affinefundmatrix(), Camera()
"""
# Reference: Hartley and Zisserman p244

# Version for projection matrices
function fundfromcameras{T1<:Real, T2<:Real}(P1::Array{T1,2}, P2::Array{T2,2})

    if (size(P1) != (3,4)) || (size(P2) != (3,4))
        error("Camera matrices must be 3x4")
    end

    C1 = nullspace(P1)  # Camera centre 1 is the null space of P1
    e2 = P2*C1          # epipole in camera 2

    e2x = [  0   -e2[3] e2[2]    # Skew symmetric matrix from e2
            e2[3]    0  -e2[1]
           -e2[2]  e2[1]   0  ]

    return F = e2x*P2*pinv(P1)
end

# Version using Camera structures
function fundfromcameras(C1::Camera, C2::Camera)
    return fundfromcameras(camera2projmatrix(C1), camera2projmatrix(C2))
end

#----------------------------------------------------------------------
"""
idealimagepts - Ideal image points with no distortion.

```
Usage:  xyideal = idealimagepts(C, xy)

Arguments:
         C - Camera structure
        xy - Image points specified as 2 x N array (x,y) / (col,row)

Returns:
   xyideal - Ideal image points.  These points correspond to the image
             locations that would be obtained if the camera had no lens
             distortion.  That is, if they had been projected using an ideal
             projection matrix computed from fx, fy, ppx, ppy, skew.
```

See also Camera(), cameraproject()
"""

# August   2015 Adapted from imagept2plane
# February 2016 Refined inversion of distortion process slightly
#               Ported to Julia

function idealimagepts{T<:Real}(C::Camera, xy::Union{Array{T,2}, Vector{T}})

    if size(xy, 1) != 2
        error("Image xy data must be a 2 x N array")
    end

    # Reverse the projection process as used by cameraproject()

    # Subtract principal point and divide by focal length to get normalised,
    # distorted image coordinates.  Note skew represents the 2D shearing
    # coefficient times fx
    y_d = (xy[2,:] - C.ppy)/C.fy
    x_d = (xy[1,:] - C.ppx - y_d*C.skew)/C.fx

    # Compute the inverse of the lens distortion effect by computing the
    # 'forward' direction lens distortion at this point and then subtracting
    # this from the current point.

    # Radial distortion factor. Here the squared radius is computed from the
    # already distorted coordinates.  The approximation we are making here is to
    # assume that the distortion is locally constant.
    rsqrd = x_d.^2 + y_d.^2
    r_d = 1 + C.k1*rsqrd + C.k2*rsqrd.^2 + C.k3*rsqrd.^3

    # Tangential distortion component, again computed from the already distorted
    # coords.
    dtx = 2*C.p1*x_d.*y_d         + C.p2*(rsqrd + 2*x_d.^2)
    dty = C.p1*(rsqrd + 2*y_d.^2) + 2*C.p2*x_d.*y_d

    # Subtract the tangential distortion components and divide by the radial
    # distortion factor to get an approximation of the undistorted normalised
    # image coordinates (with no skew)
    x_n = (x_d - dtx)./r_d
    y_n = (y_d - dty)./r_d

    # Finally project back to pixel coordinates.
    x_p = C.ppx + x_n*C.fx + y_n*C.skew
    y_p = C.ppy + y_n*C.fy

    return xyideal = [x_p
                      y_p]
end

#--------------------------------------------------------------------------
"""
solvestereopt - Homogeneous linear solution of a stereo point
```
Usage:
        pt = solvestereopt(xy, P)
        pt = solvestereopt(xy, C)
        (pt, xy_reproj) = solvestereopt(xy, P, reprojecterror=true)
        (pt, xy_reproj) = solvestereopt(xy, C, reprojecterror=true)

Multiview stereo: Solves 3D location of a point given image coordinates of
that point in two, or more, images.

Arguments:    xy - 2xN matrix of x, y image coordinates, one column for
                   each camera.
               P - N array of corresponding 3x4 image projection
                   matrices, or
               C - An N array of Camera structures
  reprojecterror - Boolean flag indicating whether reprojection errors should be retunred.

Returns:      pt - 3D location in space returned in normalised
                   homogeneous coordinates (a 4-vector with last element = 1)
       xy_reproj - 2xN matrix of reprojected image coordinates.
```

See also: idealimagepts(), camstruct2projmatrix(), Camera()
"""

function solvestereopt{T1<:Real,T2<:Real}(xy::Array{T1,2}, P::Array{Array{T2,2}}; reprojecterror=false)

    (dim,N) = size(xy, 1, 2)
    @assert N == length(P) "Number of points and cameras must match"
    @assert dim == 2  "Image coordinates must be a 2xN array of inhomogeneous coords"
    @assert N >= 2  "Must have at least 2 camera views"

    # Build eqn of the form A*pt = 0
    A = zeros(2*N, 4)
    for n = 1:N
	A[2*n-1,:] = xy[1,n]*P[n][3,:] - P[n][1,:]
	A[2*n  ,:] = xy[2,n]*P[n][3,:] - P[n][2,:]
    end

    (~,~,v) = svd(A)
    pt = hnormalise(v[:,4])

    if reprojecterror
        # Project the point back into the source images to determine the residual
        # error
        xy_reproj = zeros(size(xy))
        for n = 1:N
            xy_reproj[:,n] = cameraproject(P[n], pt[1:3])
        end

        return pt, xy_reproj
    else
        return pt
    end

end

# Version where the camera data is an array of Camera structures
function solvestereopt{T<:Real}(xy::Array{T,2}, C::Array{Camera}; reprojecterror=false)

    (dim,N) = size(xy, 1, 2)
    @assert N == length(C) "Number of points and cameras must match"
    @assert dim == 2  "Image coordinates must be a 2xN array of inhomogeneous coords"
    @assert N >= 2  "Must have at least 2 camera views"

    # Correct image points from each camera so that they correspond to image
    # points from an ideal camera with no lens distortion.  Also generate the
    # corresponding ideal projection matrices for each Camera structure
    xyideal = zeros(size(xy)) # copy xy so that xy is not changed
    for n = 1:N
        xyideal[:,n] = idealimagepts(C[n], xy[:,n])
        P[n] = camstruct2projmatrix(C[n])
    end

    return solvestereopt(xyideal, P, reprojecterror=reprojecterror)
end

#---------------------------------------------------------------------
"""
undistortimage - Removes lens distortion from an image

```
Usage 1:  nimg = undistortimage(img, f, ppx, ppy, k1, k2, k3, p1, p2)

Arguments:
         img - Image to be corrected.
           f - Focal length in terms of pixel units
               (focal_length_mm/pixel_size_mm)
    ppx, ppy - Principal point location in pixels.
  k1, k2, k3 - Radial lens distortion parameters.
      p1, p2 - Tangential lens distortion parameters.

Usage 2.  nimg = undistortimage(img, camera)

Arguments:
         img - Image to be corrected.
         cam - Camera structure.

Returns:
         nimg - Corrected image.
```

It is assumed that radial and tangential distortion parameters are
computed/defined with respect to normalised image coordinates corresponding
to an image plane 1 unit from the projection centre.  This is why the
focal length is required.

"""

# ** Allow for separate valuees of fx and fy ??

# Version for image represented by 2D array
function undistortimage{T<:Real}(img::Array{T,2}, f::Real,
                       ppx::Real, ppy::Real, k1::Real, k2::Real, k3::Real,
                       p1::Real, p2::Real)

    # Strategy: Generate a grid of coordinate values corresponding to
    # an ideal undistorted image.  We then apply the imaging process
    # to these coordinates, including lens distortion, to obtain the
    # actual distorted image locations.  In this process these
    # distorted image coordinates end up being stored in a matrix that
    # is indexed via the original ideal, undistorted coords.  Thus for
    # every undistorted pixel location we can determine the location
    # in the distorted image that we should map the grey value from.

    (rows, cols) = size(img)
    nimg = zeros(size(img))

    # Set up interpolant
    itp = interpolate(img, BSpline(Linear()), OnCell())

    # Interpolate values from distorted image locations to their
    # ideal locations.
    for xu = 1:cols, yu = 1:rows
        (xd, yd) = distortedcoords(xu, yu, f, ppx, ppy, k1, k2, k3, p1, p2)
        nimg[yu,xu] = itp[yd, xd]
    end

    return nimg
end

function undistortimage(img, cam::Camera)
    undistortimage(img, cam.fx, cam.ppx, cam.ppy, cam.k1, cam.k2, cam.k3, cam.p1, cam.p2)
end

# Version for image represented by 3D array
function undistortimage{T<:Real}(img::Array{T,3}, f::Real,
                       ppx::Real, ppy::Real, k1::Real, k2::Real, k3::Real,
                       p1::Real, p2::Real)

    (rows, cols, channels) = size(img)
    nimg = zeros(size(img))

    # Interpolate values from distorted image locations to their
    # ideal locations. ** should construct array of interpolants **
    for chan = 1:channels
        itp = interpolate(img[:,:,chan], BSpline(Linear()), OnCell())

        for xu = 1:cols, yu = 1:rows
            (xd, yd) = distortedcoords(xu, yu, f, ppx, ppy, k1, k2, k3, p1, p2)

            if xd >= 1 && xd <= cols && yd >= 1 && yd <= rows
                nimg[yu, xu, chan] = itp[yd, xd]
            end
        end
    end

    return nimg
end

# Unexported function used by undistortimage()

function distortedcoords(xu::Real, yu::Real, f::Real, ppx::Real, ppy::Real,
                         k1::Real, k2::Real, k3::Real, p1::Real, p2::Real)

    # Convert undistorted x, y values to normalised values with the
    # origin at the principal point.  Dividing pixel coordinates by
    # the focal length (defined in pixels) gives us normalised coords
    # corresponding to z = 1
    x = (xu-ppx)/f
    y = (yu-ppy)/f

    # Radial lens distortion component
    r2 = x^2 + y^2                  # Squared normalized radius.
    dr = k1*r2 + k2*r2^2 + k3*r2^3  # Distortion scaling factor.

    # Tangential distortion component (Beware of different p1,p2
    # orderings used in the literature)
    dtx =    2*p1*x*y      +  p2*(r2 + 2*x^2)
    dty = p1*(r2 + 2*y^2)  +    2*p2*x*y

    # Apply the radial and tangential distortion components to x and y
    x = x + dr*x + dtx
    y = y + dr*y + dty

    # Now rescale by f and add the principal point back to get
    # distorted x and y coordinates.
    xd = x*f + ppx
    yd = y*f + ppy

    return (xd, yd)

end

#--------------------------------------------------------------------------
"""
hline - Plot a 2D line defined in homogeneous coordinates.

Function for ploting 2D homogeneous lines defined by 2 points
or a line defined by a single homogeneous vector
```
Usage 1:   hline(p1,p2, linestyle="b-")
Arguments: p1, p2 -  Two 3-vectors defining points in homogeneous
                     coordinates

Usage 2:    hline(l, linestyle="b-")
Argument:   l - A 3-vector defining a line in homogeneous coordinates

```
Note that in the case where a homogeneous line is supplied as the
argument the extent of the line drawn depends on the current axis
limits.  This will require you to set the desired limits with a call
to PyPlot.axis() prior to calling this function.

Side effect: PyPlot hold() state will be set to true
"""

# Using PyPlot

# Case when 2 homogeneous points are supplied
function hline(p1i::Vector, p2i::Vector, linestyle::Compat.ASCIIString="b-")

    p1 = p1i./p1i[3]    # make sure homogeneous points lie in z=1 plane
    p2 = p2i./p2i[3]

    hold(true)
    plot([p1[1], p2[1]], [p1[2], p2[2]], linestyle);
end

# Case when homogeneous line is supplied
function hline(li::Vector, linestyle::Compat.ASCIIString="b-")

    l = li./li[3]   # ensure line in z = 1 plane (not needed?)

    if abs(l[1]) > abs(l[2])   # line is more vertical
        p1 = hcross(l, [0, -1, PyPlot.ylim()[1]])
        p2 = hcross(l, [0, -1, PyPlot.ylim()[2]])

    else                       # line more horizontal
        p1 = hcross(l, [-1, 0, PyPlot.xlim()[1]])
        p2 = hcross(l, [-1, 0, PyPlot.xlim()[2]])
    end

    hold(true)
    plot([p1[1], p2[1]], [p1[2], p2[2]], linestyle);
end

#--------------------------------------------------------------------------
"""
plotcamera - Plots graphical representation of camera(s) showing pose.

```
Usage: plotcamera(C, l; col=[0,0,1], plotCamPath=false, fig=1)

Arguments:
           C - Camera structure (or array of Camera structures)
           l - The length of the sides of the rectangular cone indicating
               the camera's field of view.
Keyword Arguments:
         col - Optional three element vector specifying the RGB colour to
               use. Defaults to blue.
 plotCamPath - Optional flag true/false to plot line joining camera centre
               positions. If omitted or empty defaults to false.
         fig - Optional figure number to be used. Defaults to 1.
```

The function plots into the current figure a graphical representation of one
or more cameras showing their pose.  This consists of a rectangular cone,
with its vertex at the camera centre, indicating the camera's field of view.
The camera's coordinate X and Y axes are also plotted at the camera centre.

See also: Camera
"""

function plotcamera(Ci, l; col=[0,0,1], plotCamPath=false, fig=1)

    if isa(Ci, Array)
        C = Ci
    else
        C = Vector(1)
        C[1] = Ci
    end

    figure(fig)
    hold(true)
    for i = 1:length(C)

        if C[i].rows == 0 || C[i].cols == 0
            warning("Camera rows and cols not specified")
            continue
        end

        f = C[i].fx  # Use fx as the focal length

        if i > 1 && plotCamPath
            plot3D([C[i-1].P[1], C[i].P[1]],
                   [C[i-1].P(2), C[i].P[2]],
                   [C[i-1].P(3), C[i].P[3]])
        end

        # Construct transform from camera coordinates to world coords
        Tw_c = [C[i].Rc_w'  C[i].P
                 0 0 0        1  ]

        # Generate the 4 viewing rays that emanate from the principal point and
        # pass through the corners of the image.
        ray = zeros(3,4)
        ray[:,1] = [-C[i].cols/2, -C[i].rows/2, f]
        ray[:,2] = [ C[i].cols/2, -C[i].rows/2, f]
        ray[:,3] = [ C[i].cols/2,  C[i].rows/2, f]
        ray[:,4] = [-C[i].cols/2,  C[i].rows/2, f]

        # Scale rays to distance l from the focal plane and make homogeneous
        ray = makehomogeneous(ray*l/f)
        ray = Tw_c*ray                 # Transform to world coords

        for n = 1:4             # Draw the rays
            plot3D([C[i].P[1], ray[1,n]],
                   [C[i].P[2], ray[2,n]],
                   [C[i].P[3], ray[3,n]],
                   color=col)
        end

        # Draw rectangle joining ends of rays
        plot3D([ray[1,1], ray[1,2], ray[1,3], ray[1,4], ray[1,1]],
               [ray[2,1], ray[2,2], ray[2,3], ray[2,4], ray[2,1]],
               [ray[3,1], ray[3,2], ray[3,3], ray[3,4], ray[3,1]],
               color=col)

        # Draw and label axes
        X = Tw_c[1:3,1]*l + C[i].P
        Y = Tw_c[1:3,2]*l + C[i].P
        Z = Tw_c[1:3,3]*l + C[i].P

        plot3D([C[i].P[1], X[1,1]], [C[i].P[2], X[2,1]], [C[i].P[3], X[3,1]],
               color=col)
        plot3D([C[i].P[1], Y[1,1]], [C[i].P[2], Y[2,1]], [C[i].P[3], Y[3,1]],
               color=col)
        #    plot3D([C[i].P[1], Z(1,1)], [C[i].P[2], Z(2,1)], [C[i].P[3], Z(3,1)],...
        #           color=col)
        text3D(X[1], X[2], X[3], "X", color=col)
        text3D(Y[1], Y[2], Y[3], "Y", color=col)

    end

end


#=

Author: Shunsuke Hori

=#

module BGP
using LS
function baseline(;
    beta = 0.9,
    psi = 0.69,
    Pi = 0.5 *ones(2,2),
    G = [0.1,0.2],
    Theta = ones(2),
    transfers = false)
    #derivatives of utiltiy function
    U(c,n) = log(c) + psi*log(1-n)
    Uc(c,n) = 1./c
    Ucc(c,n) = -c.^(-2.0)
    Un(c,n) = -psi./(1.0-n)
    Unn(c,n) = -psi./(1.0-n).^2.0
    n_less_than_one = true
    return LS.Para(beta, Pi, G, Theta, transfers,
                    U, Uc, Ucc, Un, Unn, n_less_than_one)
end

#Model 1
M1 = baseline()

#Model 2

M2 = baseline()
M2.G = [0.15]
M2.Pi = ones(1,1)
M2.Theta = [1.0]
end


using ComplexBigMatrices
using LinearAlgebra
using Test

@testset "ComplexBigMatrices.jl" begin
    # Create Float64 and BigFloat Matrices
    setprecision(512)
    a = [[1 1.0im];[1.0im 1]];
    biga = Complex{BigFloat}.(a)

    # @test sum(Float64.(abs.(eigen(a).vectors - eigen(biga).vectors))) = 0.0
    @test sum(Float64.(abs.(exp(a) - exp(biga)))) < 1e-12
    @test sum(Float64.(abs.(log(a) - log(biga)))) < 1e-12
end


using Images, ImageFiltering, FileIO

function path_weights(weights)
	ret = copy(weights)
	dim = size(weights)
	for i in 2 : dim[1]
		for j in 1 : dim[2]
			min = ret[i - 1, j]
			if j > 1 && ret[i - 1, j - 1] < min
				min = ret[i - 1, j - 1]
			end
			if j < dim[2] && ret[i - 1, j + 1] < min
				min = ret[i - 1, j + 1]
			end
			ret[i, j] += min
		end
	end
	return ret
end

function back_track(weights)
	dim = size(weights)

	buffer::Array{Int, 1} = ones(Int, dim[1])

	buffer[1] = 1
	for i in 2 : dim[2]
		if weights[dim[1], i] < weights[dim[1], buffer[1]]
			buffer[1] = i
		end
	end

	for i in dim[1] - 1 : -1 : 1
		k = dim[1] - i + 1
		j = buffer[k - 1]
		buffer[k] = j
		if j > 1 && weights[i, j - 1] < weights[i, buffer[k]]
			buffer[k] = j - 1
		end
		if j < dim[2] && weights[i, j + 1] < weights[i, buffer[k]]
			buffer[k] = j + 1
		end
	end

	ret::Array{Tuple{Int, Int}, 1} = Array{Tuple{Int, Int}, 1}(undef, dim[1])

	for i in 1 : dim[1]
		ret[i] = dim[1] - i + 1, buffer[i]
	end

	return ret
end

function gradiant(image)
	kernel = [1 0 -1; 2 0 -2;1 0 -1]
	sobel_x = imfilter(image, centered(kernel))
	grad = imfilter(sobel_x, centered(kernel'))
end

function absolute_gradiant(image)
	height, width = size(image)
	grad = gradiant(image)
	reshape([abs(pixel.r)+abs(pixel.g)+abs(pixel.b) for pixel in grad],height,width)
end

function reduce_width(image::Array{ColorTypes.RGBA{FixedPointNumbers.Normed{UInt8,8}},2}, reduction)
	# image = load(image_path)

	for i in 1:reduction
		h, w = size(image)

		#=
		Det finnes mange måter å gi hver piksel en vekt.
		Her brukes summen av absoluttverdiene avgradientene,
		men det kan endres om man vil.
		=#
		weights = absolute_gradiant(image)
		cumulative_weights = path_weights(weights)

		#=
		Her bruker vi stien vi finner med back_track til å lage en boolsk
		matrise der cellene vi ønsker å fjerne fra bildet har verdien false
		=#
		crop_matrix = ones(Bool,h,w)
		for (i,j) in back_track(cumulative_weights)
			crop_matrix[i,j] = false
		end

		#=
		Et boolsk oppslag i matrisen vår fjerner alle celler der den boolske
		matrisen er false. Fordi resultatet av et slikt oppslag er en 1-dimensjonell
		matrise så bruker vi reshape for å få originalformen tilbake, minus den ene
		pikselen vi har fjernet i bredden.
		=#
		image = reshape(image'[crop_matrix'],w-1,h)'
	end

	image
end


@testset "SkewT" begin
  # Test it compiles.
  st = CDE.Util.SkewT(randn()*3, rand(), rand(LogNormal(3.5, .5)), rand()*3)

  skew = -3
  scale = 2

  altskew = CDE.Util.toaltskew(scale, skew)
  altscale = CDE.Util.toaltscale(scale, skew)

  @test isapprox(CDE.Util.scalefromaltskewt(altscale, altskew), scale)
  @test isapprox(CDE.Util.skewfromaltskewt(altscale, altskew), skew)
end

#= Test via plots
using StatsPlots

st = CDE.Util.SkewT(randn(), rand(), rand(LogNormal(2.5, .5)), randn()*3)
@time x = rand(st, Int(1e6));
xx = collect(range(-6, 6, length=10000));
histogram(x, color=:grey, normalize=true, label=nothing, linecolor=:grey)
plot!(xx, pdf.(st, xx), add=true, lw=2, color=:blue, label=nothing)
density!(x, label="kde", ls=:dot, color=:red, lw=3)
xlims!(minimum(x), maximum(x))

st1 = CDE.Util.SkewT(randn(), rand(), rand(LogNormal(2.5, .5)), randn()*3)
st2 = CDE.Util.SkewT(randn(), rand(), rand(LogNormal(2.5, .5)), randn()*3)
mm = MixtureModel([st1, st2], [.3, .7])
@time y = rand(mm, Int(1e6));
yy = collect(range(-6, 6, length=10000));
histogram(y, color=:grey, normalize=true, label=nothing, linecolor=:grey)
plot!(yy, pdf.(mm, yy), add=true, lw=2, color=:blue, label=nothing)
density!(y, label="kde", ls=:dot, color=:red, lw=3)
xlims!(minimum(y), maximum(y))
=#


doc"""
    BetaBinomial(n,α,β)

A *Beta-binomial distribution* is the compound distribution of the [`Binomial`](:func:`Binomial`) distribution where the probability of success `p` is distributed according to the [`Beta`](:func:`Beta`). It has three parameters: `n`, the number of trials and two shape parameters `α`, `β`

$P(X = k) = {n \choose k} B(k + \alpha, n - k + \beta) / B(\alpha, \beta),  \quad \text{ for } k = 0,1,2, \ldots, n.$

```julia
BetaBinomial(n, a, b)      # BetaBinomial distribution with n trials and shape parameters a, b

params(d)       # Get the parameters, i.e. (n, a, b)
ntrials(d)      # Get the number of trials, i.e. n
```

External links:

* [Beta-binomial distribution on Wikipedia](https://en.wikipedia.org/wiki/Beta-binomial_distribution)
"""

immutable BetaBinomial{T<:Real} <: DiscreteUnivariateDistribution
    n::Int
    α::T
    β::T

    function (::Type{BetaBinomial{T}}){T}(n::Int, α::T, β::T)
        @check_args(BetaBinomial, n >= zero(n) && α >= zero(α) && β >= zero(β))
        new{T}(n, α, β)
    end
end

BetaBinomial{T<:Real}(n::Integer, α::T, β::T) = BetaBinomial{T}(n, α, β)
BetaBinomial(n::Integer, α::Real, β::Real) = BetaBinomial(n, promote(α, β)...)
BetaBinomial(n::Integer, α::Integer, β::Integer) = BetaBinomial(n, Float64(α), Float64(β))

@distr_support BetaBinomial 0 d.n
insupport(d::BetaBinomial, x::Real) = 0 <= x <= d.n

#### Conversions
function convert{T <: Real, S <: Real}(::Type{BetaBinomial{T}}, n::Int, α::S, β::S)
    BetaBinomial(n, T(α), T(β))
end
function convert{T <: Real, S <: Real}(::Type{BetaBinomial{T}}, d::BetaBinomial{S})
    BetaBinomial(d.n, T(d.α), T(d.β))
end

#### Parameters

ntrials(d::BetaBinomial) = d.n

params(d::BetaBinomial) = (d.n, d.α, d.β)
@inline partype{T<:Real}(d::BetaBinomial{T}) = T

#### Properties

mean(d::BetaBinomial) = (d.n * d.α) / (d.α + d.β)
function var(d::BetaBinomial)
    n, α, β = d.n, d.α, d.β
    numerator = n * α * β * (α + β + n)
    denominator = (α + β)^2 * (α + β + 1)
    return numerator / denominator
end

function skewness(d::BetaBinomial)
    n, α, β = d.n, d.α, d.β
    t1 = (α + β + 2n) * (β - α) / (α + β + 2)
    t2 = sqrt((1 + α +β) / (n * α * β * (n + α + β)))
    return t1 * t2
end

function kurtosis(d::BetaBinomial)
    n, α, β = d.n, d.α, d.β
    alpha_beta_sum = α + β
    alpha_beta_product = α * β
    numerator = ((alpha_beta_sum)^2) * (1 + alpha_beta_sum)
    denominator = (n * alpha_beta_product) * (alpha_beta_sum + 2) * (alpha_beta_sum + 3) * (alpha_beta_sum + n)
    left = numerator / denominator
    right = (alpha_beta_sum) * (alpha_beta_sum - 1 + 6n) + 3*alpha_beta_product * (n - 2) + 6n^2
    right -= (3*alpha_beta_product * n * (6 - n)) / alpha_beta_sum
    right -= (18*alpha_beta_product * n^2) / (alpha_beta_sum)^2
    return (left * right) - 3
end

function pdf(d::BetaBinomial, k::Int)
    n, α, β = d.n, d.α, d.β
    chooseinv = (n + 1) * beta(k + 1, n - k + 1)
    numerator = beta(k + α, n - k + β)
    denominator = beta(α, β)
    return numerator / (denominator * chooseinv)
end

function pdf(d::BetaBinomial)
    n, α, β = d.n, d.α, d.β
    denominator = beta(α, β)
    values = Array{eltype(denominator)}(n+1)
    for (i,k) in enumerate(0:n)
        chooseinv = (n + 1) * beta(k + 1, n - k + 1)
        numerator = beta(k + α, n - k + β)
        @inbounds values[i] = numerator / (denominator * chooseinv)
    end
    return values
end

entropy(d::BetaBinomial) = entropy(Categorical(pdf(d)))
median(d::BetaBinomial) = median(Categorical(pdf(d))) - 1
mode{T<:Real}(d::BetaBinomial{T}) = indmax(pdf(d)) - one(T)
modes(d::BetaBinomial) = [x - 1 for x in modes(Categorical(pdf(d)))]

quantile(d::BetaBinomial, p::Float64) = quantile(Categorical(pdf(d)), p) - 1


"""
    eigROT_Distance(P, Q; edge_length = 1, α = 1.0)

computes the ROT distance matrix of P's column vectors on a graph.

# Input Argument
- `P::Matrix{Float64}`: a matrix whose columns are vector measures with the same total mass.
- `Q::SparseMatrixCSC{Int64,Int64}`: the unweighted incidence matrix of the graph.
- `edge_length::Any`: the length vector (default: 1 represents unweighted graphs)
- `α::Float64`: default is 1.0. ROT parameter.

# Output Argument
- `dis::Matrix{Float64}`: distance matrix, dis[i,j] = d_{ROT}(pᵢ, pⱼ; α).

"""
function eigROT_Distance(P::Matrix{Float64}, Q::SparseMatrixCSC{Int64,Int64};
                            edge_length::Any = 1, α::Float64 = 1.0)
    n = size(P, 2)
    total_mass = sum(P, dims = 1)[:]
    if norm(total_mass - total_mass[1] * ones(n), Inf) > 10^4 * eps()
        @error("P's column measures do not share the same total mass.")
        return
    end
    dis = zeros(n,n)
    le2 = [edge_length; edge_length]
    Q2 = [Q -Q]
    m2 = size(Q2, 2)
    for i = 1:(n - 1), j = (i + 1):n
        f = P[:, i] - P[:, j]
        md = Model(optimizer_with_attributes(Clp.Optimizer, "LogLevel" => 0))
        @variable(md, w[1:m2] >= 0.0)
        edge_length == 1 ? @objective(md, Min, sum(w)) : @objective(md, Min, sum(w .* le2))
        @constraint(md, Q2 * w .== f)
        JuMP.optimize!(md)
        wt = abs.(JuMP.value.(w))
        dis[i,j] = edge_length == 1 ? norm(wt.^α, 1) : norm((wt.^α) .* le2, 1)
    end
    return dis + dis'
end

"""
    ROT_Distance(A, B, Q; edge_length = 1, α = 1.0)

computes the ROT distance matrix from A's column vectors to B's column vectors.
If A, B are vector inputs, then it also returns the cost value and the optimal
transport plan.

# Input Argument
- `A::Any`: a vector or matrix whose columns are initial probability measures.
- `B::Any`: a vector or matrix whose columns are terminal probability measures.
- `Q::SparseMatrixCSC{Int64,Int64}`: the unweighted incidence matrix of the graph.
- `edge_length::Any`: the length vector (default: `1` represents unweighted graphs)
- `α::Float64`: default is `1.0`. ROT parameter.
- `retPlan::Bool`: an indicator if return the optimal plan (default: `false`)

# Output Argument
- `dis::Matrix{Float64}`: distance matrix, dis[i,j] = d_{ROT}(aᵢ, bⱼ; α).

"""
function ROT_Distance(A::Any, B::Any, Q::SparseMatrixCSC{Int64,Int64};
                        edge_length::Any = 1, α::Float64 = 1.0, retPlan::Bool = false)
    m = ndims(A) > 1 ? size(A, 2) : 1
    n = ndims(B) > 1 ? size(B, 2) : 1
    dis = zeros(m, n)
    le2 = [edge_length; edge_length]
    Q2 = [Q -Q]
    m2 = size(Q2, 2)
    for i = 1:m, j = 1:n
        f = (ndims(A) > 1 && ndims(B) > 1) ? B[:,j] - A[:,i] : B - A
        md = Model(optimizer_with_attributes(Clp.Optimizer, "LogLevel" => 0))
        @variable(md, w[1:m2] >= 0.0)
        edge_length == 1 ? @objective(md, Min, sum(w)) : @objective(md, Min, sum(w .* le2))
        @constraint(md, Q2 * w .== f)
        JuMP.optimize!(md)
        wt = abs.(JuMP.value.(w))
        dis[i, j] = edge_length == 1 ? norm(wt.^α, 1) : norm((wt.^α) .* le2, 1)
        if ndims(A) == 1 && ndims(B) == 1
            if retPlan
                return dis[1, 1], wt
            else
                return dis[1, 1]
            end
        end
    end
    return dis
end

using OptimalTransport
"""
    eigEMD_Distance(P, C)

EIGEMD\\_DISTANCE computes the EMD distance matrix of P's column vectors on a graph.

# Input Argument
- `P::Matrix{Float64}`: a matrix whose columns are probability measures.
- `C::Matrix{Float64}`: the cost matrix, which is the ground metric between pairwise nodes.

# Output Argument
- `dis::Matrix{Float64}`: distance matrix, dis[i,j] = d\\_EMD(pᵢ, pⱼ).

"""
function eigEMD_Distance(P, C)
    n = size(P,2)
    dis = zeros(n,n)
    for i = 1:n-1, j = i+1:n
        dis[i,j] = emd2(P[:,i], P[:,j], C)
    end
    return dis + dis'
end


# SPDX-License-Identifier: X11
# 2020-10-18 (Timed)
# Tag (200pt)

A, V = parse.(Int, split(readline()))
B, W = parse.(Int, split(readline()))
T = parse(Int, readline())

S = V - W
D = abs(B - A)

if S <= 0
  println("NO")
elseif T * S < D
  println("NO")
else
  println("YES")
end


# All functionality corresponding to the correlation coefficient of the
# convergent cross mapping (CCM) as Loss function Γ.

## Constructors:

"""
    CCM_ρ <: AbstractLoss

    Constructor for the CCM_ρ loss function (correlation coefficient of the
    convergent cross mapping) based on Sugihara et al.[^Sugihara2012], see also
    [`ccm`](@ref). In this case MCDTS tries to maximize the correlation coefficient
    of the convergent cross mapping from the input `data` and `Y_CCM`, the time
    series CCM should cross map to from `data` (see [`mcdts_embedding`](@ref)).

    ## Fieldnames
    * `timeseries`: The time series CCM should cross map to.
    * `threshold:: = 1: A threshold for the sufficient correlation of the
      cross-mapped values and the true values from `Y_CMM` for the current embedding.
      When the correlation coefficient exeeds this threshold in an embedding cycle
      the embedding stops.
    * `samplesize::Real = 1.`: the fraction of all phase space points
     to be considered in the computation of CCM_ρ.

    ## Defaults
    * When calling `CCM_ρ(timeseries)`, a CCM_ρ-object is created, storing
      `timeseries`, which is considered to be causally depended and the
      `threshold=1` is used, i.e. no threshold, since the correlation coefficient
      can not exceed 1.

    [^Sugihara2012]: Sugihara et al., [Detecting Causality in Complex Ecosystems. Science 338, 6106, 496-500](https://doi.org/10.1126/science.1227079)
"""
struct CCM_ρ <: AbstractLoss
    timeseries::Vector
    threshold::AbstractFloat
    samplesize::Real

    # Constraints and Defaults
    CCM_ρ(x,y=1.0,z=1) = begin
        @assert 0 < y ≤ 1 "Threshold must be a ∈ (0 1]"
        @assert 0 < z ≤ 1. "The samplesize must be in the interval (0 1]."
        typeof(y) <: Int ? new(x,convert(AbstractFloat, -y),z) : new(x,-y,z)
    end
end

## Functions:

"""
    Return the loss based on the negative correlation coefficient for CCM.
"""
function compute_loss(Γ::CCM_ρ, Λ::AbstractDelayPreselection, dps::Vector{P}, Y_act::Dataset{D, T}, Ys, τs, w::Int, ts::Int, τ_vals, ts_vals; metric=Euclidean(), kwargs...) where {P, D, T}

    Y_other = DelayEmbeddings.standardize(Γ.timeseries)
    samplesize = Γ.samplesize

    max_idx = get_max_idx(Λ, dps, τ_vals, ts_vals, ts) # get the candidate delays
    isempty(max_idx) && return Float64[], Int64[], []

    ρ_CCM = zeros(Float64, length(max_idx))

    for (i,τ_idx) in enumerate(max_idx)
        # create candidate phase space vector for this peak/τ-value
        tau_trials = (τ_vals...,τs[τ_idx-1],)
        ts_trials = (ts_vals...,ts,)
        Y_trial = genembed(Ys, tau_trials.*(-1), ts_trials)
        # account for value-shift due to negative lags
        Ys_other = Y_other[1+maximum(tau_trials.*(-1)):length(Y_trial)+maximum(tau_trials.*(-1))]
        # compute ρ_CCM for Y_trial and Y_other
        ρ_CCM[i], _, _ = MCDTS.ccm(Y_trial, Ys_other; metric = metric, w = w, samplesize = samplesize)
    end
    return -ρ_CCM, max_idx, [nothing for i in max_idx]
end

"""
    ccm(X, y; kwargs...) → ρ, y_hat, y_idx

    Compute the convergent crossmapping (CCM) [^Sugihara2012](Sugihara et al. 2012) of a
    vector time series `X` (an embedded time series `x`) on the time series `y`
    NOTE: 'X' and 'y' must have the same length and you have to make sure that
    'y' starts at the same time index as 'X' does. - When using [`genembed`](@ref)
    with negative delays to construct `X` from `x`, which is mandatory here, then
    'y' needs to be shifted by the largest negative delay value, which has been
    used to construct `X`.

    Returns the correlation coefficient of `y` and its predicted values for `y_hat`,
    based on the nearest neighbour structure of `X`. `y_idx` are the corresponding
    indices, which have been used for computing `y_hat`.
    It is said that 'y' causes 'x', if ρ increases with increasing time series
    length AND ρ is "quite high".

    Keyword arguments:
    *`metric = Euclidean()`: The metric for vector distance computation.
    *`w::Int = 1`: The Theiler window in sampling units.
    *`lags::Array = [0]`: The lag for the cross mapping, in order to detect time lagged
                          causal relationships. The output ρ is an array of size
                          `length(lags)`, the output Y_hat is the one corresponding
                          to a lag of zero.
    * `samplesize::Real = 0.1`: fraction of all phase space points (=`length(X)`)
                                to be considered (fiducial points v)

"""
function ccm(X::Dataset{D,T},Y::Vector{T}; metric = Euclidean(), w::Int = 1,
    lags::AbstractArray = [0], samplesize::Real = 1.) where {D,T<:Real}

    K = D+1
    @assert length(X)==length(Y)
    @assert 0 < samplesize ≤ 1. "`samplesize` for computing ρ-CCM must be ∈ (0,1]"
    XX = Matrix(X)
    N = length(X)

    # consider subset of state space points
    NN = floor(Int, samplesize*N)
    if samplesize == 1
        ns = 1:N # the fiducial point indices
    else
        ns = sample(1:N, NN; replace=false) # the fiducial point indices
    end

    vxs = X[ns] # the fiducial points in the data set
    vtree = KDTree(X, metric)
    allNNidxs, allNNdist = DelayEmbeddings.all_neighbors(vtree, vxs, ns, K, w)
    Y_hat = zeros(T, NN) # preallocation
    # loop over each fiducial point
    for (i,v) in enumerate(vxs)
        NNidxs = allNNidxs[i] # indices of k nearest neighbors to v
        NNdist = allNNdist[i] # indices of k nearest neighbors to v
        # determine weights
        u = zeros(K)
        ws = zeros(K)
        @inbounds for (k, j) in enumerate(NNdist)
            u[k] = exp(-(j/NNdist[1]))
        end
        ws = u ./ sum(u)
        # compute Y_hat as a wheighted mean
        Y_hat[i] = sum(ws .* Y[NNidxs])
    end
    ρ = Statistics.cor(Y_hat, Y[ns])
    return ρ, Dataset(Y_hat), ns
end


module ShercliffTest

using Gridap
using Test

using GridapMHD: shercliff
using GridapMHD: analytical_shercliff_u
using GridapMHD: analytical_shercliff_j
using GridapMHD: compute_u_j_errors

# Problem setting
ρ = 1.0
ν = 1.0
σ = 1.0

U0 = 10.0
B0 = 10.0
L = 1.0

Re = U0 * L / ν
Ha = B0 * L * sqrt(σ/(ρ*ν))
K = Ha / (1-0.825*Ha^(-1/2)-Ha^(-1))
∂p∂z = -Re * K / L^3

f_u = VectorValue(0.0,0.0, -∂p∂z) * L/U0^2

xh, trian, dΩ = shercliff(nx=5,ny=5,Re=Re,Ha=Ha,f_u=f_u)

uh, ph, jh, φh = xh
divj = (∇⋅jh)

# Scale unknowns
uh = uh * U0
jh = jh * σ * B0 * U0

# Analyical solutions
side_wall_semilength = 1.0
hartmann_wall_semilength = 1.0
number_fourier_sumands = 10
wall_thickness = 1.0
wall_conductivity = 0.0
K = Ha / (1-0.825*Ha^(-1/2)-Ha^(-1))
∂p∂z = -Re * K / L^3
u0(x) = analytical_shercliff_u(side_wall_semilength, hartmann_wall_semilength,
  wall_thickness, wall_conductivity, σ, ν*ρ, ∂p∂z, Ha, number_fourier_sumands, x)

j0(x) = analytical_shercliff_j(side_wall_semilength, hartmann_wall_semilength,
  wall_thickness, wall_conductivity, σ, ν*ρ, ∂p∂z, Ha, number_fourier_sumands, x)

eu_l2, ej_l2 = compute_u_j_errors(uh, jh, u0, j0, dΩ)
e_divj = sqrt(sum(∫(divj*divj)* dΩ))

@test eu_l2 < 0.3
@test ej_l2 < 5
@test e_divj < 1e-12

end #module


abstract type RegressionModels end
#If only we could add methods to abstract types...
# (M::RegressionModel)(X) = RegressionOut(X, M)
# maybe in Julia 2.0? - Update: Julia 1.2+ allows this!!!

struct ClassicLeastSquares <: RegressionModels
    Coefficients::Array
    Bias::Bool
end

struct RidgeRegression <: RegressionModels
    CLS::ClassicLeastSquares
end

"""
    ClassicLeastSquares( X, Y; Bias = false )

Constructs a ClassicLeastSquares regression model of the form `Y` = A`X` with or without a `Bias` term. Returns a CLS object.
"""
function ClassicLeastSquares( X::Array, Y::Array; Bias = false )
    Z = (Bias) ? hcat( ones( size( X )[ 1 ] ), X ) : X
    return ClassicLeastSquares(Base.inv(Z' * Z) * Z' * Y, Bias)
end

"""
    OrdinaryLeastSquares( X, Y; Bias = false )

Makes a ClassicLeastSquares regression model of the form `Y` = A`X` with or without a `Bias` term. Returns a CLS object.
This is a wrapper function for CLS, because most other fields refer to this as OLS.
"""
function OrdinaryLeastSquares( X, Y; Bias = false )
    Z = (Bias) ? hcat( ones( size( X )[ 1 ] ), X ) : X
    return ClassicLeastSquares(Base.inv(Z' * Z) * Z' * Y, Bias)
end

function PredictFn(X, M::Union{ClassicLeastSquares, RidgeRegression})
    Z = ( M.Bias ) ? hcat( ones(size( X )[ 1 ] ), X ) : X
    return Z * M.Coefficients
end

"""
    (M::ClassicLeastSquares)(X)

Makes an inference from `X` using a ClassicLeastSquares object.
"""
(M::ClassicLeastSquares)(X) = PredictFn(X, M)

"""
    RidgeRegression( X, Y, Penalty; Bias = false )

Makes a RidgeRegression model of the form `Y` = A`X` with or without a `Bias` term and has an L2 `Penalty`. Returns a CLS object.
"""
function RidgeRegression( X, Y, Penalty; Bias = false )
    Y = forceMatrix(Y)
    Z = (Bias) ? hcat( ones( size( X )[ 1 ] ), X ) : X
    return RidgeRegression( ClassicLeastSquares( Base.inv( (Z' * Z) .+ (Penalty .* Diagonal( ones( size(Z)[2] ) ) ) ) * Z' * Y, Bias) )
end

"""
    (M::RidgeRegression)(X)

Makes an inference from `X` using a RidgeRegression object which wraps a ClassicLeastSquares object.
"""
(M::RidgeRegression)(X) = PredictFn(X, M.CLS)

struct KRR
    kernel::Kernel
    RR::RidgeRegression
end

"""
    KernelRidgeRegression( X, Y, Penalty; KernelParameter = 0.0, KernelType = "linear" )

Makes a KernelRidgeRegression model of the form `Y` = A`K` using a user specified Kernel("Linear", or "Guassian") and has an L2 `Penalty`.
Returns a KRR Wrapper for a CLS object.
"""
function KernelRidgeRegression( X, Y, Penalty; KernelParameter = 0.0, KernelType = "linear" )
    Kern = Kernel( KernelParameter, KernelType, X )
    return KRR(Kern, RidgeRegression( Kern(X), Y, Penalty ) )
end

"""
    (M::KRR)(X)

Makes an inference from `X` using a KRR object which wraps a ClassicLeastSquares object.
"""
(M::KRR)(X) = PredictFn(M.kernel(X), M.RR.CLS)

struct LSSVM
    kernel::Kernel
    RR::RidgeRegression
end

function formatlssvminput(X)
    Y = zeros(size(X) .+ 1)
    Y[ 1, 1 ] = 1.0
    Y[ 2:end, 2:end ] .= X
    return Y
end

"""
    LSSVM( X, Y, Penalty; KernelParameter = 0.0, KernelType = "linear" )

Makes a LSSVM model of the form `Y` = A`K` with a bias term using a user specified Kernel("linear", or "gaussian") and has an L2 `Penalty`.
Returns a LSSVM Wrapper for a CLS object.
"""
function LSSVM( X, Y, Penalty; KernelParameter = 0.0, KernelType = "linear" )
    Kern = Kernel( KernelParameter, KernelType, X )
    return LSSVM(   Kern,
                    RidgeRegression( formatlssvminput( Kern( X ) ),
                    vcat( 0.0, Y ),
                    Penalty ) )
end

"""
    (M::LSSVM)(X)

Makes an inference from `X` using a LSSVM object.
"""
(M::LSSVM)(X) = PredictFn( formatlssvminput( M.kernel(X)), M.RR.CLS )[2:end,:]

struct PrincipalComponentRegression <: RegressionModels
    PCA::PCA
    CLS::ClassicLeastSquares
end

"""
    PrincipalComponentRegression(PCAObject, Y )

Makes a PrincipalComponentRegression model object from a PCA Object and property value `Y`.
"""
function PrincipalComponentRegression(PCAObject::PCA, Y )
    return PrincipalComponentRegression(PCAObject, ClassicLeastSquares( PCAObject.Scores, forceMatrix(Y) ) )
end

PredictFn(X, M::PrincipalComponentRegression) = M.CLS( M.PCA( X ) )

"""
    (M::PrincipalComponentRegression)( X )

Makes an inference from `X` using a PrincipalComponentRegression object.
"""
(M::PrincipalComponentRegression)( X ) = PredictFn( X, M )

struct PartialLeastSquares <: RegressionModels
    XLoadings::Array
    XScores::Array
    YLoadings::Array
    YScores::Array
    XWeights::Array
    RWeights::Array
    Coefficients::Array
    Factors::Int
    XVariance::Array
    YVariance::Array
end

"""
    PartialLeastSquares( X, Y; Factors = minimum(size(X)) - 2, tolerance = 1e-8, maxiters = 200 )

Returns a PartialLeastSquares regression model object from arrays `X` and `Y`.

1. PARTIAL LEAST-SQUARES REGRESSION: A TUTORIAL PAUL GELADI and BRUCE R.KOWALSKI. Analytica Chimica Acta, 186, (1986) PARTIAL LEAST-SQUARES REGRESSION:
2. Martens H., NÊs T. Multivariate Calibration. Wiley: New York, 1989.
3. Re-interpretation of NIPALS results solves PLSR inconsistency problem. Rolf Ergon. Published in Journal of Chemometrics 2009; Vol. 23/1: 72-75
"""
function PartialLeastSquares( X, Y; Factors = minimum(size(X)) - 2, tolerance = 1e-8, maxiters = 200 )
    (Xrows, Xcols) = size(X)
    Y = forceMatrix(Y)
    (Yrows, Ycols) = size(Y)
    @assert Factors < (reduce(min, (Xrows, Xcols) ) - 1)
    Xd = copy(X); Yd = copy(Y)
    Xvar = Statistics.var(X, dims = 1)
    Yvar = Statistics.var(Y, dims = 1)
    Coefficients = []
    T = zeros(Xrows, Factors); t = zeros(Xrows); tprime = zeros(Xrows);
    U = zeros(Yrows, Factors); u = zeros(Yrows)
    P = zeros(Xcols, Factors); p = zeros(Xcols);
    Q = zeros(Ycols, Factors); q = zeros(Ycols)
    W = zeros(Xcols, Factors); w = zeros(Xrows);
    for factor in 1:Factors
        u = (Ycols == 1) ? Yd[:,1] : Yd[ :, argmax( [ (Yd[:,col]' * Yd[:,col])[1] for col in 1:Ycols] ) ]
        for iter in 1:maxiters
            w = X' * u
            w = w ./ sqrt.(w' * w)
            t = Xd * w
            q = Y' * (t ./ (t' * t) )
            diff = t .- tprime
            if (Ycols == 1) || ( sqrt.( diff' * diff )[1] < tolerance )
                break
            end
            u = Y * (q ./ (q' * q))
            tprime = t;
        end#End for ALS iterations
        tnorm = t ./ ( t' * t )
        p = X' * tnorm
        Xd = Xd .- ( t * p' )
        Yd = Yd .- ( t * q' )
        #Update Model Variables
        Q[:,factor] = q; T[:,factor] = t
        U[:,factor] = u; P[:,factor] = p
        W[:, factor] = w
    end#end for factors
    R = (Factors == 1) ? W : W * Base.inv( P' * W )
    #Use a more modern way to solve for the regression coefficients (2)
    Coefficients = R * Q'
    #An Equivalent way to obtain the regression coefficients.
    #Coefficients = W*Base.inv(W'*X'*X*W)*W'*X'*Y*Q
    return PartialLeastSquares(P, T, Q, U, W, R, Coefficients, Factors, Xvar, Yvar)
end

function PredictFn(X, M::PartialLeastSquares; Factors)
    Coeffs = []
    if Factors < M.Factors
        R = (Factors == 1) ? M.XWeights[:, 1:Factors] : M.XWeights[:, 1:Factors] * Base.inv( M.XLoadings[:, 1:Factors]' * M.XWeights[:, 1:Factors]  )
        Coeffs = R * M.YLoadings[:, 1:Factors]'
    else
        Coeffs = M.Coefficients
    end
    return X * Coeffs
end

"""
    (M::PartialLeastSquares)

Makes an inference from `X` using a PartialLeastSquares object.
"""
(M::PartialLeastSquares)(X; Factors = M.Factors) = PredictFn(X, M; Factors = Factors)

struct ELM <: RegressionModels
    Reservoir::Array
    Coefficients::Array
    Fn::Function
end

"""
    sigmoid(x)

Applies the sigmoid function to a scalar value X. Returns a scalar. Can be broad-casted over an Array.
"""
sigmoid(x) = 1.0 / (1.0 + exp(-1.0 * x))

"""
    ExtremeLearningMachine(X, Y, ReservoirSize = 10; ActivationFn = sigmoid)

Returns a ELM regression model object from arrays `X` and `Y`, with a user specified `ReservoirSize` and `ActivationFn`.

Extreme learning machine: a new learning scheme of feedforward neural networks. Guang-Bin Huang ; Qin-Yu Zhu ; Chee-Kheong Siew. 	2004 IEEE International Joint...
"""
function ExtremeLearningMachine(X, Y, ReservoirSize = 10; ActivationFn = sigmoid)
    W = randn( size( X )[2], ReservoirSize )
    return ELM( W, LinearAlgebra.pinv( ActivationFn.(X * W) ) * Y,
                ActivationFn)
end

PredictFn(X, M::ELM) = M.Fn.(X * M.Reservoir) * M.Coefficients;

"""
    (M::ELM)(X)

Makes an inference from `X` using a ELM object.
"""
(M::ELM)(X) = PredictFn(X, M)

mutable struct block
    value::Float64 #Avg value
    weight::Float64
    size::Int32
    previous::Int32
    next::Int32
end

"""
    MonotoneRegression(x, w = nothing)

Performs a monotone/isotonic regression on a vector x. This can be weighted
with a vector w.

Code was translated directly from:
Exceedingly Simple Monotone Regression. Jan de Leeuw. Version 02, March 30, 2017
"""
function MonotoneRegression(x::Array{Float64,1}, w = nothing)
    bins = length(x)
    retx = ones(bins)
    retx[:] = x[:]
    if isa(w, Nothing)
        w = ones(bins)
    end
    bins = bins + 1
    blocks = [ block(x[i], w[i], 1, i-1, i+1) for i in 1:(bins-1)]
    #I have a one off error somewhere an easy solution is just to pad
    push!(blocks, block( 0.0, w[end], 1, bins-1, bins+1) )
    active = 1
    continue_reg = true
    while( continue_reg )
        upsatisfied = false
        next = blocks[active].next
        if next == bins
            upsatisfied = true
        elseif blocks[next].value > blocks[active].value
            upsatisfied = true
        end
        if !upsatisfied
            ww = blocks[active].weight + blocks[next].weight
            nextnext = blocks[next].next
            wxactive = blocks[active].weight * blocks[active].value
            wxnext = blocks[next].weight * blocks[next].value
            blocks[active].value = (wxactive + wxnext) / ww
            blocks[active].weight = ww
            blocks[active].size += blocks[next].size
            blocks[active].next = nextnext
            if nextnext < bins
                blocks[nextnext].previous = active
            end
            blocks[next].size = 0
        end
        downsatisfied = false
        previous = blocks[active].previous

        if previous == 0
            downsatisfied = true
        elseif blocks[previous].value < blocks[active].value
            downsatisfied = true
        end
        if !downsatisfied
            ww = blocks[active].weight + blocks[previous].weight
            previousprevious = blocks[previous].previous
            wxactive = blocks[active].weight * blocks[active].value
            wxprevious = blocks[previous].weight * blocks[previous].value
            blocks[active].value = (wxactive + wxprevious) / ww
            blocks[active].weight = ww
            blocks[active].size += blocks[previous].size
            blocks[active].previous = previousprevious
            if previousprevious > 0
                blocks[previousprevious].next = active
            end
            blocks[previous].size = 0
        end
        if (blocks[active].next == bins) && (downsatisfied)
            continue_reg = false
        end
        if (upsatisfied && downsatisfied)
            active = next
        end
    end
    k = 1
    for i in 1:bins
        blksize = blocks[i].size;
        if (blksize > 0) && (i < bins )
            retx[k:(k+(blksize-1))] .= blocks[i].value;
            k += blksize
        end
    end
    return retx
end


# Author: Lukasz Bratos

# import Pkg
# Pkg.add("Polynomials")
using Polynomials

# wspolczynniki wielomianu
p = [1, -210.0, 20615.0,-1256850.0, 53327946.0,-1672280820.0, 40171771630.0,
    -756111184500.0, 11310276995381.0, -135585182899530.0, 1307535010540395.0,
    -10142299865511450.0, 63030812099294896.0, -311333643161390640.0,
    1206647803780373360.0, -3599979517947607200.0, 8037811822645051776.0,
    -12870931245150988800.0, 13803759753640704000.0, -8752948036761600000.0,
    2432902008176640000.0]

# wspolczynniki postaci iloczynowej
p_roots = [x for x in 1.0:20.0]
println(p_roots)

# wspolczynniki zmodyfikowanego wielomianu
p_mod = [1, -210.0 - 2 ^ -23, 20615.0,-1256850.0, 53327946.0,-1672280820.0, 40171771630.0,
-756111184500.0, 11310276995381.0, -135585182899530.0, 1307535010540395.0,
-10142299865511450.0, 63030812099294896.0, -311333643161390640.0,
1206647803780373360.0, -3599979517947607200.0, 8037811822645051776.0,
-12870931245150988800.0, 13803759753640704000.0, -8752948036761600000.0,
2432902008176640000.0]

# wielomianu w postaci normalnej
A = Poly(reverse(p))
# wielomian w postaci iloczynowej
B = poly(p_roots)
# zmodyfikowany wielomian w postaci normalnej
C = Poly(reverse(p_mod))

# pierwiastki wielomianow
A_roots = reverse(roots(A))
C_roots = reverse(roots(C))

for i in 1:20
    println("$i & $(A_roots[i]) & $(abs(polyval(A, A_roots[i]))) & $(abs(polyval(B, A_roots[i]))) & ", abs(A_roots[i] - i), " \\\\ \\hline")
end

for i in 1:20
    println("$i & $(C_roots[i]) & $(abs(polyval(C, C_roots[i]))) & ", abs(C_roots[i] - i), " \\\\ \\hline")
end


function nutsepsilon(x::GeneralNode, logfgrad::Function, delta::Float64, target::Float64)
    n = size(x)[1] - 1
    log_target = log(target)
    # molifier is necessary!
    # blv = get_branchlength_vector(x)
    # set_branchlength_vector!(x, molifier.(blv, delta))

    logf0, gr = logfgrad(x)

    r0 = randn(n)
    x0 = Tree_HMC_State(deepcopy(x), r0, gr, logf0)
    x1 = transfer(x0)
    H0 = hamiltonian(x0)
    epsilon = 1.0
    _ = refraction!(x0, epsilon, logfgrad, delta)
    Hp = hamiltonian(x0)

    prob = Hp - H0
    direction = prob > log_target ? 1 : -1

    while direction == 1 ? prob > log_target : prob < log_target
        epsilon = direction == 1 ? 2 * epsilon : 0.5 * epsilon

        x2 = transfer(x1)
        _ = refraction!(x2, epsilon, logfgrad, delta)

        Hp = hamiltonian(x2)

        prob = Hp - H0
    end

    epsilon
end

function nutsepsilon(x::Vector{<:Real}, logfgrad::Function, target::Float64)

    logf0, gr = logfgrad(x)
    target = log(target)

    r0 = randn(size(x))
    x0 = Array_HMC_State(x, r0, gr, logf0)
    x1 = transfer(x0)
    H0 = hamiltonian(x0)
    epsilon = 1.0
    leapfrog!(x0, epsilon, logfgrad)
    Hp = hamiltonian(x0)

    prob = Hp - H0

    direction = prob > target ? 1 : -1
    restart = false
    while direction == 1 ? prob > target : prob < target
        epsilon = direction == 1 ? 2 * epsilon : 0.5 * epsilon
        if isapprox(epsilon, 0)
            throw("The estimated stepsize is too small. ϵ: $epsilon")
        elseif epsilon > 1e7
            throw("The estimated stepsize is too large. ϵ: $epsilon")
        end
        x2 = transfer(x1)
        leapfrog!(x2, epsilon, logfgrad)

        Hp = hamiltonian(x2)

        prob = Hp - H0

    end

    if restart
        @warn "restarting initialization of ϵ"
        epsilon = nutsepsilon(x, logfgrad, exp(target))
    end
    epsilon
end

@inline function scale_fac(x::T, delta::T)::T where {T<:Real}
    x < delta ? x/delta : 1.0
end

@inline function molifier(x::T, delta::Real)::T where {T<:Real}
    x >= delta ? x : (x^2 + delta^2) / (2.0 * delta)
end # function

@inline function abs_adapter(x::R)::Float64 where R <:Real
    x / (1+abs(x))
end


function [Grid] = HexagonGridNeu(nx,ny,r,OrientFace)
dphi=2*pi/6;
dx=sqrt(3)*r;
dy=2*r;
x0=sqrt(3)/2*r;
y0=r;

NumNodes=2*(2*nx+1);
Nodes(1:NumNodes)=Node([0 0 0],0);
NodeNumber=1;
yShift=y0;
for j=1:ny
  phi=0;
  xShift=x0;
  for i=1:nx
    phi=phi-dphi;
    x=sin(phi)+xShift;
    y=-cos(phi)+yShift;
    z=0;
    Nodes(NodeNumber)=Node([x;y;z],NodeNumber);
    NodeNumber=NodeNumber+1;
    phi=phi+dphi;
    x=sin(phi)+xShift;
    y=-cos(phi)+yShift;
    z=0;
    Nodes(NodeNumber)=Node([x;y;z],NodeNumber);
    NodeNumber=NodeNumber+1;
    xShift=xShift+dx;
  end
  phi=phi-dphi;
  x=sin(phi)+xShift;
  y=-cos(phi)+yShift;
  z=0;
  Nodes(NodeNumber)=Node([x;y;z],NodeNumber);
  NodeNumber=NodeNumber+1;

  xShift=x0;
  phi=pi;
  yShift=yShift;
  for i=1:nx
    phi=phi+dphi;
    x=sin(phi)+xShift;
    y=-cos(phi)+yShift;
    z=0;
    Nodes(NodeNumber)=Node([x;y;z],NodeNumber);
    NodeNumber=NodeNumber+1;
    phi=phi-dphi;
    x=sin(phi)+xShift;
    y=-cos(phi)+yShift;
    z=0;
    Nodes(NodeNumber)=Node([x;y;z],NodeNumber);
    NodeNumber=NodeNumber+1;
    xShift=xShift+dx;
  end
  phi=phi+dphi;
    x=sin(phi)+xShift;
    y=-cos(phi)+yShift;
    z=0;
    Nodes(NodeNumber)=Node([x;y;z],NodeNumber);
    NodeNumber=NodeNumber+1;
end

Grid.Nodes=Nodes;
Grid.NumNodes=NumNodes;

NumEdges=2*2*nx+nx+1;
Edges(1:NumEdges)=Edge([1 2],Grid,0,0);
EdgeNumber=1;

N1=1;
N2=N1+1;
for i=1:2*nx
  Edges(EdgeNumber)=Edge([N1 N2],Grid,EdgeNumber,EdgeNumber);
  EdgeNumber=EdgeNumber+1;
  N1=N2;
  N2=N2+1;
end
N1=2*nx+2;
N2=N1+1;
for j=1:ny
  for i=1:2*nx
    Edges(EdgeNumber)=Edge([N1 N2],Grid,EdgeNumber,EdgeNumber);
    EdgeNumber=EdgeNumber+1;
    N1=N2;
    N2=N2+1;
  end
end
for j=1:ny
  N1=1;
  N2=N1+2*nx+1;
  for i=1:nx+1
    Edges(EdgeNumber)=Edge([N1 N2],Grid,EdgeNumber,EdgeNumber);
    EdgeNumber=EdgeNumber+1;
    N1=N1+2;
    N2=N2+2;
  end
end
Grid.Edges=Edges;
Grid.NumEdges=NumEdges;
Grid.NumEdgesI=NumEdges;
Grid.NumEdgesB=0;

NumFaces=nx;
Faces(1:NumFaces)=Face([0 0 0 0],Grid,0,'x');
FaceNumber=1;
Type='o';
E1=1;
E2=E1+1;
E3=2*2*nx+2;
E4=E2+2*nx;
E5=E4-1;
E6=E3-1;
for i=1:nx
  Faces(FaceNumber)=Face([E1 E2 E3 E4 E5 E6],Grid,FaceNumber,Type,OrientFace);
  E1=E1+2;
  E2=E2+2;
  E3=E3+1;
  E4=E4+2;
  E5=E5+2;
  E6=E6+1;
  FaceNumber=FaceNumber+1;
end
Grid.Faces=Faces;
Grid.NumFaces=NumFaces;

Grid=FacesInEdges(Grid);
Grid=FacesInNodes(Grid);

end


# SmoothGrad

"""
    smooth_grad(img, model; method=viz_backprop,top_k=1, mean=0, var=1, n_imgs=50, normalized_n_scaled=true)

Method to visually sharpen the gradient based saliency maps. The key idea is to evaluate the average of gradients of images obtained after adding gaussian noise to the original image(`img`). The results suggest the estimated smoothed gradient, leads to visually more coherent sensitivity maps than the unsmoothed gradient, with the resulting visualization aligning better to the human eye with meaningful features.

# Arguments
 - `img`: Original image of interest for visualization
 - `model`: Model to be used for visualization
 - `method=viz_backprop`: Method to be used while evaluating gradients. Can take methods like `viz_guidedbackprop`, `viz_backprop`, `viz_deconvolution` or any other custom function.
 - `top_k=1`: function evaluates the smoothed gradient for `top_k` predictions just as in case of normal `viz_backprop` function.
 - `mean=0`: Mean of the gaussian noise to be added to the image to generate sample space
 - `var=1`: Variance of the gaussian noise to be added to image to generate sample space
 - `n_imgs=50`: Number of images to be generated and used in sample space while evaluation of `smooth_grad`.
 - `normalized_n_scaled=true`: if true, then the noise is first scaled to 0-255 before adding to the image.

Ref: (https://arxiv.org/pdf/1706.03825.pdf)
"""
function smooth_grad(img, model; method=viz_backprop, top_k=1, mean=0, var=1, n_imgs=50, normalized_n_scaled=true)
    smooth_grad = zeros(size(img)...)
    for i in n_imgs
        noise = (var^0.5).*randn(size(img)...) .+ mean
        if (normalized_n_scaled==false)
            noise *= 255.0
        end
        noise = Float32.(noise)
        noised_img = img + noise
        smooth_grad += method(noised_img, model, top_k = top_k)
    end
    smooth_grad/n_imgs
end

# Gradient X Image

"""
    grad_times_image(img, model; top_k=1)

An extension to the saliency approach. Returns an array of products of the `img` with the gradient evaluated with respect to the provided `img` for the `model`, for `top_k` prediction classes.
"""
function grad_times_image(img, model; top_k=1)
    grads = viz_backprop(img, model, top_k=top_k)
    grads_times_image = []
    for i in 1:length(grads)
        push!(grads_times_image, (grads[i][1].*img, grads[i][2],grads[i][3]))
    end
    grads_times_image
end


#== # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# Description
#
#   Functions to verify if the satellite is within the station visibility
#   circle.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # ==#

export satellite_check_station

"""
    function satellite_check_station(r_e::Vector, rs_e::Vector, minElev::Number)

Check if the satellite with position vector `r_e` (ECEF) is inside the
visibility circle of a ground station with position vector `rs_e` (ECEF) and a
minimum elevation angle of `minElev` [rad].

Notice that `r_e` and `rs_e` must be represented in the same ECEF frame, and
must have the same unit.

Returns `true` if the satellite is inside the visibility circle, or `false`
otherwise.

"""
function satellite_check_station(r_e::Vector, rs_e::Vector, minElev::Number)
    # Check if the satellite is within the visibility circle of the station.
    dr_e = r_e - rs_e
    cos_beta = ( ( dr_e/norm(dr_e) )'*( rs_e/norm(rs_e) ) )[1]

    if (cos_beta > cos(pi/2-minElev))
        return true
    else
        return false
    end
end

"""
    function satellite_check_station(r_e::Vector, lat_s::Number, lon_s::Number, h_s::Number, minElev::Number)

Check if the satellite with position vector `r_e` (ECEF) is inside the
visibility circle of a ground station with latitude `lat_s` [rad], longitude
`lon_s` [rad], altitude `h_s` (WGS-84), and a minimum elevation angle of
`minElev` [rad].

Notice that the units of `r_e` and `h_s` must be the same.

Returns `true` if the satellite is inside the visibility circle, or `false`
otherwise.

"""
function satellite_check_station(r_e::Vector,
                                 lat_s::Number,
                                 lon_s::Number,
                                 h_s::Number,
                                 minElev::Number)
    # Convert the ground station LLA to the ECEF frame.
    rs_e = GeodetictoECEF(lat_s, lon_s, h_s)

    satellite_check_station(r_e, rs_e, minElev)
end


"""
Find the convex hull of a set of points in 2D.

Usage:

```
using QuickHull, FixedSizeArrays
points = map(Vec, rand(10), rand(10))
hull = qhull(points)
```

or

```
using QuickHull
xs, ys = rand(10), rand(10)
hullx, hully = qhull(xs, ys)
```

Documentation is available at https://tlycken.github.io/QuickHull.jl/latest
"""
module QuickHull

using FixedSizeArrays

export qhull

"""
`hullx, hully = qhull(xs, ys)`
"""
function qhull(xs, ys)
    points = map(Vec, xs, ys)
    hull = qhull(points)
    x, y = collect(zip(map(p -> (p...), hull)...))
    collect(x), collect(y)
end

"""
`hull = qhull(points::Vector{Vec{2,T}})`
"""
function qhull{T<:Vec{2}}(points::Vector{T})
    hull = Vector{T}(0)

    A = points[findmin(map(p->p[1], points))[2]]
    B = points[findmax(map(p->p[1], points))[2]]

    push!(hull, A)

    s1, s2 = partition(points, A, B)

    qhull!(hull, A, B, s1)

    push!(hull, B)

    qhull!(hull, B, A, s2)

    hull
end

"""
Find the convex hull of a set of points in 2D.
"""
qhull

function qhull!(hull, P, Q, points)
    length(points) == 0 && return

    C = points[findmax(map(p -> distance(P,Q,p), points))[2]]

    s1, s2 = partition(points, P, Q, C)

    qhull!(hull, P, C, s1)
    push!(hull, C)
    qhull!(hull, C, Q, s2)
end

function partition{T}(points, A::T, B::T)
    s1, s2 = Vector{T}(0), Vector{T}(0)

    for p in points
        (p == A || p == B) && continue
        push!((right_of(A,B,p) ? s1 : s2), p)
    end

    s1, s2
end

function partition{T}(points, P::T, Q::T, C::T)
    s1, s2 = Vector{T}(0), Vector{T}(0)

    for p in points
        p == C && continue

        if right_of(P, C, p)
            push!(s1, p)
        elseif right_of(C, Q, p)
            push!(s2, p)
        end
    end

    s1, s2
end

@inline distance(A,B,C) = abs(cross(C-A, C-B))

@inline cross(A,B) = A[1]*B[2]-B[1]*A[2]

@inline right_of(A, B, C) = cross(B-A,C-A) < 0
end # module


# Jonas August 2021

#=

This problem was recently asked by Google.

Given a list of numbers and a number k, return whether any two numbers from the list add up to k.

For example, given [10, 15, 3, 7] and k of 17, return true since 10 + 7 is 17.

Bonus: Can you do this in one pass?

=#

# Two pass method
function haspair2(list, sum)
   for i in 1:length(list)
      for j in 1:length(list)
        if j == 1
	    continue
	end
	if list[i] + list[j] == sum
	    return true
	end
      end
   end
   false
end

#=
TODO: single-pass
I can see quadratic-time method with only a single pass of list, by introducing a pass on a different array, so not really an improvement IMO.
=#

using Test

@test haspair2([10, 15, 3, 7], 17)
@test !haspair2([10, 15, 3, 7], 26)


# Find a Profile-most Probable k-mer in a String

struct ProfileColumn
    A::Float64
    C::Float64
    G::Float64
    T::Float64
end

convert_to_floatlist(line) = map(x -> parse(Float64, x), split(line))

function read_profile_matrix(lines, k)
    matrix = Array{Float64, 2}(undef, 4, k)
    for (ix, line) in enumerate(lines)
        matrix[ix, :] = convert_to_floatlist(line)
    end
    [ProfileColumn(column...) for column in eachcol(matrix)]
end

calc_kmer_probability(kmer, profile_matrix) =
    foldl((p, (nucleotide, column)) -> p * getfield(column, Symbol(nucleotide)), zip(kmer, profile_matrix); init=1.0)

function profile_most_probable_kmer(text, profile_matrix, k)
    max_probability = 0.0
    most_probable_kmer = text[1:k]
    for ix in 1:(length(text)-k+1)
        kmer = text[ix:ix+k-1]
        p = calc_kmer_probability(kmer, profile_matrix)
        if p > max_probability
            max_probability = p
            most_probable_kmer = kmer
        end
    end
    most_probable_kmer
end

function main()
    text = readline()
    k = parse(Int64, readline())
    lines = readlines()
    profile_matrix = read_profile_matrix(lines, k)
    result = profile_most_probable_kmer(text, profile_matrix, k)
    println(result)
end

main()


using BEAST
using CompScienceMeshes
using WiltonInts84
using Base.Test

p1 = point(0,0,0)
p2 = point(1,0,0)
p3 = point(0,1,0)

q1 = point(2,2,0)
q2 = point(2,1,0)
q3 = point(1,2,0)

τ = [p1,p2,p3]
σ = [q1,q2,q3]
m, M = BEAST.minmaxdist(τ, σ)

@show m, sqrt(2.0)
@show M, sqrt(8.0)

@test m ≈ sqrt(2.0)
@test M ≈ sqrt(8.0)

ΔR = sqrt(2)/2
r = BEAST.rings(τ,σ,ΔR)
@show r

cm = point(0.5,0.5,0.0)
cM = point(0.0,0.0,0.0)
S,V,G = WiltonInts84.wiltonints(q1,q2,q3,cm,(r[1]-2)*ΔR,(r[1]-1)*ΔR,Val{0})
@show S[3]

S,V,G = WiltonInts84.wiltonints(q1,q2,q3,cM,(r[end]-1)*ΔR,(r[end])*ΔR,Val{0})
@show S[3]

h = 0.05
Γ = meshrectangle(10.0, h, h, 3)
X = raviartthomas(Γ)

timestep = 1.0
numsteps = 10
S = timebasisc0d1(timestep, numsteps)
S = BEAST.timebasisspline2(timestep, numsteps)

c = 1.0
Ts = MWSingleLayerTDIO(c,1.0,0.0,0,0)
Th = MWSingleLayerTDIO(c,0.0,1.0,0,0)

Zs = BEAST.assemble(Ts,X,X,S)
Zh = BEAST.assemble(Th,X,X,S)

## Test the allcatestorage routine and in particular its type stability
h = 0.05
Γ = meshrectangle(10.0, h, h, 3)
X = raviartthomas(Γ)

timestep = 1.0
numsteps = 10
S = timebasisc0d1(timestep, numsteps)
S = BEAST.timebasisspline2(timestep, numsteps)

c = 1.0
Ts = MWSingleLayerTDIO(c,1.0,0.0,0,0)
Th = MWSingleLayerTDIO(c,0.0,1.0,0,0)

U = timebasiscxd0(Δt, Nt)
T = timebasisc0d1(Δt, Nt)

W = X ⊗ U
V = X ⊗ T
@code_warntype BEAST.allocatestorage(Ts,W,V)