EventHub:
    type: EventHub

# Queues events from background threads so they can be executed in python
QueuedEventHub:
    depends_on: ["EventHub"]
    type: QueuedEventHub

NetworkPublisher:
    depends_on: ["QueuedEventHub"]
    type: NetworkPublisher
    update_interval: 1

# Exectures Motions
MotionManager:
    type: MotionManager
    depends_on: ["Vehicle", "Controller", "QueuedEventHub", "EventHub", "StateEstimator"]

# Handles Timers
TimerManager:
    type: TimerManager
    depends_on: ["EventHub"]

# Object that creates the vehicle interface
Vehicle:
    # The full name of the class to instantiate in this case its a wrapper
    # for the base C++ vehicle
    type: Vehicle
    # Time between updates in ms
    update_interval: 25
    priority: high

    depends_on: ["EventHub", "Logging"]

    MagBoomName: MagBoom

    TopThrusterThrottle: 0.6

    # The list of devices to create for the vehicle
    Devices:
        # NOTE: All current numbers here are BS and need to updated
        #VelocitySensor:
        #    depends_on: ["SensorBoard"]
        #    type: VisionVelocitySensor
        SensorBoard:
            type: SensorBoard
            update_interval: 40
            priority: high
            depthCalibSlope: 35.5
            depthCalibIntercept: 1182
            # Position is for Tortuga 2 measured in inches as (-11, 1.5, 3)
            # Position for Tortuga 4 measured in cm as (-44.0, 10.0, 5.3)
            depthSensorLocation: [-0.916666667, 0.125, 0.25]
            servo1FirePosition: 4000 # Range 256-4352
            servo2FirePosition: 256 # Range 256-4352
        MarkerDropper:
            type: SBMarkerDropper
            depends_on: ["SensorBoard"]
        TorpedoLauncher:
            type: SBTorpedoLauncher
            depends_on: ['SensorBoard']

            servo1FirePosition: 400
            servo2FirePosition: 3500
            servo3FirePosition: 3500
            servo4FirePosition: 400
        Grabber:
            type: SBGrabber
            depends_on: ["SensorBoard"]

        Sonar:
            depends_on: ['SensorBoard']
            type: SBSonar
        Battery1:
            depends_on: ['SensorBoard']
            type: SBPowerSource
            id: 0

        Battery2:
            depends_on: ['SensorBoard']
            type: SBPowerSource
            id: 1

        Battery3:
            depends_on: ['SensorBoard']
            type: SBPowerSource
            id: 2

        Battery4:
            depends_on: ['SensorBoard']
            type: SBPowerSource
            id: 3

        Shore:
            depends_on: ['SensorBoard']
            type: SBPowerSource
            id: 4

        Temp1:
            depends_on: ['SensorBoard']
            type: SBTempSensor
            id: 0

        Temp2:
            depends_on: ['SensorBoard']
            type: SBTempSensor
            id: 5

        Temp3:
            depends_on: ['SensorBoard']
            type: SBTempSensor
            id: 6

        # This entry creates a thruster
        StarboardThruster:
            depends_on: ['SensorBoard']
            # serial: 147
            type: SBThruster

            # String representing the address of the thruster
            address: 1
            # Number determined by testing
            calibration_factor: 25
            # Number used to flip thruster forces (if thruster is reversed    
            # on the vehicle)
            direction: 1
            # Vector in body coordinate system from center of mass
            location: [0.00, 0.20, 0.00]

        PortThruster:
            depends_on: ['SensorBoard']
            # serial: 149
            type: SBThruster
            address: 0
            calibration_factor: 25
            direction: 1
            location: [0.00, -0.20, 0.00]

        ForeThruster:
            depends_on: ['SensorBoard']
            type: SBThruster
            # serial: 146
            address: 3
            calibration_factor: 25
            direction: -1
            location: [0.4, 0.00, 0.00]

        AftThruster:
            depends_on: ['SensorBoard']
            # Serial 148
            type: SBThruster
            address: 5
            calibration_factor: 25
            direction: -1
            location: [-0.40, 0.00, 0.00]

        TopThruster:
            depends_on: ['SensorBoard']
            # serial: 149
            type: SBThruster
            address: 2
            calibration_factor: 25
            direction: 1
            location: [0.09, -0.0, -0.13]

        BottomThruster:
            depends_on: ['SensorBoard']
            type: SBThruster
            address: 4
            calibration_factor: 25
            direction: -1
            location: [0.09, 0.0, 0.10]
            
        MagBoom:
            type: IMU
            num: 1
            devfile: /dev/magboom
            update_interval: 5
            priority: high

            #from 2011-4-2011
            imuToVehicleRotMatrix: [[ -0.2991, -0.9530, -0.0479 ],
                                    [ -0.9541, 0.2996, -0.0020 ],
                                    [ 0.0162, 0.0451, -0.9989 ]]

            magXBias: -0.026977
            magYBias: 0.019687
            magZBias: -0.064661

            gyroXBias: -0.006018
            gyroYBias: 0.004329
            gyroZBias: 0.004822

            accelXBias: -0.003667
            accelYBias: -0.019106
            accelZBias: -0.021700

            localMagneticPitch: 60

        IMU:
            type: IMU
            num: 0
            update_interval: 5
            priority: high
            # These are in VEHICLE FRAME
            imuToVehicleRotMatrix: [[ -0.6234, 0.0290, 0.7814 ],
                                    [ 0.7818, 0.0102, 0.6234 ],
                                    [ 0.0101, 0.9995, -0.0291 ]]

            magXBias: -0.130466
            magYBias: -0.491657
            magZBias: 0.045481

            gyroXBias: 0.022581
            gyroYBias: -0.027298
            gyroZBias: -0.036842

            accelXBias: -0.019185
            accelYBias: -0.029437
            accelZBias: 0.028014

            # Set me to zero to ignore mag.
            #magCorruptThresh: 0.0
            localMagneticPitch: 60

        DVL:
            type: DVL
            update_interval: 50
            depends_on: []
            devfile: /dev/dvl

