cordic.hs

{-# LANGUAGE TemplateHaskell #-}

module Cordic where
import CLaSH.Prelude as C

-- one step cordic for first quadrant 1 and 4
cordicQ14 :: (NumSFixedC 2 s, NumSFixedC 2 r) 
                     => SFixed 2 s-- fixing angle (constant)
                     -> Int -- shift size
                     -> (SFixed 2 r,SFixed 2 r) -- (x,y) or cos,sin
                     -> SFixed 2 s -- goal angle
                     -> SFixed 2 s -- curent angle
                     -> ((SFixed 2 r,SFixed 2 r),SFixed 2 s) -- returns ((x,y),angle)
cordicQ14 angle shift (x,y) goal beta 
                 | goal >= beta = ((x - sifty, y + siftx ), beta + angle)
                 | otherwise    = ((x + sifty, y - siftx ), beta - angle)
                     where  
                       sifty = y `shiftR` shift 
                       siftx = x `shiftR` shift
                       r = (shift,sifty,siftx)

type Angle a = SFixed 2 a
type XY a = (SFixed 2 a,SFixed 2 a)

-- full circle is 4.0 and 90° is 1.0
angleLookupFlatTH ::  Vec 52 (SFixed 2 52)
angleLookupFlatTH = $(lift $ fmap 
                       (\p -> (fLitR $ (atan(1/(2^p))*(2.0/pi))) :: SFixed 2 52)
                       (iterate d52 (+1) 0))

angleLookupGenerator2TH :: (Num b, KnownNat frac2, KnownNat (2 + frac2),
      (n + n1) ~ 52 , frac2 ~ n ) => SNat n -> Vec n (Fixed Signed 2 frac2, b)
angleLookupGenerator2TH prec = zip (resizeF <$> take prec angleLookupFlatTH) (iterate prec (+1) 0) 

cordicAllSteps prec goal =  postscanl 
                        ( cordicScanL goal) 
                        ((1,0) ,0) 
                        ((angleLookupGenerator2TH prec) ) 
                          where
                            cordicScanL :: (NumSFixedC 2 a, (a + a') ~ 52) 
                                         => Angle a 
                                         -> (XY a, Angle a)
                                         -> (Angle a,Int) 
                                         -> (XY a, Angle a)
                            cordicScanL goal (xy,ang) (fixang,shift) = cordicQ14 fixang shift xy goal ang

cordic prec angle 
            | angle >= 1.0 = ((0-y',x'),a'+1)
            | angle < -1.0 = ((y'',0-x''),a''-1)
            | otherwise = crd angle
               where 
                crd = last . cordicAllSteps prec
                ((x',y'),a') = crd (angle - 1)
                ((x'',y''),a'') = crd (angle + 1)

-- 16 bit precision cordic
topEntity = cordic d16

main = print $ cordic d16 0.5