Cleanups and fixes in CV.Filters

Cleanups and fixes in CV.Filters

Added documentation, cleaned up code
and fixed the bilateral filter.

{-#LANGUAGE ForeignFunctionInterface, TypeFamilies, FlexibleInstances#-}
#include "cvWrapLEO.h"
{-#OPTIONS_GHC -fwarn-unused-imports#-}

-- | This module is a collection of various image filters
module CV.Filters(
               gaussian,gaussianOp
              ,blurOp,blur,blurNS
              ,bilateral
              ,HasMedianFiltering,median
              ,susan,getCentralMoment,getAbsCentralMoment
              ,getMoment,secondMomentBinarize,secondMomentBinarizeOp
              ,secondMomentAdaptiveBinarize,secondMomentAdaptiveBinarizeOp
              ,selectiveAvg,convolve2D,convolve2DI,haar,haarAt
              ,IntegralImage(),integralImage,verticalAverage) where

import Foreign.C.Types
import Foreign.Ptr
import Foreign.Marshal.Utils
import CV.Bindings.ImgProc

import Utils.GeometryClass
import CV.Matrix (Matrix,withMatPtr)
import CV.ImageOp

import System.IO.Unsafe

--import C2HSTools
{#import CV.Image#}

-- Low level wrapper for Susan filtering:
--  IplImage* susanSmooth(IplImage *src, int w, int h
--                     ,double t, double sigma);

-- | SUSAN adaptive smoothing filter, see <http://users.fmrib.ox.ac.uk/~steve/susan/susan/susan.html>
susan :: (Int, Int) -> Double -> Double
     -> Image GrayScale D32 -> Image GrayScale D32
susan (w,h) t sigma image = unsafePerformIO $ do
                            withGenImage image $ \img ->
                             creatingImage
                                ({#call susanSmooth#} img (fromIntegral w) (fromIntegral h)
                                                         (realToFrac t) (realToFrac sigma))
-- TODO: ADD checks above!

-- | A selective average filter is an edge preserving noise reduction filter.
--   It is a standard gaussian filter which ignores pixel values
--   that are more than a given threshold away from the filtered pixel value.
selectiveAvg :: (Int, Int) -> Double
     -> Image GrayScale D32 -> Image GrayScale D32
selectiveAvg (w,h) t image = unsafePerformIO $ do
                              withGenImage image $ \img ->
                               creatingImage
                                ({#call selectiveAvgFilter#}
                                    img (realToFrac t) (fromIntegral w) (fromIntegral h))
-- TODO: ADD checks above!

getCentralMoment n (w,h) image = unsafePerformIO $ do
                            withGenImage image $ \img ->
                             creatingImage
                                ({#call getNthCentralMoment#} img n w h)

getAbsCentralMoment n (w,h) image = unsafePerformIO $ do
                            withGenImage image $ \img ->
                             creatingImage
                                ({#call getNthAbsCentralMoment#} img n w h)

getMoment n (w,h) image = unsafePerformIO $ do
                            withGenImage image $ \img ->
                             creatingImage
                                ({#call getNthMoment#} img n w h)
-- TODO: ADD checks above!

secondMomentBinarizeOp t = ImgOp $ \image ->
                            withGenImage image  (flip {#call smb#} $ t)
secondMomentBinarize t i = unsafeOperate (secondMomentBinarizeOp t) i

secondMomentAdaptiveBinarizeOp w h t = ImgOp $ \image ->
                            withGenImage image
                                (\i-> {#call smab#} i w h t)
secondMomentAdaptiveBinarize w h t i = unsafeOperate (secondMomentAdaptiveBinarizeOp w h t) i

#c
enum SmoothType {
    BlurNoScale =  CV_BLUR_NO_SCALE,
    Blur        =  CV_BLUR,
    Gaussian    =  CV_GAUSSIAN,
    Median      =  CV_MEDIAN ,
    Bilateral   =  CV_BILATERAL
};
#endc
{#enum SmoothType {}#}

{#fun cvSmooth as smooth'
    {withGenImage* `Image GrayScale D32'
    ,withGenImage* `Image GrayScale D32'
    ,`Int',`Int',`Int',`Float',`Float'}
    -> `()'#}

-- | Image operation which applies gaussian or unifarm smoothing with a given window size to the image.
gaussianOp,blurOp,blurNSOp :: (Int, Int) -> ImageOperation GrayScale D32
gaussianOp m =  withMask m $ \(w,h) img -> smooth' img img (fromEnum Gaussian) w h 0 0
blurOp m = withMask m $ \(w,h) img -> smooth' img img (fromEnum Blur) w h 0 0
blurNSOp m = withMask m $ \(w,h) img -> smooth' img img (fromEnum BlurNoScale) w h 0 0

-- | Create a new image by applying gaussian, or uniform smoothing.
gaussian,blur,blurNS :: (Int, Int) -> Image GrayScale D32 -> Image GrayScale D32
gaussian = unsafeOperate . gaussianOp
blur     = unsafeOperate . blurOp
blurNS   = unsafeOperate . blurNSOp

withMask (w,h) op
    | maskIsOk (w,h) = ImgOp $ op (w,h)
    | otherwise = error "One of aperture dimensions is incorrect (should be >=1 and odd))"

-- | Apply bilateral filtering
bilateral :: (Int,Int) -> (Int,Int) -> Image a D8 -> Image a D8
bilateral (w,h) (s1,s2) img = unsafePerformIO $
            withClone img $ \clone ->
             withGenImage img $ \cimg ->
              withGenImage clone $ \ccln -> do
                   {#call cvSmooth#} cimg ccln  (fromIntegral $ fromEnum Bilateral)
                        (fromIntegral w) (fromIntegral h)
                        (realToFrac s1) (realToFrac s2)

class HasMedianFiltering a where
    median :: (Int,Int) -> a -> a

instance HasMedianFiltering (Image GrayScale D8) where
    median = median'

instance HasMedianFiltering (Image RGB D8) where
    median = median'

-- | Perform median filtering on an eight bit image.
median' :: (Int,Int) -> Image c D8 -> Image c D8
median' (w,h) img
  | maskIsOk (w,h) = unsafePerformIO $ do
                    clone2 <- cloneImage img
                    withGenImage img $ \c1 ->
                     withGenImage clone2 $ \c2 ->
                        {#call cvSmooth#} c1 c2  (fromIntegral $ fromEnum Median)
                                                 (fromIntegral w) (fromIntegral h) 0 0
                    return clone2
  | otherwise = error "One of aperture dimensions is incorrect (should be >=1 and odd))"

maskIsOk (w,h) = odd w && odd h && w >0 && h>0

-- General 2D comvolutions
-- Convolve image with specified kernel stored in flat list.
-- Kernel must have dimensions (w,h) and specified anchor point
-- (x,y) within (0,0) and (w,h)
convolve2D :: (Point2D anchor, ELP anchor ~ Int) =>
              Matrix D32 -> anchor -> Image GrayScale D32 -> Image GrayScale D32
convolve2D kernel anchor image = unsafePerformIO $
                                      let result = emptyCopy image
                                      in withGenImage image $ \c_img->
                                         withGenImage result $ \c_res->
                                         withMatPtr kernel $ \c_mat ->
                                         with (convertPt anchor) $ \c_pt ->
                                         c'wrapFilter2 c_img c_res c_mat c_pt
                                         >> return result

convolve2DI (x,y) kernel image = unsafePerformIO $
                                      withImage image $ \img->
                                      withImage kernel $ \k ->
                                      creatingImage $
                                       {#call wrapFilter2DImg#}
                                        img k x y

-- | Replace pixel values by the average of the row.
verticalAverage :: Image GrayScale D32 -> Image GrayScale D32
verticalAverage image = unsafePerformIO $ do
                    let (w,h) = getSize image
                    s <- create (w,h)
                    withGenImage image $ \i -> do
                     withGenImage s $ \sum -> do
                      {#call vertical_average#} i sum
                    return s

-- | A type for storing integral images. Integral image stores for every pixel the sum of pixels
--   above and left of it. Such images are used for significantly accelerating the calculation of
--   area averages.
newtype IntegralImage = IntegralImage (Image GrayScale D64)
instance Sized IntegralImage where
    type Size IntegralImage = (Int,Int)
    getSize (IntegralImage i) = getSize i

instance GetPixel IntegralImage where
    type P IntegralImage = Double
    getPixel = getPixel

-- | Calculate the integral image from the given image.
integralImage :: Image GrayScale D32 -> IntegralImage
integralImage image = unsafePerformIO $ do
                    let (w,h) = getSize image
                    s <- create (w+1,h+1)
                    withGenImage image $ \i -> do
                     withGenImage s $ \sum -> do
                      {#call cvIntegral#} i sum nullPtr nullPtr
                      return $ IntegralImage s

-- |Filter the image with box shaped averaging mask.
haar :: IntegralImage -> (Int,Int,Int,Int) -> Image GrayScale D32
haar (IntegralImage image) (a',b',c',d') = unsafePerformIO $ do
                    let (w,h) = getSize image
                    let [a,b,c,d] = map fromIntegral [a',b',c',d']
                    r <- create (w,h)
                    withImage image $ \sum ->
                     withImage r $ \res -> do
                            {#call haarFilter#} sum
                                (min a c)
                                (max b d)
                                (max a c)
                                (min b d)
                                res
                            return r

-- | Get an average of a given region.
haarAt  :: IntegralImage -> (Int,Int,Int,Int) -> Double

haarAt (IntegralImage ii) (a,b,w,h) = realToFrac $ unsafePerformIO $ withImage ii $ \i ->
                                        {#call haar_at#} i (f a) (f b) (f w) (f h)
                                    where f = fromIntegral

{-#LANGUAGE ForeignFunctionInterface, TypeFamilies, FlexibleInstances#-}
#include "cvWrapLEO.h"
{-#OPTIONS_GHC -fwarn-unused-imports#-}

-- | This module is a collection of various image filters
module CV.Filters(gaussian,gaussianOp
              ,blurOp,blur,blurNS
              ,HasMedianFiltering,median
              ,susan,getCentralMoment,getAbsCentralMoment
              ,getMoment,secondMomentBinarize,secondMomentBinarizeOp
              ,secondMomentAdaptiveBinarize,secondMomentAdaptiveBinarizeOp
              ,selectiveAvg,convolve2D,convolve2DI,haar,haarAt
              ,IntegralImage(),integralImage,verticalAverage) where

import Foreign.C.Types
import Foreign.Ptr
import Foreign.Marshal.Utils
import CV.Bindings.ImgProc

import Utils.GeometryClass
import CV.Matrix (Matrix,withMatPtr)
import CV.ImageOp

import System.IO.Unsafe

--import C2HSTools
{#import CV.Image#}

-- Low level wrapper for Susan filtering:
--  IplImage* susanSmooth(IplImage *src, int w, int h
--                     ,double t, double sigma);

-- | SUSAN adaptive smoothing filter, see <http://users.fmrib.ox.ac.uk/~steve/susan/susan/susan.html>
susan :: (Int, Int) -> Double -> Double
     -> Image GrayScale D32 -> Image GrayScale D32
susan (w,h) t sigma image = unsafePerformIO $ do
                            withGenImage image $ \img ->
                             creatingImage
                                ({#call susanSmooth#} img (fromIntegral w) (fromIntegral h)
                                                         (realToFrac t) (realToFrac sigma))
-- TODO: ADD checks above!

-- | A selective average filter is an edge preserving noise reduction filter.
--   It is a standard gaussian filter which ignores pixel values
--   that are more than a given threshold away from the filtered pixel value.
selectiveAvg :: (Int, Int) -> Double
     -> Image GrayScale D32 -> Image GrayScale D32
selectiveAvg (w,h) t image = unsafePerformIO $ do
                              withGenImage image $ \img ->
                               creatingImage
                                ({#call selectiveAvgFilter#}
                                    img (realToFrac t) (fromIntegral w) (fromIntegral h))
-- TODO: ADD checks above!

getCentralMoment n (w,h) image = unsafePerformIO $ do
                            withGenImage image $ \img ->
                             creatingImage
                                ({#call getNthCentralMoment#} img n w h)

getAbsCentralMoment n (w,h) image = unsafePerformIO $ do
                            withGenImage image $ \img ->
                             creatingImage
                                ({#call getNthAbsCentralMoment#} img n w h)

getMoment n (w,h) image = unsafePerformIO $ do
                            withGenImage image $ \img ->
                             creatingImage
                                ({#call getNthMoment#} img n w h)
-- TODO: ADD checks above!

secondMomentBinarizeOp t = ImgOp $ \image ->
                            withGenImage image  (flip {#call smb#} $ t)
secondMomentBinarize t i = unsafeOperate (secondMomentBinarizeOp t) i

secondMomentAdaptiveBinarizeOp w h t = ImgOp $ \image ->
                            withGenImage image
                                (\i-> {#call smab#} i w h t)
secondMomentAdaptiveBinarize w h t i = unsafeOperate (secondMomentAdaptiveBinarizeOp w h t) i

data SmoothType = BlurNoScale | Blur
                | Gaussian | Median
                | Bilateral
                deriving(Enum)

{#fun cvSmooth as smooth'
    {withGenImage* `Image GrayScale D32'
    ,withGenImage* `Image GrayScale D32'
    ,`Int',`Int',`Int',`Float',`Float'}
    -> `()'#}

gaussianOp (w,h)
    | maskIsOk (w,h) = ImgOp $ \img ->
                               smooth' img img (fromEnum Gaussian) w h 0 0
    | otherwise = error "One of aperture dimensions is incorrect (should be >=1 and odd))"

gaussian = unsafeOperate.gaussianOp

blurOp (w,h)
    | maskIsOk (w,h) = ImgOp $ \img ->
                               smooth' img img (fromEnum Blur) w h 0 0
    | otherwise = error "One of aperture dimensions is incorrect (should be >=1 and odd))"

blurNSOp (w,h)
    | maskIsOk (w,h) = ImgOp $ \img ->
                               smooth' img img (fromEnum BlurNoScale) w h 0 0
    | otherwise = error "One of aperture dimensions is incorrect (should be >=1 and odd))"

blur size image = let r = unsafeOperate (blurOp size) image
                in  r
blurNS size image = let r = unsafeOperate (blurNSOp size) image
                in  r

-- | TODO: This doesn't give a reasonable result. Investigate
bilateral :: Int -> Int -> Image GrayScale D8 -> Image GrayScale D8
bilateral colorS spaceS img = unsafePerformIO $
            withClone img $ \clone ->
             withGenImage img $ \cimg ->
              withGenImage clone $ \ccln -> do
                   {#call cvSmooth#} cimg ccln  (fromIntegral $ fromEnum Bilateral)
                        (fromIntegral colorS) (fromIntegral spaceS) 0 0

-- TODO: The type is not exactly correct

class HasMedianFiltering a where
    median :: (Int,Int) -> a -> a

instance HasMedianFiltering (Image GrayScale D8) where
    median = median'

instance HasMedianFiltering (Image RGB D8) where
    median = median'

-- | Perform median filtering on an eight bit image.
median' :: (Int,Int) -> Image c D8 -> Image c D8
median' (w,h) img
  | maskIsOk (w,h) = unsafePerformIO $ do
                    clone2 <- cloneImage img
                    withGenImage img $ \c1 ->
                     withGenImage clone2 $ \c2 ->
                        {#call cvSmooth#} c1 c2  (fromIntegral $ fromEnum Median)
                                                 (fromIntegral w) (fromIntegral h) 0 0
                    return clone2
  | otherwise = error "One of aperture dimensions is incorrect (should be >=1 and odd))"

maskIsOk (w,h) = odd w && odd h && w >0 && h>0

-- General 2D comvolutions
-- Convolve image with specified kernel stored in flat list.
-- Kernel must have dimensions (w,h) and specified anchor point
-- (x,y) within (0,0) and (w,h)
convolve2D :: (Point2D anchor, ELP anchor ~ Int) =>
              Matrix D32 -> anchor -> Image GrayScale D32 -> Image GrayScale D32
convolve2D kernel anchor image = unsafePerformIO $
                                      let result = emptyCopy image
                                      in withGenImage image $ \c_img->
                                         withGenImage result $ \c_res->
                                         withMatPtr kernel $ \c_mat ->
                                         with (convertPt anchor) $ \c_pt ->
                                         c'wrapFilter2 c_img c_res c_mat c_pt
                                         >> return result

convolve2DI (x,y) kernel image = unsafePerformIO $
                                      withImage image $ \img->
                                      withImage kernel $ \k ->
                                      creatingImage $
                                       {#call wrapFilter2DImg#}
                                        img k x y

-- | Replace pixel values by the average of the row.
verticalAverage :: Image GrayScale D32 -> Image GrayScale D32
verticalAverage image = unsafePerformIO $ do
                    let (w,h) = getSize image
                    s <- create (w,h)
                    withGenImage image $ \i -> do
                     withGenImage s $ \sum -> do
                      {#call vertical_average#} i sum
                    return s

-- | A type for storing integral images. Integral image stores for every pixel the sum of pixels
--   above and left of it. Such images are used for significantly accelerating the calculation of
--   area averages.
newtype IntegralImage = IntegralImage (Image GrayScale D64)
instance Sized IntegralImage where
    type Size IntegralImage = (Int,Int)
    getSize (IntegralImage i) = getSize i

instance GetPixel IntegralImage where
    type P IntegralImage = Double
    getPixel = getPixel

-- | Calculate the integral image from the given image.
integralImage :: Image GrayScale D32 -> IntegralImage
integralImage image = unsafePerformIO $ do
                    let (w,h) = getSize image
                    s <- create (w+1,h+1)
                    withGenImage image $ \i -> do
                     withGenImage s $ \sum -> do
                      {#call cvIntegral#} i sum nullPtr nullPtr
                      return $ IntegralImage s

-- |Filter the image with box shaped averaging mask.
haar :: IntegralImage -> (Int,Int,Int,Int) -> Image GrayScale D32
haar (IntegralImage image) (a',b',c',d') = unsafePerformIO $ do
                    let (w,h) = getSize image
                    let [a,b,c,d] = map fromIntegral [a',b',c',d']
                    r <- create (w,h)
                    withImage image $ \sum ->
                     withImage r $ \res -> do
                            {#call haarFilter#} sum
                                (min a c)
                                (max b d)
                                (max a c)
                                (min b d)
                                res
                            return r

-- | Get an average of a given region.
haarAt  :: IntegralImage -> (Int,Int,Int,Int) -> Double

haarAt (IntegralImage ii) (a,b,w,h) = realToFrac $ unsafePerformIO $ withImage ii $ \i ->
                                        {#call haar_at#} i (f a) (f b) (f w) (f h)
                                    where f = fromIntegral


Make more networky

Make more networky

{-# LANGUAGE ScopedTypeVariables #-}
module Network.Bluetooth.Linux.Socket where

import Control.Concurrent
import Control.Exception
import Control.Monad

import Data.Either

import Foreign.C.Error
import Foreign.C.Types
import Foreign.Marshal.Alloc
import Foreign.Marshal.Utils
import Foreign.Ptr
import Foreign.Storable

import Network.Bluetooth.Exception
import Network.Bluetooth.Linux.Addr
import Network.Bluetooth.Linux.Protocol
import Network.Bluetooth.Utils
import Network.Socket
import Network.Socket.Internal

import System.Posix.Internals (setNonBlockingFD)

#include <bluetooth/bluetooth.h>
#include <sys/socket.h>
#include "wr_l2cap.h"
#include "wr_rfcomm.h"

bluetoothSocket :: BluetoothProtocol -> IO Socket
bluetoothSocket proto = socket AF_BLUETOOTH sockType protoNum
  where
    sockType = case proto of
        L2CAP  -> SeqPacket
        RFCOMM -> Stream
    protoNum = cFromEnum proto

bluetoothBind :: Socket -> BluetoothAddr -> BluetoothPort -> IO ()
bluetoothBind (MkSocket fd _ _ proto sockStatus) bdaddr portNum =
    modifyMVar_ sockStatus $ \status -> do
        when (status /= NotConnected) . ioError . userError $
          "bind: can't peform bind on socket in status " ++ show status
        let protoEnum = cToEnum proto
        unless (isBluetoothPortValid protoEnum portNum) . throwIO $ BluetoothPortException protoEnum portNum
        case protoEnum of
             L2CAP  -> callBind $ SockAddrL2CAP AF_BLUETOOTH (c_htobs portNum) bdaddr
             RFCOMM -> callBind . SockAddrRFCOMM AF_BLUETOOTH bdaddr $ fromIntegral portNum
        return Bound
  where
    callBind :: SockAddrBluetooth a => a -> IO ()
    callBind sockaddr = throwSocketErrorIfMinus1Retry_ "bind" $ c_bind fd sockaddr

bluetoothBindAnyPort :: Socket -> BluetoothAddr -> IO BluetoothPort
bluetoothBindAnyPort sock@(MkSocket _ _ _ proto _) bdaddr = do
    port <- bindAnyPort (cToEnum proto) bdaddr
    bluetoothBind sock bdaddr $ fromIntegral port
    return port

bluetoothListen :: Socket -> Int -> IO ()
bluetoothListen = listen

bluetoothAccept :: Socket -> IO (Socket, BluetoothAddr)
bluetoothAccept sock@(MkSocket _ family sockType proto sockStatus) = do
    currentStatus <- readMVar sockStatus
    when (currentStatus /= Connected && currentStatus /= Listening) . ioError . userError $
      "accept: can't perform accept on socket (" ++ show (family,sockType,proto) ++ ") in status " ++ show currentStatus
    case cToEnum proto of
         L2CAP  -> alloca $ \(sockaddrPtr :: Ptr SockAddrL2CAP) ->
           callAccept sock sockaddrPtr $ sizeOf (undefined :: SockAddrL2CAP)
         RFCOMM -> alloca $ \(sockaddrPtr :: Ptr SockAddrRFCOMM) ->
           callAccept sock sockaddrPtr $ sizeOf (undefined :: SockAddrRFCOMM)
  where
    callAccept :: SockAddrBluetooth a => Socket -> Ptr a -> Int -> IO (Socket, BluetoothAddr)
    callAccept sock'@(MkSocket fd _ _ _ _) sockaddrPtr size = do
          newFd <- throwSocketErrorWaitRead sock' "accept" . fmap fst $ c_accept fd sockaddrPtr size
          setNonBlockIfNeeded newFd
          bdaddr <- fmap sockAddrBluetoothAddr $ peek sockaddrPtr
          newStatus <- newMVar Connected
          return (MkSocket newFd family sockType proto newStatus, bdaddr)

bluetoothConnect :: Socket -> BluetoothAddr -> BluetoothPort -> IO ()
bluetoothConnect sock@(MkSocket fd _ _ proto sockStatus) bdaddr portNum = do
    status <- takeMVar sockStatus
    when (status /= NotConnected && status /= Bound) . ioError . userError $
        "connect: can't peform connect on socket in status " ++ show status
    let protoEnum = cToEnum proto
    unless (isBluetoothPortValid protoEnum portNum) . throwIO $ BluetoothPortException protoEnum portNum

    let connectLoop :: IO ()
        connectLoop = do
            r <- case protoEnum of
                L2CAP  -> c_connect fd $ SockAddrL2CAP AF_BLUETOOTH (c_htobs portNum) bdaddr
                RFCOMM -> c_connect fd . SockAddrRFCOMM AF_BLUETOOTH bdaddr $ fromIntegral portNum
            putMVar sockStatus Connected
            when (r == -1) $ do
                err <- getErrno
                case () of
                    _ | err == eINTR       -> connectLoop
                    _ | err == eINPROGRESS -> connectBlocked
                    _otherwise             -> throwSocketError "connect"

        connectBlocked :: IO ()
        connectBlocked = do
            threadWaitWrite $ fromIntegral fd
            err <- getSocketOption sock SoError
            unless (err == 0) . throwSocketErrorCode "connect" $ fromIntegral err

    connectLoop `onException` close sock

bluetoothSocketPort :: Socket -> IO BluetoothPort
bluetoothSocketPort sock@(MkSocket _ _ _ proto status) = do
    currentStatus <- readMVar status
    when (currentStatus == NotConnected || currentStatus == Closed) . ioError . userError $
      "getsockname: can't get name of socket in status " ++ show currentStatus
    case cToEnum proto of
         L2CAP -> alloca $ \(sockaddrPtr :: Ptr SockAddrL2CAP) ->
           callGetSockName sock sockaddrPtr $ sizeOf (undefined :: SockAddrL2CAP)
         RFCOMM -> alloca $ \(sockaddrPtr :: Ptr SockAddrRFCOMM) ->
           callGetSockName sock sockaddrPtr $ sizeOf (undefined :: SockAddrRFCOMM)
  where
    callGetSockName :: SockAddrBluetooth a => Socket -> Ptr a -> Int -> IO BluetoothPort
    callGetSockName (MkSocket fd _ _ _ _) sockaddrPtr size = do
        throwSocketErrorIfMinus1Retry_ "getsockname" . fmap fst $ c_getsockname fd sockaddrPtr size
        fmap sockAddrPort $ peek sockaddrPtr

-------------------------------------------------------------------------------

bindAnyPort :: BluetoothProtocol -> BluetoothAddr -> IO BluetoothPort
bindAnyPort proto addr = do
    avails <- flip filterM (portRange proto) $ \portNum -> do
        sock <- bluetoothSocket proto
        res  <- try $ bluetoothBind sock addr portNum
        close sock
        return $ isRight (res :: Either IOError ())
    case avails of
        portNum:_ -> return portNum
        _         -> ioError $ userError "Unable to find any available port"
  where
    portRange L2CAP  = [4097, 4099 .. 32767]
    portRange RFCOMM = [1 .. 30]

setNonBlockIfNeeded :: CInt -> IO ()
setNonBlockIfNeeded = flip setNonBlockingFD True

{#fun unsafe bind as c_bind
  `SockAddrBluetooth a' =>
  { id               `CInt'
  , withCastLenConv* `a'&
  }               -> `Int' #}

{#fun unsafe accept as c_accept
  `SockAddrBluetooth a' =>
  { id      `CInt'
  , castPtr `Ptr a'
  ,         `Int'  peekFromIntegral*
  }      -> `CInt' id #}

{#fun unsafe connect as c_connect
  `SockAddrBluetooth a' =>
  { id               `CInt'
  , withCastLenConv* `a'&
  }               -> `Int' #}

{#fun unsafe getsockname as c_getsockname
  `SockAddrBluetooth a' =>
  { id      `CInt'
  , castPtr `Ptr a'
  ,         `Int'  peekFromIntegral*
  }          -> `CInt' id #}

{#fun pure wr_htobs as c_htobs
  `Integral i' =>
  { fromIntegral `i'
  }           -> `i' fromIntegral #}

{-# LANGUAGE ScopedTypeVariables #-}
module Network.Bluetooth.Linux.Socket where

import Control.Concurrent
import Control.Exception
import Control.Monad

import Data.Either

import Foreign.C.Error
import Foreign.C.Types
import Foreign.Marshal.Alloc
import Foreign.Marshal.Utils
import Foreign.Ptr
import Foreign.Storable

import Network.Bluetooth.Exception
import Network.Bluetooth.Linux.Addr
import Network.Bluetooth.Linux.Protocol
import Network.Bluetooth.Utils
import Network.Socket
import Network.Socket.Internal

#include <bluetooth/bluetooth.h>
#include <sys/socket.h>
#include "wr_l2cap.h"
#include "wr_rfcomm.h"

bluetoothSocket :: BluetoothProtocol -> IO Socket
bluetoothSocket proto = socket AF_BLUETOOTH sockType protoNum
  where
    sockType = case proto of
        L2CAP  -> SeqPacket
        RFCOMM -> Stream
    protoNum = cFromEnum proto

bluetoothBind :: Socket -> BluetoothAddr -> BluetoothPort -> IO ()
bluetoothBind (MkSocket fd _ _ proto sockStatus) bdaddr portNum =
    modifyMVar_ sockStatus $ \status -> do
        when (status /= NotConnected) . ioError . userError $
          "bind: can't peform bind on socket in status " ++ show status
        let protoEnum = cToEnum proto
        unless (isBluetoothPortValid protoEnum portNum) . throwIO $ BluetoothPortException protoEnum portNum
        case protoEnum of
             L2CAP  -> callBind $ SockAddrL2CAP AF_BLUETOOTH (c_htobs portNum) bdaddr
             RFCOMM -> callBind . SockAddrRFCOMM AF_BLUETOOTH bdaddr $ fromIntegral portNum
        return Bound
  where
    callBind :: SockAddrBluetooth a => a -> IO ()
    callBind sockaddr = throwSocketErrorIfMinus1Retry_ "bind" $ c_bind fd sockaddr

bluetoothBindAnyPort :: Socket -> BluetoothAddr -> IO BluetoothPort
bluetoothBindAnyPort sock@(MkSocket _ _ _ proto _) bdaddr = do
    port <- bindAnyPort (cToEnum proto) bdaddr
    bluetoothBind sock bdaddr $ fromIntegral port
    return port

bluetoothListen :: Socket -> Int -> IO ()
bluetoothListen = listen

bluetoothAccept :: Socket -> IO (Socket, BluetoothAddr)
bluetoothAccept sock@(MkSocket _ family sockType proto sockStatus) = do
    currentStatus <- readMVar sockStatus
    when (currentStatus /= Connected && currentStatus /= Listening) . ioError . userError $
      "accept: can't perform accept on socket (" ++ show (family,sockType,proto) ++ ") in status " ++ show currentStatus
    case cToEnum proto of
         L2CAP  -> alloca $ \(sockaddrPtr :: Ptr SockAddrL2CAP) ->
           callAccept sock sockaddrPtr $ sizeOf (undefined :: SockAddrL2CAP)
         RFCOMM -> alloca $ \(sockaddrPtr :: Ptr SockAddrRFCOMM) ->
           callAccept sock sockaddrPtr $ sizeOf (undefined :: SockAddrRFCOMM)
  where
    callAccept :: SockAddrBluetooth a => Socket -> Ptr a -> Int -> IO (Socket, BluetoothAddr)
    callAccept sock'@(MkSocket fd _ _ _ _) sockaddrPtr size = do
          newFd <- throwSocketErrorWaitRead sock' "accept" . fmap fst $ c_accept fd sockaddrPtr size
          bdaddr <- fmap sockAddrBluetoothAddr $ peek sockaddrPtr
          newStatus <- newMVar Connected
          return (MkSocket newFd family sockType proto newStatus, bdaddr)

bluetoothConnect :: Socket -> BluetoothAddr -> BluetoothPort -> IO ()
bluetoothConnect sock@(MkSocket fd _ _ proto sockStatus) bdaddr portNum = do
    status <- takeMVar sockStatus
    when (status /= NotConnected && status /= Bound) . ioError . userError $
        "connect: can't peform connect on socket in status " ++ show status
    let protoEnum = cToEnum proto
    unless (isBluetoothPortValid protoEnum portNum) . throwIO $ BluetoothPortException protoEnum portNum

    let connectLoop :: IO ()
        connectLoop = do
            r <- case protoEnum of
                L2CAP  -> c_connect fd $ SockAddrL2CAP AF_BLUETOOTH (c_htobs portNum) bdaddr
                RFCOMM -> c_connect fd . SockAddrRFCOMM AF_BLUETOOTH bdaddr $ fromIntegral portNum
            putMVar sockStatus Connected
            when (r == -1) $ do
                err <- getErrno
                case () of
                    _ | err == eINTR       -> connectLoop
                    _ | err == eINPROGRESS -> connectBlocked
                    _otherwise             -> throwSocketError "connect"

        connectBlocked :: IO ()
        connectBlocked = do
            threadWaitWrite $ fromIntegral fd
            err <- getSocketOption sock SoError
            unless (err == 0) . throwSocketErrorCode "connect" $ fromIntegral err

    connectLoop `onException` close sock

bluetoothSocketPort :: Socket -> IO BluetoothPort
bluetoothSocketPort sock@(MkSocket _ _ _ proto status) = do
    currentStatus <- readMVar status
    when (currentStatus == NotConnected || currentStatus == Closed) . ioError . userError $
      "getsockname: can't get name of socket in status " ++ show currentStatus
    case cToEnum proto of
         L2CAP -> alloca $ \(sockaddrPtr :: Ptr SockAddrL2CAP) ->
           callGetSockName sock sockaddrPtr $ sizeOf (undefined :: SockAddrL2CAP)
         RFCOMM -> alloca $ \(sockaddrPtr :: Ptr SockAddrRFCOMM) ->
           callGetSockName sock sockaddrPtr $ sizeOf (undefined :: SockAddrRFCOMM)
  where
    callGetSockName :: SockAddrBluetooth a => Socket -> Ptr a -> Int -> IO BluetoothPort
    callGetSockName (MkSocket fd _ _ _ _) sockaddrPtr size = do
        throwSocketErrorIfMinus1Retry_ "getsockname" . fmap fst $ c_getsockname fd sockaddrPtr size
        fmap sockAddrPort $ peek sockaddrPtr

-------------------------------------------------------------------------------

bindAnyPort :: BluetoothProtocol -> BluetoothAddr -> IO BluetoothPort
bindAnyPort proto addr = do
    avails <- flip filterM (portRange proto) $ \portNum -> do
        sock <- bluetoothSocket proto
        res  <- try $ bluetoothBind sock addr portNum
        close sock
        return $ isRight (res :: Either IOError ())
    case avails of
        portNum:_ -> return portNum
        _         -> ioError $ userError "Unable to find any available port"
  where
    portRange L2CAP  = [4097, 4099 .. 32767]
    portRange RFCOMM = [1 .. 30]

{#fun unsafe bind as c_bind
  `SockAddrBluetooth a' =>
  { id               `CInt'
  , withCastLenConv* `a'&
  }               -> `Int' #}

{#fun unsafe accept as c_accept
  `SockAddrBluetooth a' =>
  { id      `CInt'
  , castPtr `Ptr a'
  ,         `Int'  peekFromIntegral*
  }      -> `CInt' id #}

{#fun unsafe connect as c_connect
  `SockAddrBluetooth a' =>
  { id               `CInt'
  , withCastLenConv* `a'&
  }               -> `Int' #}

{#fun unsafe getsockname as c_getsockname
  `SockAddrBluetooth a' =>
  { id      `CInt'
  , castPtr `Ptr a'
  ,         `Int'  peekFromIntegral*
  }          -> `CInt' id #}

{#fun pure wr_htobs as c_htobs
  `Integral i' =>
  { fromIntegral `i'
  }           -> `i' fromIntegral #}


For compatibility reasons re-ordered Image.montage output

{-#LANGUAGE ForeignFunctionInterface, ViewPatterns,ParallelListComp, FlexibleInstances, FlexibleContexts, TypeFamilies, EmptyDataDecls #-}
#include "cvWrapLeo.h"
module CV.Image where

import System.Posix.Files
import System.Mem

import Foreign.C.Types
import Foreign.C.String
import Foreign.ForeignPtr
import Foreign.Ptr
import Control.Parallel.Strategies
import Control.DeepSeq

-- import C2HSTools

import Data.Maybe(catMaybes)
import Data.List(genericLength)
import Foreign.Marshal.Array
import Foreign.Marshal.Alloc
import Foreign.Ptr
import Foreign.Storable
import System.IO.Unsafe
import Data.Word

-- Colorspaces
data GrayScale
data RGB
data RGB_Channel = Red | Green | Blue deriving (Eq,Ord,Enum)
data RGBA
data LAB
data LAB_Channel = LAB_L | LAB_A | LAB_B deriving (Eq,Ord,Enum)
type family ChannelOf a :: *
type instance ChannelOf RGB_Channel = RGB
type instance ChannelOf LAB_Channel = LAB

-- Bit Depths
type D8  = Word8
type D32 = Float
type D64 = Double

newtype Image channels depth = S BareImage

unS (S i) = i -- Unsafe and ugly

withImage :: Image c d -> (Ptr BareImage ->IO a) -> IO a
withImage (S i) op = withBareImage i op
--withGenNewImage (S i) op = withGenImage i op

-- Ok. this is just the example why I need image types
withUniPtr with x fun = with x $ \y ->
                    fun (castPtr y)

withGenImage = withUniPtr withImage
withGenBareImage = withUniPtr withBareImage

{#pointer *IplImage as BareImage foreign newtype#}

foreign import ccall "& wrapReleaseImage" releaseImage :: FinalizerPtr BareImage

instance NFData (Image a b) where
    rnf a@(S (BareImage fptr)) = (unsafeForeignPtrToPtr) fptr `seq` a `seq` ()-- This might also need peek?

creatingImage fun = do
              iptr <- fun
--              {#call incrImageC#} -- Uncomment this line to get statistics of number of images allocated by ghc
              fptr <- newForeignPtr releaseImage iptr
              return . S . BareImage $ fptr

creatingBareImage fun = do
              iptr <- fun
--              {#call incrImageC#} -- Uncomment this line to get statistics of number of images allocated by ghc
              fptr <- newForeignPtr releaseImage iptr
              return . BareImage $ fptr

unImage (S (BareImage fptr)) = fptr

data Tag tp;
rgb = undefined :: Tag RGB
rgba = undefined :: Tag RGBA
lab = undefined :: Tag LAB

composeMultichannelImage :: (CreateImage (Image tp a)) => Maybe (Image GrayScale a) -> Maybe (Image GrayScale a) -> Maybe (Image GrayScale a) -> Maybe (Image GrayScale a) -> Tag tp -> Image tp a
composeMultichannelImage (c1)
                         (c2)
                         (c3)
                         (c4)
                         totag
    = unsafePerformIO $ do
        res <- create (size) -- TODO: Check channel count -- This is NOT correct
        withMaybe c1 $ \cc1 ->
         withMaybe c2 $ \cc2 ->
          withMaybe c3 $ \cc3 ->
           withMaybe c4 $ \cc4 ->
            withGenImage res $ \cres -> {#call cvMerge#} cc1 cc2 cc3 cc4 cres
        return res
    where
        withMaybe (Just i) op = withGenImage i op
        withMaybe (Nothing) op = op nullPtr
        size = getSize . head . catMaybes $ [c1,c2,c3,c4]

-- Load Image as grayscale image.

--loadImage n = do
--              exists <- fileExist n
--              if not exists then return Nothing
--                            else do
--                              i <- withCString n $ \name ->
--                                     creatingImage ({#call cvLoadImage #} name (0))
--                              bw <- imageTo32F i
--                              return $ Just bw

loadImage :: FilePath -> IO (Maybe (Image GrayScale D32))
loadImage n = do
              exists <- fileExist n
              if not exists then return Nothing
                            else do
                              i <- withCString n $ \name ->
                                     creatingBareImage ({#call cvLoadImage #} name (0))
                              bw <- imageTo32F i
                              return . Just . S $ bw

loadColorImage :: FilePath -> IO (Maybe (Image RGB D32))
loadColorImage n = do
              exists <- fileExist n
              if not exists then return Nothing
                            else do
                              i <- withCString n $ \name ->
                                     creatingBareImage ({#call cvLoadImage #} name 1)
                              bw <- imageTo32F i
                              return . Just . S  $ bw

class IntSized a where
    getSize :: a -> (Int,Int)

instance IntSized BareImage where
   -- getSize :: (Integral a, Integral b) => Image c d -> (a,b)
    getSize image = unsafePerformIO $ withBareImage image $ \i -> do
                 w <- {#call getImageWidth#} i
                 h <- {#call getImageHeight#} i
                 return (fromIntegral w,fromIntegral h)

instance IntSized (Image c d) where
    getSize = getSize . unS

cvRGBtoGRAY = 7 :: CInt-- NOTE: This will break.
cvRGBtoLAB = 45 :: CInt-- NOTE: This will break.

rgbToLab :: Image RGB D32 -> Image LAB D32
rgbToLab = S . convertTo cvRGBtoLAB 3 . unS

rgbToGray :: Image RGB D32 -> Image GrayScale D32
rgbToGray = S . convertTo cvRGBtoGRAY 1 . unS

class GetPixel a where
    type P a :: *
    getPixel   :: (Integral i) => (i,i) -> a -> P a

instance GetPixel (Image GrayScale D32) where
    type P (Image GrayScale D32) = D32
    getPixel (fromIntegral -> x, fromIntegral -> y) image = realToFrac $ unsafePerformIO $
             withGenImage image $ \img -> {#call wrapGet32F2D#} img y x

instance  GetPixel (Image RGB D32) where
    type P (Image RGB D32) = (D32,D32,D32)
    getPixel (fromIntegral -> x, fromIntegral -> y) image
        = unsafePerformIO $ do
                     withGenImage image $ \img -> do
                              r <- {#call wrapGet32F2DC#} img y x 0
                              g <- {#call wrapGet32F2DC#} img y x 1
                              b <- {#call wrapGet32F2DC#} img y x 2
                              return (realToFrac r,realToFrac g, realToFrac b)

convertTo :: CInt -> CInt -> BareImage -> BareImage
convertTo code channels img = unsafePerformIO $ creatingBareImage $ do
    res <- {#call wrapCreateImage32F#} w h channels
    withBareImage img $ \cimg ->
        {#call cvCvtColor#} (castPtr cimg) (castPtr res) code
    return res
 where
    (fromIntegral -> w,fromIntegral -> h) = getSize img

class CreateImage a where
    create :: (Int,Int) -> IO a

instance CreateImage (Image GrayScale D32) where
    create (w,h) = creatingImage $ {#call wrapCreateImage32F#} (fromIntegral w) (fromIntegral h) 1
instance CreateImage (Image LAB D32) where
    create (w,h) = creatingImage $ {#call wrapCreateImage32F#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGB D32) where
    create (w,h) = creatingImage $ {#call wrapCreateImage32F#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGBA D32) where
    create (w,h) = creatingImage $ {#call wrapCreateImage32F#} (fromIntegral w) (fromIntegral h) 4

instance CreateImage (Image GrayScale D64) where
    create (w,h) = creatingImage $ {#call wrapCreateImage64F#} (fromIntegral w) (fromIntegral h) 1
instance CreateImage (Image LAB D64) where
    create (w,h) = creatingImage $ {#call wrapCreateImage64F#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGB D64) where
    create (w,h) = creatingImage $ {#call wrapCreateImage64F#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGBA D64) where
    create (w,h) = creatingImage $ {#call wrapCreateImage64F#} (fromIntegral w) (fromIntegral h) 4

instance CreateImage (Image GrayScale D8) where
    create (w,h) = creatingImage $ {#call wrapCreateImage8U#} (fromIntegral w) (fromIntegral h) 1
instance CreateImage (Image LAB D8) where
    create (w,h) = creatingImage $ {#call wrapCreateImage8U#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGB D8) where
    create (w,h) = creatingImage $ {#call wrapCreateImage8U#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGBA D8) where
    create (w,h) = creatingImage $ {#call wrapCreateImage8U#} (fromIntegral w) (fromIntegral h) 4

empty :: (CreateImage (Image a b)) => (Int,Int) -> (Image a b)
empty size = unsafePerformIO $ create size

emptyCopy :: (CreateImage (Image a b)) => Image a b -> IO (Image a b)
emptyCopy img = create (getSize img)

emptyCopy' :: (CreateImage (Image a b)) => Image a b -> (Image a b)
emptyCopy' img = unsafePerformIO $ create (getSize img)

-- | Save image. This will convert the image to 8 bit one before saving
saveImage :: FilePath -> Image c d -> IO ()
saveImage filename image = do
                           fpi <- imageTo8Bit $ unS image
                           withCString  filename $ \name  ->
                            withGenBareImage fpi    $ \cvArr ->
							 alloca (\defs -> poke defs 0 >> {#call cvSaveImage #} name cvArr defs >> return ())

getArea :: (IntSized a) => a -> Int
getArea = uncurry (*).getSize

getRegion :: (Int,Int) -> (Int,Int) -> Image c d -> Image c d
getRegion (fromIntegral -> x,fromIntegral -> y) (fromIntegral -> w,fromIntegral -> h) image
    | x+w <= width && y+h <= height = S . getRegion' (x,y) (w,h) $ unS image
    | otherwise                   = error $ "Region outside image:"
                                            ++ show (getSize image) ++
                                            "/"++show (x+w,y+h)
 where
  (fromIntegral -> width,fromIntegral -> height) = getSize image

getRegion' (x,y) (w,h) image = unsafePerformIO $
                               withBareImage image $ \i ->
                                 creatingBareImage ({#call getSubImage#}
                                                i x y w h)

-- | Tile images by overlapping them on a black canvas.
tileImages image1 image2 (x,y) = unsafePerformIO $
                               withImage image1 $ \i1 ->
                                withImage image2 $ \i2 ->
                                 creatingImage ({#call simpleMergeImages#}
                                                i1 i2 x y)
-- | Blit image2 onto image1.
blitFix = blit
blit image1 image2 (x,y)
    | badSizes  = error $ "Bad blit sizes: " ++ show [(w1,h1),(w2,h2)]++"<-"++show (x,y)
    | otherwise = withImage image1 $ \i1 ->
                   withImage image2 $ \i2 ->
                    ({#call plainBlit#} i1 i2 (fromIntegral y) (fromIntegral x))
    where
     ((w1,h1),(w2,h2)) = (getSize image1,getSize image2)
     badSizes = x+w2>w1 || y+h2>h1 || x<0 || y<0

blitM :: (CreateImage (Image c d)) => (Int,Int) -> [((Int,Int),Image c d)] -> Image c d
blitM (rw,rh) imgs = resultPic
    where
     resultPic = unsafePerformIO $ do
                    r <- create (fromIntegral rw,fromIntegral rh)
                    sequence_ [blit r i (fromIntegral x, fromIntegral y)
                              | ((x,y),i) <- imgs ]
                    return r

subPixelBlit
  :: Image c d -> Image c d -> (CDouble, CDouble) -> IO ()

subPixelBlit (image1) (image2) (x,y)
    | badSizes  = error $ "Bad blit sizes: " ++ show [(w1,h1),(w2,h2)]++"<-"++show (x,y)
    | otherwise = withImage image1 $ \i1 ->
                   withImage image2 $ \i2 ->
                    ({#call subpixel_blit#} i1 i2 y x)
    where
     ((w1,h1),(w2,h2)) = (getSize image1,getSize image2)
     badSizes = ceiling x+w2>w1 || ceiling y+h2>h1 || x<0 || y<0

safeBlit i1 i2 (x,y) = unsafePerformIO $ do
                  res <- cloneImage i1-- createImage32F (getSize i1) 1
                  blit res i2 (x,y)
                  return res

-- | Blit image2 onto image1.
--   This uses an alpha channel bitmap for determining the regions where the image should be "blended" with
--   the base image.
blendBlit image1 image1Alpha image2 image2Alpha (x,y) =
                               withImage image1 $ \i1 ->
                                withImage image1Alpha $ \i1a ->
                                 withImage image2Alpha $ \i2a ->
                                  withImage image2 $ \i2 ->
                                   ({#call alphaBlit#} i1 i1a i2 i2a x y)

cloneImage img = withGenImage img $ \image ->
                    creatingImage ({#call cvCloneImage #} image)

withClone img fun = do
                result <- cloneImage img
                fun result
                return result

unsafeImageTo32F img = unsafePerformIO $ withGenImage img $ \image ->
                creatingImage
                 ({#call ensure32F #} image)

unsafeImageTo8Bit img = unsafePerformIO $ withGenImage img $ \image ->
                creatingImage
                 ({#call ensure8U #} image)

imageTo32F img = withGenBareImage img $ \image ->
                creatingBareImage
                 ({#call ensure32F #} image)

imageTo8Bit img = withGenBareImage img $ \image ->
                creatingBareImage
                 ({#call ensure8U #} image)

-- Manipulating regions of interest:
setROI (fromIntegral -> x,fromIntegral -> y)
       (fromIntegral -> w,fromIntegral -> h)
       image = withImage image $ \i ->
                            {#call wrapSetImageROI#} i x y w h

resetROI image = withImage image $ \i ->
                  {#call cvResetImageROI#} i

setCOI chnl image = withImage image $ \i ->
                            {#call cvSetImageCOI#} i (fromIntegral chnl)
resetCOI image = withImage image $ \i ->
                  {#call cvSetImageCOI#} i 0

-- #TODO: Replace the Int below with proper channel identifier
getChannel :: (Enum a) => a -> Image (ChannelOf a) d -> Image GrayScale d
getChannel no image = unsafePerformIO $ creatingImage $ do
    let (w,h) = getSize image
    setCOI (1+fromEnum no) image
    cres <- {#call wrapCreateImage32F#} (fromIntegral w) (fromIntegral h) 1
    withGenImage image $ \cimage ->
      {#call cvCopy#} cimage (castPtr cres) (nullPtr)
    resetCOI image
    return cres

withIOROI pos size image op = do
            setROI pos size image
            x <- op
            resetROI image
            return x

withROI :: (Int, Int) -> (Int, Int) -> Image c d -> (Image c d -> a) -> a
withROI pos size image op = unsafePerformIO $ do
                        setROI pos size image
                        let x = op image -- BUG
                        resetROI image
                        return x

-- Manipulating image pixels
--setPixel :: (CInt,CInt) -> CDouble -> Image c d -> IO ()
--setPixel (x,y) v image = withGenImage image $ \img ->
--                          {#call wrapSet32F2D#} img y x v

getAllPixels image =  [getPixel (i,j) image
                      | i <- [0..width-1 ]
                      , j <- [0..height-1]]
                    where
                     (width,height) = getSize image

getAllPixelsRowMajor image =  [getPixel (i,j) image
                              | j <- [0..height-1]
                              , i <- [0..width-1]
                              ]
                    where
                     (width,height) = getSize image

-- |Create a montage form given images (u,v) determines the layout and space the spacing
--  between images. Images are assumed to be the same size (determined by the first image)
montage :: (CreateImage (Image c d)) => (Int,Int) -> Int -> [Image c d] -> Image c d
montage (u',v') space' imgs = resultPic
    where
     space = fromIntegral space'
     (u,v) = (fromIntegral u', fromIntegral v')
     (rw,rh) = (u*xstep,v*ystep)
     (w,h) = getSize (head imgs)
     (xstep,ystep) = (fromIntegral space + w,fromIntegral space + h)
     edge = space`div`2
     resultPic = unsafePerformIO $ do
                    r <- create (rw,rh)
                    sequence_ [blit r i (edge +  x*xstep, edge + y*ystep)
                               | y <- [0..u-1] , x <- [0..v-1]
                               | i <- imgs ]
                    return r

{-#LANGUAGE ForeignFunctionInterface, ViewPatterns,ParallelListComp, FlexibleInstances, FlexibleContexts, TypeFamilies, EmptyDataDecls #-}
#include "cvWrapLeo.h"
module CV.Image where

import System.Posix.Files
import System.Mem

import Foreign.C.Types
import Foreign.C.String
import Foreign.ForeignPtr
import Foreign.Ptr
import Control.Parallel.Strategies
import Control.DeepSeq

-- import C2HSTools

import Data.Maybe(catMaybes)
import Data.List(genericLength)
import Foreign.Marshal.Array
import Foreign.Marshal.Alloc
import Foreign.Ptr
import Foreign.Storable
import System.IO.Unsafe
import Data.Word

-- Colorspaces
data GrayScale
data RGB
data RGB_Channel = Red | Green | Blue deriving (Eq,Ord,Enum)
data RGBA
data LAB
data LAB_Channel = LAB_L | LAB_A | LAB_B deriving (Eq,Ord,Enum)
type family ChannelOf a :: *
type instance ChannelOf RGB_Channel = RGB
type instance ChannelOf LAB_Channel = LAB

-- Bit Depths
type D8  = Word8
type D32 = Float
type D64 = Double

newtype Image channels depth = S BareImage

unS (S i) = i -- Unsafe and ugly

withImage :: Image c d -> (Ptr BareImage ->IO a) -> IO a
withImage (S i) op = withBareImage i op
--withGenNewImage (S i) op = withGenImage i op

-- Ok. this is just the example why I need image types
withUniPtr with x fun = with x $ \y ->
                    fun (castPtr y)

withGenImage = withUniPtr withImage
withGenBareImage = withUniPtr withBareImage

{#pointer *IplImage as BareImage foreign newtype#}

foreign import ccall "& wrapReleaseImage" releaseImage :: FinalizerPtr BareImage

instance NFData (Image a b) where
    rnf a@(S (BareImage fptr)) = (unsafeForeignPtrToPtr) fptr `seq` a `seq` ()-- This might also need peek?

creatingImage fun = do
              iptr <- fun
--              {#call incrImageC#} -- Uncomment this line to get statistics of number of images allocated by ghc
              fptr <- newForeignPtr releaseImage iptr
              return . S . BareImage $ fptr

creatingBareImage fun = do
              iptr <- fun
--              {#call incrImageC#} -- Uncomment this line to get statistics of number of images allocated by ghc
              fptr <- newForeignPtr releaseImage iptr
              return . BareImage $ fptr

unImage (S (BareImage fptr)) = fptr

data Tag tp;
rgb = undefined :: Tag RGB
rgba = undefined :: Tag RGBA
lab = undefined :: Tag LAB

composeMultichannelImage :: (CreateImage (Image tp a)) => Maybe (Image GrayScale a) -> Maybe (Image GrayScale a) -> Maybe (Image GrayScale a) -> Maybe (Image GrayScale a) -> Tag tp -> Image tp a
composeMultichannelImage (c1)
                         (c2)
                         (c3)
                         (c4)
                         totag
    = unsafePerformIO $ do
        res <- create (size) -- TODO: Check channel count -- This is NOT correct
        withMaybe c1 $ \cc1 ->
         withMaybe c2 $ \cc2 ->
          withMaybe c3 $ \cc3 ->
           withMaybe c4 $ \cc4 ->
            withGenImage res $ \cres -> {#call cvMerge#} cc1 cc2 cc3 cc4 cres
        return res
    where
        withMaybe (Just i) op = withGenImage i op
        withMaybe (Nothing) op = op nullPtr
        size = getSize . head . catMaybes $ [c1,c2,c3,c4]

-- Load Image as grayscale image.

--loadImage n = do
--              exists <- fileExist n
--              if not exists then return Nothing
--                            else do
--                              i <- withCString n $ \name ->
--                                     creatingImage ({#call cvLoadImage #} name (0))
--                              bw <- imageTo32F i
--                              return $ Just bw

loadImage :: FilePath -> IO (Maybe (Image GrayScale D32))
loadImage n = do
              exists <- fileExist n
              if not exists then return Nothing
                            else do
                              i <- withCString n $ \name ->
                                     creatingBareImage ({#call cvLoadImage #} name (0))
                              bw <- imageTo32F i
                              return . Just . S $ bw

loadColorImage :: FilePath -> IO (Maybe (Image RGB D32))
loadColorImage n = do
              exists <- fileExist n
              if not exists then return Nothing
                            else do
                              i <- withCString n $ \name ->
                                     creatingBareImage ({#call cvLoadImage #} name 1)
                              bw <- imageTo32F i
                              return . Just . S  $ bw

class IntSized a where
    getSize :: a -> (Int,Int)

instance IntSized BareImage where
   -- getSize :: (Integral a, Integral b) => Image c d -> (a,b)
    getSize image = unsafePerformIO $ withBareImage image $ \i -> do
                 w <- {#call getImageWidth#} i
                 h <- {#call getImageHeight#} i
                 return (fromIntegral w,fromIntegral h)

instance IntSized (Image c d) where
    getSize = getSize . unS

cvRGBtoGRAY = 7 :: CInt-- NOTE: This will break.
cvRGBtoLAB = 45 :: CInt-- NOTE: This will break.

rgbToLab :: Image RGB D32 -> Image LAB D32
rgbToLab = S . convertTo cvRGBtoLAB 3 . unS

rgbToGray :: Image RGB D32 -> Image GrayScale D32
rgbToGray = S . convertTo cvRGBtoGRAY 1 . unS

class GetPixel a where
    type P a :: *
    getPixel   :: (Integral i) => (i,i) -> a -> P a

instance GetPixel (Image GrayScale D32) where
    type P (Image GrayScale D32) = D32
    getPixel (fromIntegral -> x, fromIntegral -> y) image = realToFrac $ unsafePerformIO $
             withGenImage image $ \img -> {#call wrapGet32F2D#} img y x

instance  GetPixel (Image RGB D32) where
    type P (Image RGB D32) = (D32,D32,D32)
    getPixel (fromIntegral -> x, fromIntegral -> y) image
        = unsafePerformIO $ do
                     withGenImage image $ \img -> do
                              r <- {#call wrapGet32F2DC#} img y x 0
                              g <- {#call wrapGet32F2DC#} img y x 1
                              b <- {#call wrapGet32F2DC#} img y x 2
                              return (realToFrac r,realToFrac g, realToFrac b)

convertTo :: CInt -> CInt -> BareImage -> BareImage
convertTo code channels img = unsafePerformIO $ creatingBareImage $ do
    res <- {#call wrapCreateImage32F#} w h channels
    withBareImage img $ \cimg ->
        {#call cvCvtColor#} (castPtr cimg) (castPtr res) code
    return res
 where
    (fromIntegral -> w,fromIntegral -> h) = getSize img

class CreateImage a where
    create :: (Int,Int) -> IO a

instance CreateImage (Image GrayScale D32) where
    create (w,h) = creatingImage $ {#call wrapCreateImage32F#} (fromIntegral w) (fromIntegral h) 1
instance CreateImage (Image LAB D32) where
    create (w,h) = creatingImage $ {#call wrapCreateImage32F#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGB D32) where
    create (w,h) = creatingImage $ {#call wrapCreateImage32F#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGBA D32) where
    create (w,h) = creatingImage $ {#call wrapCreateImage32F#} (fromIntegral w) (fromIntegral h) 4

instance CreateImage (Image GrayScale D64) where
    create (w,h) = creatingImage $ {#call wrapCreateImage64F#} (fromIntegral w) (fromIntegral h) 1
instance CreateImage (Image LAB D64) where
    create (w,h) = creatingImage $ {#call wrapCreateImage64F#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGB D64) where
    create (w,h) = creatingImage $ {#call wrapCreateImage64F#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGBA D64) where
    create (w,h) = creatingImage $ {#call wrapCreateImage64F#} (fromIntegral w) (fromIntegral h) 4

instance CreateImage (Image GrayScale D8) where
    create (w,h) = creatingImage $ {#call wrapCreateImage8U#} (fromIntegral w) (fromIntegral h) 1
instance CreateImage (Image LAB D8) where
    create (w,h) = creatingImage $ {#call wrapCreateImage8U#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGB D8) where
    create (w,h) = creatingImage $ {#call wrapCreateImage8U#} (fromIntegral w) (fromIntegral h) 3
instance CreateImage (Image RGBA D8) where
    create (w,h) = creatingImage $ {#call wrapCreateImage8U#} (fromIntegral w) (fromIntegral h) 4

empty :: (CreateImage (Image a b)) => (Int,Int) -> (Image a b)
empty size = unsafePerformIO $ create size

emptyCopy :: (CreateImage (Image a b)) => Image a b -> IO (Image a b)
emptyCopy img = create (getSize img)

emptyCopy' :: (CreateImage (Image a b)) => Image a b -> (Image a b)
emptyCopy' img = unsafePerformIO $ create (getSize img)

-- | Save image. This will convert the image to 8 bit one before saving
saveImage :: FilePath -> Image c d -> IO ()
saveImage filename image = do
                           fpi <- imageTo8Bit $ unS image
                           withCString  filename $ \name  ->
                            withGenBareImage fpi    $ \cvArr ->
							 alloca (\defs -> poke defs 0 >> {#call cvSaveImage #} name cvArr defs >> return ())

getArea :: (IntSized a) => a -> Int
getArea = uncurry (*).getSize

getRegion :: (Int,Int) -> (Int,Int) -> Image c d -> Image c d
getRegion (fromIntegral -> x,fromIntegral -> y) (fromIntegral -> w,fromIntegral -> h) image
    | x+w <= width && y+h <= height = S . getRegion' (x,y) (w,h) $ unS image
    | otherwise                   = error $ "Region outside image:"
                                            ++ show (getSize image) ++
                                            "/"++show (x+w,y+h)
 where
  (fromIntegral -> width,fromIntegral -> height) = getSize image

getRegion' (x,y) (w,h) image = unsafePerformIO $
                               withBareImage image $ \i ->
                                 creatingBareImage ({#call getSubImage#}
                                                i x y w h)

-- | Tile images by overlapping them on a black canvas.
tileImages image1 image2 (x,y) = unsafePerformIO $
                               withImage image1 $ \i1 ->
                                withImage image2 $ \i2 ->
                                 creatingImage ({#call simpleMergeImages#}
                                                i1 i2 x y)
-- | Blit image2 onto image1.
blitFix = blit
blit image1 image2 (x,y)
    | badSizes  = error $ "Bad blit sizes: " ++ show [(w1,h1),(w2,h2)]++"<-"++show (x,y)
    | otherwise = withImage image1 $ \i1 ->
                   withImage image2 $ \i2 ->
                    ({#call plainBlit#} i1 i2 (fromIntegral y) (fromIntegral x))
    where
     ((w1,h1),(w2,h2)) = (getSize image1,getSize image2)
     badSizes = x+w2>w1 || y+h2>h1 || x<0 || y<0

blitM :: (CreateImage (Image c d)) => (Int,Int) -> [((Int,Int),Image c d)] -> Image c d
blitM (rw,rh) imgs = resultPic
    where
     resultPic = unsafePerformIO $ do
                    r <- create (fromIntegral rw,fromIntegral rh)
                    sequence_ [blit r i (fromIntegral x, fromIntegral y)
                              | ((x,y),i) <- imgs ]
                    return r

subPixelBlit
  :: Image c d -> Image c d -> (CDouble, CDouble) -> IO ()

subPixelBlit (image1) (image2) (x,y)
    | badSizes  = error $ "Bad blit sizes: " ++ show [(w1,h1),(w2,h2)]++"<-"++show (x,y)
    | otherwise = withImage image1 $ \i1 ->
                   withImage image2 $ \i2 ->
                    ({#call subpixel_blit#} i1 i2 y x)
    where
     ((w1,h1),(w2,h2)) = (getSize image1,getSize image2)
     badSizes = ceiling x+w2>w1 || ceiling y+h2>h1 || x<0 || y<0

safeBlit i1 i2 (x,y) = unsafePerformIO $ do
                  res <- cloneImage i1-- createImage32F (getSize i1) 1
                  blit res i2 (x,y)
                  return res

-- | Blit image2 onto image1.
--   This uses an alpha channel bitmap for determining the regions where the image should be "blended" with
--   the base image.
blendBlit image1 image1Alpha image2 image2Alpha (x,y) =
                               withImage image1 $ \i1 ->
                                withImage image1Alpha $ \i1a ->
                                 withImage image2Alpha $ \i2a ->
                                  withImage image2 $ \i2 ->
                                   ({#call alphaBlit#} i1 i1a i2 i2a x y)

cloneImage img = withGenImage img $ \image ->
                    creatingImage ({#call cvCloneImage #} image)

withClone img fun = do
                result <- cloneImage img
                fun result
                return result

unsafeImageTo32F img = unsafePerformIO $ withGenImage img $ \image ->
                creatingImage
                 ({#call ensure32F #} image)

unsafeImageTo8Bit img = unsafePerformIO $ withGenImage img $ \image ->
                creatingImage
                 ({#call ensure8U #} image)

imageTo32F img = withGenBareImage img $ \image ->
                creatingBareImage
                 ({#call ensure32F #} image)

imageTo8Bit img = withGenBareImage img $ \image ->
                creatingBareImage
                 ({#call ensure8U #} image)

-- Manipulating regions of interest:
setROI (fromIntegral -> x,fromIntegral -> y)
       (fromIntegral -> w,fromIntegral -> h)
       image = withImage image $ \i ->
                            {#call wrapSetImageROI#} i x y w h

resetROI image = withImage image $ \i ->
                  {#call cvResetImageROI#} i

setCOI chnl image = withImage image $ \i ->
                            {#call cvSetImageCOI#} i (fromIntegral chnl)
resetCOI image = withImage image $ \i ->
                  {#call cvSetImageCOI#} i 0

-- #TODO: Replace the Int below with proper channel identifier
getChannel :: (Enum a) => a -> Image (ChannelOf a) d -> Image GrayScale d
getChannel no image = unsafePerformIO $ creatingImage $ do
    let (w,h) = getSize image
    setCOI (1+fromEnum no) image
    cres <- {#call wrapCreateImage32F#} (fromIntegral w) (fromIntegral h) 1
    withGenImage image $ \cimage ->
      {#call cvCopy#} cimage (castPtr cres) (nullPtr)
    resetCOI image
    return cres

withIOROI pos size image op = do
            setROI pos size image
            x <- op
            resetROI image
            return x

withROI :: (Int, Int) -> (Int, Int) -> Image c d -> (Image c d -> a) -> a
withROI pos size image op = unsafePerformIO $ do
                        setROI pos size image
                        let x = op image -- BUG
                        resetROI image
                        return x

-- Manipulating image pixels
--setPixel :: (CInt,CInt) -> CDouble -> Image c d -> IO ()
--setPixel (x,y) v image = withGenImage image $ \img ->
--                          {#call wrapSet32F2D#} img y x v

getAllPixels image =  [getPixel (i,j) image
                      | i <- [0..width-1 ]
                      , j <- [0..height-1]]
                    where
                     (width,height) = getSize image

getAllPixelsRowMajor image =  [getPixel (i,j) image
                              | j <- [0..height-1]
                              , i <- [0..width-1]
                              ]
                    where
                     (width,height) = getSize image

-- |Create a montage form given images (u,v) determines the layout and space the spacing
--  between images. Images are assumed to be the same size (determined by the first image)
montage :: (CreateImage (Image c d)) => (Int,Int) -> Int -> [Image c d] -> Image c d
montage (u',v') space' imgs = resultPic
    where
     space = fromIntegral space'
     (u,v) = (fromIntegral u', fromIntegral v')
     (rw,rh) = (u*xstep,v*ystep)
     (w,h) = getSize (head imgs)
     (xstep,ystep) = (fromIntegral space + w,fromIntegral space + h)
     edge = space`div`2
     resultPic = unsafePerformIO $ do
                    r <- create (rw,rh)
                    sequence_ [blit r i (edge +  x*xstep, edge + y*ystep)
                               | x <- [0..u-1] , y <- [0..v-1]
                               | i <- imgs ]
                    return r