RemoteController:
    type: RemoteController
    depends_on: ["QueuedEventHub", "Vehicle", "StateEstimator", "Controller"]
    yawGain: 30
    speedGain: 2
    tSpeedGain: 2

NetworkController:
    type: NetworkController
    depends_on: ["EventHub"]
    update_interval: 10

Logging:
    type: Logging
    #update_interval: -1
    Appenders:
        ThrusterLog:
            type: File
            fileName: thrusters.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        PowerLog:
            type: File
            fileName: power.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        ControlLog:
            type: File
            fileName: control.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        IMULog:
            type: File
            fileName: imu.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        DVLLog:
            type: File
            fileName: dvl.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        VehicleLog:
            type: File
            fileName: vehicle.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        ThrusterSigLog:
            type: File
            fileName: thrusterSig.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        StEstDVLLog:
            type: File
            filename: stEstDVL.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        StEstDepthLog:
            type: File
            filename: stEstDepth.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        StEstIMULog:
            type: File
            filename: stEstIMU.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        BuoyDetectorLog:
            type: File
            filename: buoyDetector.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        LoversLaneDetectorLog:
            type: File
            filename: loversLaneDetector.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        CupidDetectorLog:
            type: File
            filename: cupidDetector.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        BinsDetectorLog:
            type: File
            filename: binsDetector.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        PipeDetectorLog:
            type: File
            filename: pipeDetector.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        CameraLog:
            type: File
            fileName: camera.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        Console:
            type: Console
            Layout:
                type: Pattern
                pattern: "%c %m %r%n"

    Categories:
        Thruster:
            appenders: ['ThrusterLog'] #,'Console']
        ThrusterSig:
            appenders: ['ThrusterSigLog']
        Power:
            appenders: ['PowerLog'] #,'Console']
        Controller:
            appenders: ['ControlLog'] #'Console']
        IMU:
            appenders: ['IMULog'] #'Console']
        DVL:
            appenders: ['DVLLog']
        StEstDVL:
            appenders: ['StEstDVLLog']
        StEstDepth:
            appenders: ['StEstDepthLog']
        StEstIMU:
            appenders: ['StEstIMULog']
        BuoyDetector:
            appenders: ['BuoyDetectorLog']
        LoversLaneDetector:
            appenders: ['LoversLaneDetectorLog']
        BinsDetector:
            appenders: ['BinsDetectorLog']
        CupidDetector:
            appenders: ['CupidDetectorLog']
        PipeDetector:
            appenders: ['PipeDetectorLog']
        Vehicle:
            appenders: ['VehicleLog']
        Camera:
            appenders: ['CameraLog', 'Console']
            priority: 'info'

