<commit_before>{-# LANGUAGE DataKinds, GADTs, MultiParamTypeClasses,
             FlexibleInstances, StandaloneDeriving, 
             GeneralizedNewtypeDeriving, FlexibleContexts #-}

-- {-# OPTIONS_GHC -ftype-function-depth=400 #-}
-- {-# OPTIONS_GHC -fcontext-stack=400 #-}

-- | Relevant paper: 
-- <NAME>, <NAME>, and <NAME>. Autonomous navigation and map 
-- building using laser range sensors in outdoor applications. 
-- Journal of Robotic Systems, 17(10):565–583, Oct. 2000.

module Slam where

import Prelude as P
import Control.Monad as CM
import Language.Hakaru.Syntax as H
import Language.Hakaru.Disintegrate
import qualified System.Random.MWC as MWC
import Language.Hakaru.Sample    
import Language.Hakaru.Vector as HV
import Control.Monad.Cont (runCont, cont)
import qualified Data.Sequence as S
import qualified Data.Foldable as F
import Control.Monad.Primitive (PrimState, PrimMonad)

-- Stuff for Data IO
import Text.Printf    
import System.Exit    
import System.Directory
import System.Environment    
import Language.Hakaru.Util.Csv (decodeFileStream)
import Data.Csv
import qualified Control.Applicative as A
import qualified Data.Vector as V
import qualified Data.ByteString.Lazy as B
    
----------
-- Inputs
----------
-- 
-- Inputs per timestamp:
-------------------------
-- 1. v_e : speed (Either this or what the paper calls v_c)
-- 2. alpha: steering angle
-- 3. z_rad_i : distances to object i
-- 4. z_I_i : intensity from objects i
-- 5. z_beta_i : angle to object i
--
-- Starting input (starting state):
------------------------------------
-- 1. GPSLon, GPSLat
-- 2. initial angle (alpha) 
-- 3. dimensions of vehicle (L,h,a,b)
--
--
-----------
-- Outputs
-----------
-- 1. GPSLon, GPSLat
-- 2. phi : world angle
-- 3. (x_i, y_i) : world coords (lon, lat) of each object i in the map
                
calcLon :: (Base repr) => repr DimA -> repr DimB -> repr DimL
        -> repr GPS                 -- ^ old_lon
        -> repr DelTime -> repr Vel -- ^ delT, old_vc
        -> repr Angle -> repr Angle -- ^ old_phi, old_alpha
        -> repr GPS
calcLon dimA dimB dimL old_lon delT old_vc old_phi old_alpha =
    old_lon + delT * (old_vc*(cos old_phi)
                      - (old_vc
                         * (dimA*(sin old_phi) + dimB*(cos old_phi))
                         * (tan old_alpha) / dimL))

calcLat :: (Base repr) => repr DimA -> repr DimB -> repr DimL
        -> repr GPS                 -- ^ old_lat
        -> repr DelTime -> repr Vel -- ^ delT, old_vc
        -> repr Angle -> repr Angle -- ^ old_phi, old_alpha
        -> repr GPS
calcLat dimA dimB dimL old_lat delT old_vc old_phi old_alpha =
    old_lat + delT * (old_vc*(sin old_phi)
                      - (old_vc
                         * (dimA*(cos old_phi) + dimB*(sin old_phi))
                         * (tan old_alpha) / dimL))
    
cVehicle :: (Base repr) => repr Prob
cVehicle = 0.42

cBeacon :: (Base repr) => repr Prob
cBeacon = 0.37

cIntensity :: (Base repr) => repr Prob
cIntensity = 19

muZRads :: (Base repr) => repr H.Real
muZRads = 40

sigmaZRads :: (Base repr) => repr Prob
sigmaZRads = 1

sqr :: (Base repr) => repr H.Real -> repr Prob
sqr a = unsafeProb $ a * a  -- pow_ (unsafeProb a) 2

let_' :: (Mochastic repr)
         => repr a -> (repr a -> repr (Measure b)) -> repr (Measure b)
let_' = bind . dirac

toHList :: (Base repr) => [repr a] -> repr [a]
toHList [] = nil
toHList (a : as) = cons a (toHList as)

map_ :: (Base repr) => (repr a -> repr b) -> repr [a] -> repr [b]
map_ f ls = unlist ls nil k
    where k a as = cons (f a) (map_ f as)

zipWith_ :: (Base repr) => (repr a -> repr b -> repr c)
         -> repr [a] -> repr [b] -> repr [c]
zipWith_ f al bl = unlist al nil k
    where k  a as = unlist bl nil (k' a as)
          k' a as b bs = cons (f a b) (zipWith_ f as bs)

foldl_ :: (Base repr) => (repr b -> repr a -> repr b)
       -> repr b -> repr [a] -> repr b
foldl_ f acc ls = unlist ls acc k
    where k a as = foldl_ f (f acc a) as

sequence' :: (Mochastic repr) => repr [Measure a] -> repr (Measure [a])
sequence' ls = unlist ls (dirac nil) k
    where k ma mas = bind ma $ \a ->
                     bind (sequence' mas) $ \as ->
                     dirac (cons a as)
          
type GPS = H.Real
type ZRad = H.Real -- The observed radial distance to a beacon
type Angle = H.Real -- Radians
type Vel = H.Real    
type DelTime = H.Real
type DimL = H.Real
type DimH = H.Real
type DimA = H.Real
type DimB = H.Real

-- simulate :: (Mochastic repr) => Simulator repr
simulate dimL dimH dimA dimB
         blons blats
         old_lon old_lat old_phi
         old_ve old_alpha delT =

    let_' (old_ve / (1 - (tan old_alpha)*dimH/dimL)) $ \old_vc ->
    let_' (calcLon dimA dimB dimL old_lon delT old_vc old_phi old_alpha) $
              \calc_lon ->
    let_' (calcLat dimA dimB dimL old_lat delT old_vc old_phi old_alpha) $
              \calc_lat ->
    let_' (old_phi + delT*old_vc*(tan old_alpha) / dimL) $ \calc_phi ->
    
    normal calc_lon ((*) cVehicle . sqrt_ . unsafeProb $ delT) `bind` \noisy_lon ->
    normal calc_lat ((*) cVehicle . sqrt_ . unsafeProb $ delT) `bind` \noisy_lat ->
    normal calc_phi ((*) cVehicle . sqrt_ . unsafeProb $ delT) `bind` \noisy_phi ->

    let_' (map_ ((-) noisy_lon) blons) $ \lon_ds ->
    let_' (map_ ((-) noisy_lat) blats) $ \lat_ds ->
        
    let_' (map_ sqrt_ (zipWith_ (+) (map_ sqr lon_ds)
                                    (map_ sqr lat_ds))) $ \calc_zrads ->
    -- inverse-square for intensities 
    let_' (map_ (\r -> cIntensity / (pow_ r 2)) calc_zrads) $ \calc_zints ->
    let_' (map_ (\r -> atan r - calc_phi)  -- removed a "+ pi/2" term
                (zipWith_ (/) lat_ds lon_ds)) $ \calc_zbetas ->

    perturb (\l -> normal (fromProb l) cBeacon) calc_zrads `bind` \noisy_zrads ->
    perturb (\l -> normal (fromProb l) cBeacon) calc_zints `bind` \noisy_zints ->
    perturb (\l -> normal l cBeacon) calc_zbetas `bind` \noisy_zbetas ->

    -- perturbReads (\l -> normal (fromProb l) cBeacon) calc_zrads `bind` \noisy_zrads ->
    -- perturbReads (\l -> normal (fromProb l) cBeacon) calc_zints `bind` \noisy_zints ->
    -- perturbReads (\l -> normal l cBeacon) calc_zbetas `bind` \noisy_zbetas ->

    -- fromNestedPair noisy_zrads $ \zrad_reprs ->
    -- fromNestedPair noisy_zbetas $ \zbeta_reprs ->

    -- perturbReads (\(b,a) -> if (withinLaser a 50) then dirac 5 else normal 40 1)
    --              (HV.fromList $ replicate shortrange (False,6::Double))
    --              `bind` \take11 ->

    -- (extractMeasure $
    --  laserAssigns (normal muZRads sigmaZRads) zrad_reprs noisy_zbetas)
    -- `bind` \laser_zrads ->

    extractMeasure (HV.pure (normal muZRads sigmaZRads)) `bind` \zrad_base ->
    let_' (laserAssigns zrad_base shortrange noisy_zrads noisy_zbetas) $ \zrad_reads ->
    
    -- fromNestedPair zrad_measures $ \base_zrads ->
    -- let_' (toNestedPair (laserAssigns base_zrads zrad_reprs zbeta_reprs)) $
    --       \laser_zrads -> 

    -- perturbReads (const $ normal 40 1) (HV.pure ()) `bind` \rpad ->
    perturbReads (const $ normal 400 1) (HV.pure ()) `bind` \ipad ->

    dirac $ pair (pair zrad_reads ipad)
                 (pair noisy_phi (pair noisy_lon noisy_lat))

testLaser :: IO ()
testLaser = do
  let base :: (Base repr) => repr (Repeat Eleven H.Real)
      -- HV.fromList $ replicate shortrange (10::Double)
      base =  toNestedPair (HV.pure 10)
      reads :: (Base repr) => repr [H.Real]
      reads = cons 30 (cons 40 nil)
      betas :: (Base repr) => repr [H.Real]
      betas = cons 7 (cons 9 nil)
      result :: (Base repr) => repr (Repeat Eleven H.Real)
                -- Repeat Eleven (Sample IO H.Real)
      result = laserAssigns base shortrange reads betas

      sim :: Sample IO (Measure (Repeat Eleven H.Real))
      sim = extractMeasure (HV.pure (normal 0 1)) `bind` \bs ->
            fromNestedPair bs $ \base_reprs ->
            -- dirac $ toNestedPair base_reprs
                
            perturbReads (\r -> normal r 2) (HV.fromList [30,40]) `bind` \rs ->
            fromNestedPair rs $ \read_reprs ->
<commit_msg>Successfully call laserAssign from slam model<commit_after>{-# LANGUAGE DataKinds, GADTs, MultiParamTypeClasses,
             FlexibleInstances, StandaloneDeriving, 
             GeneralizedNewtypeDeriving, FlexibleContexts #-}

-- {-# OPTIONS_GHC -ftype-function-depth=400 #-}
-- {-# OPTIONS_GHC -fcontext-stack=400 #-}

-- | Relevant paper: 
-- <NAME>, <NAME>, and <NAME>. Autonomous navigation and map 
-- building using laser range sensors in outdoor applications. 
-- Journal of Robotic Systems, 17(10):565–583, Oct. 2000.

module Slam where

import Prelude as P
import Control.Monad as CM
import Language.Hakaru.Syntax as H
import Language.Hakaru.Disintegrate
import qualified System.Random.MWC as MWC
import Language.Hakaru.Sample    
import Language.Hakaru.Vector as HV
import Control.Monad.Cont (runCont, cont)
import qualified Data.Sequence as S
import qualified Data.Foldable as F
import Control.Monad.Primitive (PrimState, PrimMonad)

-- Stuff for Data IO
import Text.Printf    
import System.Exit    
import System.Directory
import System.Environment    
import Language.Hakaru.Util.Csv (decodeFileStream)
import Data.Csv
import qualified Control.Applicative as A
import qualified Data.Vector as V
import qualified Data.ByteString.Lazy as B
    
----------
-- Inputs
----------
-- 
-- Inputs per timestamp:
-------------------------
-- 1. v_e : speed (Either this or what the paper calls v_c)
-- 2. alpha: steering angle
-- 3. z_rad_i : distances to object i
-- 4. z_I_i : intensity from objects i
-- 5. z_beta_i : angle to object i
--
-- Starting input (starting state):
------------------------------------
-- 1. GPSLon, GPSLat
-- 2. initial angle (alpha) 
-- 3. dimensions of vehicle (L,h,a,b)
--
--
-----------
-- Outputs
-----------
-- 1. GPSLon, GPSLat
-- 2. phi : world angle
-- 3. (x_i, y_i) : world coords (lon, lat) of each object i in the map
                
calcLon :: (Base repr) => repr DimA -> repr DimB -> repr DimL
        -> repr GPS                 -- ^ old_lon
        -> repr DelTime -> repr Vel -- ^ delT, old_vc
        -> repr Angle -> repr Angle -- ^ old_phi, old_alpha
        -> repr GPS
calcLon dimA dimB dimL old_lon delT old_vc old_phi old_alpha =
    old_lon + delT * (old_vc*(cos old_phi)
                      - (old_vc
                         * (dimA*(sin old_phi) + dimB*(cos old_phi))
                         * (tan old_alpha) / dimL))

calcLat :: (Base repr) => repr DimA -> repr DimB -> repr DimL
        -> repr GPS                 -- ^ old_lat
        -> repr DelTime -> repr Vel -- ^ delT, old_vc
        -> repr Angle -> repr Angle -- ^ old_phi, old_alpha
        -> repr GPS
calcLat dimA dimB dimL old_lat delT old_vc old_phi old_alpha =
    old_lat + delT * (old_vc*(sin old_phi)
                      - (old_vc
                         * (dimA*(cos old_phi) + dimB*(sin old_phi))
                         * (tan old_alpha) / dimL))
    
cVehicle :: (Base repr) => repr Prob
cVehicle = 0.42

cBeacon :: (Base repr) => repr Prob
cBeacon = 0.37

cIntensity :: (Base repr) => repr Prob
cIntensity = 19

muZRads :: (Base repr) => repr H.Real
muZRads = 40

sigmaZRads :: (Base repr) => repr Prob
sigmaZRads = 1

muZInts :: (Base repr) => repr H.Real
muZInts = 40

sigmaZInts :: (Base repr) => repr Prob
sigmaZInts = 1

sqr :: (Base repr) => repr H.Real -> repr Prob
sqr a = unsafeProb $ a * a  -- pow_ (unsafeProb a) 2

let_' :: (Mochastic repr)
         => repr a -> (repr a -> repr (Measure b)) -> repr (Measure b)
let_' = bind . dirac

toHList :: (Base repr) => [repr a] -> repr [a]
toHList [] = nil
toHList (a : as) = cons a (toHList as)

map_ :: (Base repr) => (repr a -> repr b) -> repr [a] -> repr [b]
map_ f ls = unlist ls nil k
    where k a as = cons (f a) (map_ f as)

zipWith_ :: (Base repr) => (repr a -> repr b -> repr c)
         -> repr [a] -> repr [b] -> repr [c]
zipWith_ f al bl = unlist al nil k
    where k  a as = unlist bl nil (k' a as)
          k' a as b bs = cons (f a b) (zipWith_ f as bs)

foldl_ :: (Base repr) => (repr b -> repr a -> repr b)
       -> repr b -> repr [a] -> repr b
foldl_ f acc ls = unlist ls acc k
    where k a as = foldl_ f (f acc a) as

sequence' :: (Mochastic repr) => repr [Measure a] -> repr (Measure [a])
sequence' ls = unlist ls (dirac nil) k
    where k ma mas = bind ma $ \a ->
                     bind (sequence' mas) $ \as ->
                     dirac (cons a as)
          
type GPS = H.Real
type ZRad = H.Real -- The observed radial distance to a beacon
type Angle = H.Real -- Radians
type Vel = H.Real    
type DelTime = H.Real
type DimL = H.Real
type DimH = H.Real
type DimA = H.Real
type DimB = H.Real

-- simulate :: (Mochastic repr) => Simulator repr
simulate dimL dimH dimA dimB
         blons blats
         old_lon old_lat old_phi
         old_ve old_alpha delT =

    let_' (old_ve / (1 - (tan old_alpha)*dimH/dimL)) $ \old_vc ->
    let_' (calcLon dimA dimB dimL old_lon delT old_vc old_phi old_alpha) $
              \calc_lon ->
    let_' (calcLat dimA dimB dimL old_lat delT old_vc old_phi old_alpha) $
              \calc_lat ->
    let_' (old_phi + delT*old_vc*(tan old_alpha) / dimL) $ \calc_phi ->
    
    normal calc_lon ((*) cVehicle . sqrt_ . unsafeProb $ delT) `bind` \noisy_lon ->
    normal calc_lat ((*) cVehicle . sqrt_ . unsafeProb $ delT) `bind` \noisy_lat ->
    normal calc_phi ((*) cVehicle . sqrt_ . unsafeProb $ delT) `bind` \noisy_phi ->

    let_' (map_ ((-) noisy_lon) blons) $ \lon_ds ->
    let_' (map_ ((-) noisy_lat) blats) $ \lat_ds ->
        
    let_' (map_ sqrt_ (zipWith_ (+) (map_ sqr lon_ds)
                                    (map_ sqr lat_ds))) $ \calc_zrads ->
    -- inverse-square for intensities 
    let_' (map_ (\r -> cIntensity / (pow_ r 2)) calc_zrads) $ \calc_zints ->
    let_' (map_ (\r -> atan r - calc_phi)  -- removed a "+ pi/2" term
                (zipWith_ (/) lat_ds lon_ds)) $ \calc_zbetas ->

    perturb (\l -> normal (fromProb l) cBeacon) calc_zrads `bind` \noisy_zrads ->
    perturb (\l -> normal (fromProb l) cBeacon) calc_zints `bind` \noisy_zints ->
    perturb (\l -> normal l cBeacon) calc_zbetas `bind` \noisy_zbetas ->

    -- perturbReads (\l -> normal (fromProb l) cBeacon) calc_zrads `bind` \noisy_zrads ->
    -- perturbReads (\l -> normal (fromProb l) cBeacon) calc_zints `bind` \noisy_zints ->
    -- perturbReads (\l -> normal l cBeacon) calc_zbetas `bind` \noisy_zbetas ->

    -- fromNestedPair noisy_zrads $ \zrad_reprs ->
    -- fromNestedPair noisy_zbetas $ \zbeta_reprs ->

    -- perturbReads (\(b,a) -> if (withinLaser a 50) then dirac 5 else normal 40 1)
    --              (HV.fromList $ replicate shortrange (False,6::Double))
    --              `bind` \take11 ->

    -- (extractMeasure $
    --  laserAssigns (normal muZRads sigmaZRads) zrad_reprs noisy_zbetas)
    -- `bind` \laser_zrads ->

    extractMeasure (HV.pure (normal muZRads sigmaZRads)) `bind` \zrad_base ->
    let_' (laserAssigns zrad_base shortrange noisy_zrads noisy_zbetas) $ \zrad_reads ->

    extractMeasure (HV.pure (normal muZInts sigmaZInts)) `bind` \zint_base ->
    let_' (laserAssigns zint_base shortrange noisy_zints noisy_zbetas) $ \zint_reads ->
        
    -- fromNestedPair zrad_measures $ \base_zrads ->
    -- let_' (toNestedPair (laserAssigns base_zrads zrad_reprs zbeta_reprs)) $
    --       \laser_zrads -> 

    -- perturbReads (const $ normal 40 1) (HV.pure ()) `bind` \rpad ->
    -- perturbReads (const $ normal 400 1) (HV.pure ()) `bind` \ipad ->

    dirac $ pair (pair zrad_reads zrad_reads)
                 (pair noisy_phi (pair noisy_lon noisy_lat))

testLaser :: IO ()
testLaser = do
  let base :: (Base repr) => repr (Repeat Eleven H.Real)
      -- HV.fromList $ replicate shortrange (10::Double)
      base =  toNestedPair (HV.pure 10)
      reads :: (Base repr) => repr [H.Real]
      reads = cons 30 (cons 40 nil)
      betas :: (Base repr) => repr [H.Real]
      betas = cons 7 (cons 9 nil)
      result :: (Base repr) => repr (Repeat Eleven H.Real)
                -- Repeat Eleven (Sample IO H.Real)
      result = laserAssigns base shortrange reads betas

      sim :: Sample IO (Measure (Repeat Eleven H.Real))
      sim = extractMeasure (HV.pure (normal 0 1)) `bind` \bs ->
            fromNestedPair bs $ \base_reprs ->
            -- dirac $ toNestedPair base_reprs
                
            perturbReads (\r -> normal r 2) (HV.fromList [30,40]) `bind` \rs ->
            fromNestedPair rs $ \read_reprs ->

{'fraction_non_alphanumeric': 0.13736495865773432, 'fraction_numerical': 0.009716941275876637, 'mean_word_length': 2.187151375264474, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 209, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7412971', 'n_tokens_mistral': 6190, 'n_tokens_neox': 5697, 'n_words': 1819}