{-# OPTIONS -fglasgow-exts #-}
module Message where
import CAN (CanMessage(..))
import ByteParser
import qualified Data.Map as Map
import Data.Generics
import Data.Bits
import Data.Word

data Message = ImuNodeCommand Word8
             | ImuReportError {imuErrorNumber :: Word8, imuErrorSequenceNumber :: Word8, imuErrorCount :: Word8}
             | ImuEnableAccel Bool
             | ImuAccelData {imuAccelDataX :: Word16, imuAccelDataY :: Word16, imuAccelDataZ :: Word16, imuAccelDataQ :: Word16}
             | ImuEnableGyro Bool
             | ImuGyroData {imuGyroDataPitch :: Word16, imuGyroDataYaw :: Word16, imuGyroDataRoll :: Word16}
             | GpsUartTransmit [Word8]
             | FcGpsTime {fcGpsTimeDay :: Word8, fcGpsTimeMonth :: Word8, fcGpsTimeYear :: Word16, fcGpsTimeHour :: Word8, fcGpsTimeMinute :: Word8, fcGpsTimeSecond :: Word8}
             | FcReportState Word8
             | FcForceState Word8
             | FcRequestState Word8
             | FcAbortLaunch
             | FcReportNodeStatus Word64
             | FcReportLinkQuality Word16 Word16
             | FcReportImportanceMask Word64
             | FcPingMessage Word64
             | FcReportLogAvail Word32
             | UnknownMessage CanMessage
             | InvalidMessage CanMessage String String
  deriving (Show, Typeable, Data)

{-#define FC_IMU_HEIGHT (FC_IMU_NID|CID_NOUN(0)|CID_DLC_4)
#define FC_IMU_BASE   (FC_IMU_NID|CID_NOUN(1)|CID_DLC_8)
#define FC_GPS_NAVSOL (FC_GPS_NID|CID_NOUN(0)|CID_DLC_4)
#define FC_GPS_LATLON (FC_GPS_NID|CID_NOUN(2)|CID_DLC_8)
#define FC_GPS_HEIGHT (FC_GPS_NID|CID_NOUN(3)|CID_DLC_4)
#define FC_GPS_SATS_VIS  (FC_GPS_NID|CID_NOUN(4)|CID_DLC_1)
#define FC_GPS_SATS_USED (FC_GPS_NID|CID_NOUN(5)|CID_DLC_1)
-}




ids = [0x180, 0x192, 0x1b9, 0x1b8, 0x1bb, 0x1ba, 0x530, 0xf82, 0x12, 0x22, 0x2, 0x4, 0x16, 0x18, 0x1a, 0x1c, 0x1e ]

constrMap = Map.fromList $ zip ids $ dataTypeConstrs $ dataTypeOf (undefined :: Message)

mkMessage' m id body = case Map.lookup id constrMap of
  Just c -> case parseBuffer (fromConstrM parseAny c) body of
    Right m' -> m'
    Left s -> InvalidMessage m (show c) s
  Nothing -> UnknownMessage m

mkMessage :: CanMessage -> Message
mkMessage m@(NoRTR id body) = mkMessage' m (id `shiftL` 1) body
mkMessage m@(RTR id len) = mkMessage' m ((id `shiftL` 1) .|. 1) [fromIntegral len]