StateEstimator:
    type: ModularStateEstimator
    depends_on: ["EventHub", "Vehicle"]
    DepthEstimationModule:
        degree: 3
        windowSize: 81
    # GreenBuoy:
    #     location: [0, 0, 8]
    #     covariance: [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
    #     attackHeading: 0
    # GreenBuoyEstimationModule:
    #     numParticles: 200
    # YellowBuoy:
    #     location: [0, 0, 8]
    #     covariance: [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
    #     attackHeading: 0
    # YellowBuoyEstimationModule:
    #     numParticles: 200
    RedBuoy:
        location: [0, 0, 8]
        covariance: [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
        attackHeading: 0
    RedBuoyEstimationModule:
        numParticles: 200

Controller:
    depends_on: ["Vehicle", "EventHub", "StateEstimator"]
    type: CombineController
    update_interval: 25

    # wait for this amout of time before doing control
    initializationPause: 2

    # hold our initial depth and heading
    holdCurrentHeading: 1
    holdCurrentDepth: 1

    velocityThreshold: 0.3
    positionThreshold: 0.3
    depthThreshold: 0.2
    orientationThreshold: 0.3

    TranslationalController:
        type: TrackingTranslationalController
        x1kp: 25
        x1ki: 1
        x1kd: 20
        x2kp: 25
        x2ki: 1
        x2kd: 20

        # type: OpenLoopTranslationalController
        # speedPGain: 8
        # sidewaysSpeedPGain: 8

    DepthController:
        type: PIDDepthController
        kp: 20
        ki: 1
        kd: 3
        dtMin: 0.005
        dtMax: 0.1

    RotationalController:
        type: NonlinearPDRotationalController
        kp: 22.5
        kd: 2
        dtMin: 0.005
        dtMax: 0.1
        desiredOrientation: [0, 0, 0, 1]

#Monitor:
    #type: Monitor
    #depends_on: ["QueuedEventHub"]

    #Battery1:
    #    name: "Battery 1"
    #    publisher: "Vehicle.Device.Batt 1"
    #    eventType: ext.vehicle.device.IVoltageProvider.UPDATE
    #    property: number
    #    warning: 26.7
    #    critical: 26

    #Battery2:
    #    name: "Battery 2"
    #    publisher: "Vehicle.Device.Batt 2"
    #    eventType: ext.vehicle.device.IVoltageProvider.UPDATE
    #    property: number
    #    warning: 26.7
    #    critical: 26

    #Battery3:
    #    name: "Battery 3"
    #    publisher: "Vehicle.Device.Batt 3"
    #    eventType: ext.vehicle.device.IVoltageProvider.UPDATE
    #    property: number
    #    warning: 26.7
    #    critical: 26

    #Battery4:
    #    name: "Battery 4"
    #    publisher: "Vehicle.Device.Batt 4"
    #    eventType: ext.vehicle.device.IVoltageProvider.UPDATE
    #    property: number
    #    warning: 26.7
    #    critical: 26

    #Battery5:
    #    name: "Battery 5"
    #    publisher: "Vehicle.Device.Batt 5"
    #    eventType: ext.vehicle.device.IVoltageProvider.UPDATE
    #    property: number
    #    warning: 26.7
    #    critical: 26