#! /usr/bin/env morseexec

from morse.builder import *
from scout.builder.robots import Boat
from scout.builder.actuators import Boatactuator

from scout.builder.robots import Scout
from scout.builder.actuators import Scoutactuator

from scout.builder.sensors import Distance
from scout.builder.sensors import Pressure
from scout.builder.sensors import Dvl
from scout.builder.sensors import BoatGps
from scout.builder.sensors import Boatvelocity
from scout.builder.sensors import Posetruth

from scout.builder.actuators import Modememitter


boat = Boat()

BoatActuator = Boatactuator()
BoatActuator.frequency(10.0)
BoatActuator.add_stream('moos','scout.actuators.boatActuatorReader.BoatActuatorReader',moos_port=9000)

Distance1 = Distance()
Distance1.frequency(1.0)
Distance1.add_stream('moos','scout.sensors.distanceNotifier.DistanceNotifier', moos_port=9000)

BoatVelocity = Boatvelocity()
BoatVelocity.frequency(10.0)
BoatVelocity.add_stream('moos', 'scout.sensors.boatvelocityNotifier.BoatVelocityNotifier', moos_port=9000)

BoatGyro = Gyroscope()
BoatGyro.frequency(10.0)
BoatGyro.add_stream('moos', 'scout.sensors.boatGyroNotifier.BoatGyroNotifier', moos_port=9000)

BoatIMU = IMU()
BoatIMU.frequency(10.0)
BoatIMU.add_stream('moos', 'scout.sensors.boatIMUNotifier.BoatIMUNotifier', moos_port=9000)

BoatGPS = BoatGps()
BoatGPS.frequency(10.0)
BoatGPS.add_stream('moos', 'scout.sensors.boatGPSNotifier.BoatGPSNotifier', moos_port=9000)

videocamera = VideoCamera()
videocamera.translate(-10.0, 0.0, 30.0)
videocamera.rotate(0,0,-0.8)


boat.append(BoatActuator)
boat.append(Distance1)
boat.append(BoatGPS)
boat.append(BoatIMU)
boat.append(BoatGyro)
boat.append(BoatVelocity)
boat.append(videocamera)

scout1 = Scout()

ScoutActuator1 = Scoutactuator()
ScoutActuator1.frequency(10.0)
ScoutActuator1.add_stream('moos','scout.actuators.scoutActuatorReader.ScoutActuatorReader',moos_port=9001)


# Pose1 = Posetruth()
# Pose1.frequency(1.0)
# Pose1.add_stream('moos','scout.sensors.poseTruthNotifier.PoseTruthNotifier', moos_port=9001)

Distance2 = Distance()
Distance2.frequency(1.0)
Distance2.add_stream('moos','scout.sensors.distanceNotifier.DistanceNotifier', moos_port=9001)

Pressure1 = Pressure()
Pressure1.frequency(10.0)
Pressure1.add_stream('moos', 'scout.sensors.pressureNotifier.PressureNotifier', moos_port=9001)

Velocity1 = Dvl()
Velocity1.frequency(10.0)
Velocity1.add_stream('moos','scout.sensors.dvlNotifier.DvlNotifier', moos_port=9001)

Gyro1 = Gyroscope()
Gyro1.frequency(10.0)
Gyro1.add_stream('moos', 'scout.sensors.gyroscopeNotifier.GyroscopeNotifier', moos_port=9001)

scout1.append(ScoutActuator1)
scout1.append(Distance2)
scout1.append(Pressure1)
scout1.append(Gyro1)
scout1.append(Velocity1)
#scout1.append(Pose1)

scout2 = Scout()

ScoutActuator2 = Scoutactuator()
ScoutActuator2.frequency(10.0)
ScoutActuator2.add_stream('moos','scout.actuators.scoutActuatorReader.ScoutActuatorReader',moos_port=9002)

Distance3 = Distance()
Distance3.frequency(1.0)
Distance3.add_stream('moos','scout.sensors.distanceNotifier.DistanceNotifier', moos_port=9002)

Pressure2 = Pressure()
Pressure2.frequency(10.0)
Pressure2.add_stream('moos', 'scout.sensors.pressureNotifier.PressureNotifier', moos_port=9002)


Velocity2 = Dvl()
Velocity2.frequency(10.0)
Velocity2.add_stream('moos','scout.sensors.dvlNotifier.DvlNotifier', moos_port=9002)

Gyro2 = Gyroscope()
Gyro2.frequency(10.0)
Gyro2.add_stream('moos', 'scout.sensors.gyroscopeNotifier.GyroscopeNotifier', moos_port=9002)

scout2.append(ScoutActuator2)
scout2.append(Distance3)
scout2.append(Pressure2)
scout2.append(Gyro2)
scout2.append(Velocity2)

scout1.translate(10,-5,0)
scout2.translate(10,5,0)

#################################  MODEM ##########################
# boat = 9000
# scout1 = 9001
# scout2 = 9002
ModemsensorList = [Modememitter() for i in range(3)]

for i in range(3):
    ModemsensorList[i].name = 'ModemSensor'
    ModemsensorList[i].frequency(1.0)

#scout1
ModemsensorList[1].add_stream('moos','scout.actuators.modemActuatorReader.ModemActuatorReader', moos_port=9001)
ModemsensorList[1].add_stream('moos','scout.actuators.modemNotifier.ModemNotifier', moos_port=9002)
scout1.append(ModemsensorList[1])

#scout2
ModemsensorList[2].add_stream('moos','scout.actuators.modemActuatorReader.ModemActuatorReader', moos_port=9002)
ModemsensorList[2].add_stream('moos','scout.actuators.modemNotifier.ModemNotifier', moos_port=9001)
scout2.append(ModemsensorList[2])

#boat
ModemsensorList[0].add_stream('moos','scout.actuators.modemActuatorReader.ModemActuatorReader', moos_port=9000)
ModemsensorList[0].add_stream('moos','scout.actuators.modemNotifier.ModemNotifier', moos_port=9001)
ModemsensorList[0].add_stream('moos','scout.actuators.modemNotifier.ModemNotifier', moos_port=9002)
boat.append(ModemsensorList[0])




# set 'fastmode' to True to switch to wireframe mode
env = Environment('water-1/deep_water', fastmode = False)
env.set_camera_location([-20.0, 0.0, 30.0])
env.set_camera_rotation([0.9, -0., -1.57])

