module SDR.Filter (
Filter(..),
Decimator(..),
Resampler(..),
haskellFilter,
fastFilterR,
fastFilterC,
fastSymmetricFilterR,
haskellDecimator,
fastDecimatorR,
fastDecimatorC,
fastSymmetricDecimatorR,
haskellResampler,
--fastResampler,
firFilter,
firDecimator,
firResampler,
dcBlockingFilter
) where
import Data.Complex
import Control.Exception hiding (assert)
import qualified Data.Vector.Generic as VG
import qualified Data.Vector.Generic.Mutable as VGM
import qualified Data.Vector.Storable as VS
import Control.Monad.Primitive
import Pipes
import SDR.Util
import SDR.FilterInternal
data Filter m v vm a = Filter {
numCoeffsF :: Int,
filterOne :: Int -> v a -> vm (PrimState m) a -> m (),
filterCross :: Int -> v a -> v a -> vm (PrimState m) a -> m ()
}
data Decimator m v vm a = Decimator {
numCoeffsD :: Int,
decimationD :: Int,
decimateOne :: Int -> v a -> vm (PrimState m) a -> m (),
decimateCross :: Int -> v a -> v a -> vm (PrimState m) a -> m ()
}
data Resampler m v vm a = Resampler {
numCoeffsR :: Int,
decimationR :: Int,
interpolationR :: Int,
resampleOne :: Int -> Int -> v a -> vm (PrimState m) a -> m Int,
resampleCross :: Int -> Int -> v a -> v a -> vm (PrimState m) a -> m Int
}
duplicate :: [a] -> [a]
duplicate = concat . map func
where func x = [x, x]
haskellFilter :: (PrimMonad m, Functor m, Num a, Mult a b, VG.Vector v a, VG.Vector v b, VGM.MVector vm a)
=> [b]
-> IO (Filter m v vm a)
haskellFilter coeffs = do
let vCoeffs = VG.fromList coeffs
evaluate vCoeffs
let filterOne = filterHighLevel vCoeffs
filterCross = filterCrossHighLevel vCoeffs
numCoeffsF = length coeffs
return $ Filter {..}
fastFilterR :: [Float]
-> IO (Filter IO VS.Vector VS.MVector Float)
fastFilterR coeffs = do
let l = length coeffs
ru = (l + 8 1) `quot` 8
numCoeffsF = ru * 8
diff = numCoeffsF l
vCoeffs = VG.fromList $ coeffs ++ replicate diff 0
evaluate vCoeffs
let filterOne = filterCAVXRR vCoeffs
filterCross = filterCrossHighLevel vCoeffs
return $ Filter {..}
fastFilterC :: [Float]
-> IO (Filter IO VS.Vector VS.MVector (Complex Float))
fastFilterC coeffs = do
let l = length coeffs
ru = (l + 4 1) `quot` 4
numCoeffsF = ru * 4
diff = numCoeffsF l
vCoeffs = VG.fromList $ duplicate $ coeffs ++ replicate diff 0
vCoeffs2 = VG.fromList $ coeffs ++ replicate diff 0
evaluate vCoeffs
let filterOne = filterCAVXRC vCoeffs
filterCross = filterCrossHighLevel vCoeffs2
return $ Filter {..}
fastSymmetricFilterR :: [Float]
-> IO (Filter IO VS.Vector VS.MVector Float)
fastSymmetricFilterR coeffs = do
let vCoeffs = VG.fromList coeffs
let vCoeffs2 = VG.fromList $ coeffs ++ reverse coeffs
evaluate vCoeffs
evaluate vCoeffs2
let filterOne = filterCAVXSymmetricRR vCoeffs
filterCross = filterCrossHighLevel vCoeffs2
numCoeffsF = length coeffs * 2
return $ Filter {..}
haskellDecimator :: (PrimMonad m, Functor m, Num a, Mult a b, VG.Vector v a, VG.Vector v b, VGM.MVector vm a)
=> Int
-> [b]
-> IO (Decimator m v vm a)
haskellDecimator decimationD coeffs = do
let vCoeffs = VG.fromList coeffs
evaluate vCoeffs
let decimateOne = decimateHighLevel decimationD vCoeffs
decimateCross = decimateCrossHighLevel decimationD vCoeffs
numCoeffsD = length coeffs
return $ Decimator {..}
fastDecimatorR :: Int
-> [Float]
-> IO (Decimator IO VS.Vector VS.MVector Float)
fastDecimatorR decimationD coeffs = do
let l = length coeffs
ru = (l + 8 1) `quot` 8
numCoeffsD = ru * 8
diff = numCoeffsD l
vCoeffs = VG.fromList $ coeffs ++ replicate diff 0
evaluate vCoeffs
let decimateOne = decimateCAVXRR decimationD vCoeffs
decimateCross = decimateCrossHighLevel decimationD vCoeffs
return $ Decimator {..}
fastDecimatorC :: Int
-> [Float]
-> IO (Decimator IO VS.Vector VS.MVector (Complex Float))
fastDecimatorC decimationD coeffs = do
let l = length coeffs
ru = (l + 4 1) `quot` 4
numCoeffsD = ru * 4
diff = numCoeffsD l
vCoeffs = VG.fromList $ duplicate $ coeffs ++ replicate diff 0
vCoeffs2 = VG.fromList $ coeffs ++ replicate diff 0
evaluate vCoeffs
let decimateOne = decimateCAVXRC decimationD vCoeffs
decimateCross = decimateCrossHighLevel decimationD vCoeffs2
return $ Decimator {..}
fastSymmetricDecimatorR :: Int
-> [Float]
-> IO (Decimator IO VS.Vector VS.MVector Float)
fastSymmetricDecimatorR decimationD coeffs = do
let vCoeffs = VG.fromList coeffs
let vCoeffs2 = VG.fromList $ coeffs ++ reverse coeffs
evaluate vCoeffs
evaluate vCoeffs2
let decimateOne = decimateCAVXSymmetricRR decimationD vCoeffs
decimateCross = decimateCrossHighLevel decimationD vCoeffs2
numCoeffsD = length coeffs * 2
return $ Decimator {..}
haskellResampler :: (PrimMonad m, Functor m, Num a, Mult a b, VG.Vector v a, VG.Vector v b, VGM.MVector vm a)
=> Int
-> Int
-> [b]
-> IO (Resampler m v vm a)
haskellResampler interpolationR decimationR coeffs = do
let vCoeffs = VG.fromList coeffs
evaluate vCoeffs
let resampleOne = resampleHighLevel interpolationR decimationR vCoeffs
resampleCross = resampleCrossHighLevel interpolationR decimationR vCoeffs
numCoeffsR = length coeffs
return $ Resampler {..}
data Buffer v a = Buffer {
buffer :: v a,
offset :: Int
}
space Buffer{..} = VGM.length buffer offset
newBuffer :: (PrimMonad m, VGM.MVector vm a) => Int -> m (Buffer (vm (PrimState m)) a)
newBuffer size = do
buf <- VGM.new size
return $ Buffer buf 0
advanceOutBuf :: (PrimMonad m, VG.Vector v a) => Int -> Buffer (VG.Mutable v (PrimState m)) a -> Int -> Pipe b (v a) m (Buffer (VG.Mutable v (PrimState m)) a)
advanceOutBuf blockSizeOut buf@(Buffer bufOut offsetOut) count =
if count == space buf then do
bufOutF <- lift $ VG.unsafeFreeze bufOut
yield bufOutF
lift $ newBuffer blockSizeOut
else
return $ Buffer bufOut (offsetOut + count)
assert loc False = error loc
assert loc True = return ()
--Filtering
firFilter :: (PrimMonad m, Functor m, VG.Vector v a, Num a)
=> Filter m v (VG.Mutable v) a
-> Int
-> Pipe (v a) (v a) m ()
firFilter Filter{..} blockSizeOut = do
inBuf <- await
outBuf <- lift $ newBuffer blockSizeOut
simple inBuf outBuf
where
simple bufIn bufferOut@(Buffer bufOut offsetOut) = do
assert "filter 1" (VG.length bufIn >= numCoeffsF)
let count = min (VG.length bufIn numCoeffsF + 1) (space bufferOut)
lift $ filterOne count bufIn (VGM.unsafeDrop offsetOut bufOut)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
let bufIn' = VG.drop count bufIn
case VG.length bufIn' < numCoeffsF of
False -> simple bufIn' bufferOut'
True -> do
next <- await
crossover bufIn' next bufferOut'
crossover bufLast bufNext bufferOut@(Buffer bufOut offsetOut) = do
assert "filter 2" (VG.length bufLast < numCoeffsF)
assert "filter 3" (VG.length bufLast > 0)
let count = min (VG.length bufLast) (space bufferOut)
lift $ filterCross count bufLast bufNext (VGM.unsafeDrop offsetOut bufOut)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
case VG.length bufLast == count of
True -> simple bufNext bufferOut'
False -> crossover (VG.drop count bufLast) bufNext bufferOut'
--Decimation
firDecimator :: (PrimMonad m, Functor m, VG.Vector v a, Num a)
=> Decimator m v (VG.Mutable v) a
-> Int
-> Pipe (v a) (v a) m ()
firDecimator Decimator{..} blockSizeOut = do
inBuf <- await
outBuf <- lift $ newBuffer blockSizeOut
simple inBuf outBuf
where
simple bufIn bufferOut@(Buffer bufOut offsetOut) = do
assert "decimate 1" (VG.length bufIn >= numCoeffsD)
let count = min (((VG.length bufIn numCoeffsD) `quot` decimationD) + 1) (space bufferOut)
lift $ decimateOne count bufIn (VGM.unsafeDrop offsetOut bufOut)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
let bufIn' = VG.drop (count * decimationD) bufIn
case VG.length bufIn' < numCoeffsD of
False -> simple bufIn' bufferOut'
True -> do
next <- await
crossover bufIn' next bufferOut'
crossover bufLast bufNext bufferOut@(Buffer bufOut offsetOut) = do
assert "decimate 2" (VG.length bufLast < numCoeffsD)
assert "decimate 3" (VG.length bufLast > 0)
let count = min (VG.length bufLast `quotUp` decimationD) (space bufferOut)
lift $ decimateCross count bufLast bufNext (VGM.unsafeDrop offsetOut bufOut)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
case VG.length bufLast <= count * decimationD of
True -> simple (VG.drop (count * decimationD VG.length bufLast) bufNext) bufferOut'
False -> crossover (VG.drop (count * decimationD) bufLast) bufNext bufferOut'
quotUp q d = (q + (d 1)) `quot` d
firResampler :: (PrimMonad m, VG.Vector v a, Num a)
=> Resampler m v (VG.Mutable v) a
-> Int
-> Pipe (v a) (v a) m ()
firResampler Resampler{..} blockSizeOut = do
inBuf <- await
outBuf <- lift $ newBuffer blockSizeOut
simple inBuf outBuf 0
where
simple bufIn bufferOut@(Buffer bufOut offsetOut) filterOffset = do
assert "resample 1" (VG.length bufIn * interpolationR >= numCoeffsR filterOffset)
let count = min (((VG.length bufIn * interpolationR numCoeffsR + filterOffset) `quot` decimationR) + 1) (space bufferOut)
endOffset <- lift $ resampleOne filterOffset count bufIn (VGM.unsafeDrop offsetOut bufOut)
assert "resample 2" ((count * decimationR + endOffset filterOffset) `rem` interpolationR == 0)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
let usedInput = (count * decimationR filterOffset) `quotUp` interpolationR
bufIn' = VG.drop usedInput bufIn
case VG.length bufIn' * interpolationR < numCoeffsR endOffset of
False -> simple bufIn' bufferOut' endOffset
True -> do
next <- await
case VG.length bufIn' == 0 of
True -> simple next bufferOut' endOffset
False -> crossover bufIn' next bufferOut' endOffset
crossover bufLast bufNext bufferOut@(Buffer bufOut offsetOut) filterOffset = do
assert "resample 3" (VG.length bufLast * interpolationR < numCoeffsR filterOffset)
let outputsComputable = (VG.length bufLast * interpolationR + filterOffset) `quotUp` decimationR
count = min outputsComputable (space bufferOut)
assert "resample 4" (count /= 0)
endOffset <- lift $ resampleCross filterOffset count bufLast bufNext (VGM.unsafeDrop offsetOut bufOut)
assert "resample 5" ((count * decimationR + endOffset filterOffset) `rem` interpolationR == 0)
bufferOut' <- advanceOutBuf blockSizeOut bufferOut count
let inputUsed = (count * decimationR filterOffset) `quotUp` interpolationR
case inputUsed >= VG.length bufLast of
True -> simple (VG.drop (inputUsed VG.length bufLast) bufNext) bufferOut' endOffset
False -> crossover (VG.drop inputUsed bufLast) bufNext bufferOut' endOffset
dcBlockingFilter :: Pipe (VS.Vector Float) (VS.Vector Float) IO ()
dcBlockingFilter = func 0 0
where
func lastSample lastOutput = do
dat <- await
out <- lift $ VGM.new (VG.length dat)
(lastSample, lastOutput) <- lift $ dcBlocker (VG.length dat) lastSample lastOutput dat out
outF <- lift $ VG.unsafeFreeze out
yield outF
func lastSample lastOutput