/*
* ----------------------------------------------------------------------------
* (C) Copyright Testing Technologies, 2001-2016. All Rights Reserved.
*
* All copies of this program, whether in whole or in part, and whether
* modified or not, must display this and all other embedded copyright
* and ownership notices in full.
*
* See the file COPYRIGHT for details of redistribution and use.
*
* You should have received a copy of the COPYRIGHT file along with
* this file; if not, write to the Testing Technologies,
* Michaelkirchstr. 17/18, 10179 Berlin, Germany.
*
* TESTING TECHNOLOGIES DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS
* SOFTWARE. IN NO EVENT SHALL TESTING TECHNOLOGIES BE LIABLE FOR ANY
* SPECIAL, DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN
* AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION,
* ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
* THIS SOFTWARE.
*
* THE SOFTWARE IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING ANY KIND OF IMPLIED OR
* EXPRESSED WARRANTY OF NON-INFRINGEMENT OR THE IMPLIED WARRANTIES
* OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE.
*
* ----------------------------------------------------------------------------- */
package org.etsi.its.tool.testingtech;
import java.io.ByteArrayInputStream;
import java.io.IOException;
import java.math.BigInteger;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.Socket;
import java.net.SocketException;
import java.net.SocketTimeoutException;
import java.text.DateFormat;
import java.text.ParseException;
import java.text.SimpleDateFormat;
import java.util.Date;
import javax.xml.parsers.ParserConfigurationException;
import javax.xml.parsers.SAXParser;
import javax.xml.parsers.SAXParserFactory;
import org.etsi.adapter.IGnssSupport;
import org.etsi.adapter.ITERequired;
import org.etsi.adapter.TERFactory;
import org.etsi.common.ByteHelper;
import org.etsi.its.adapter.ports.AdapterControlPort;
import org.etsi.its.adapter.ports.AdapterPort;
import org.etsi.its.adapter.ports.PortEvent;
import org.etsi.ttcn.tci.CharstringValue;
import org.etsi.ttcn.tci.Value;
import org.etsi.ttcn.tri.TriAddress;
import org.etsi.ttcn.tri.TriCommunicationTE;
import org.etsi.ttcn.tri.TriMessage;
import org.etsi.ttcn.tri.TriStatus;
import org.xml.sax.Attributes;
import org.xml.sax.SAXException;
import org.xml.sax.helpers.DefaultHandler;
import com.testingtech.ttcn.tci.codec.tabular.TabularDecoder;
import com.testingtech.ttcn.tci.codec.tabular.TabularException;
import com.testingtech.ttcn.tri.TriStatusImpl;
import com.testingtech.util.BitArrayInputStream;
import com.testingtech.util.BitArrayInputStream.Endianness;
import com.testingtech.util.ReferenceObject;
public class GnssRemoteControl implements IGnssSupport {
private final String STATUS_START = "";
private final String STATUS_END = "";
private final String DATA_START = "";
private final String DATA_END = "";
private Socket soc = null;
private static Boolean receiverRunning = false;
private static GnssRemoteControl gnssRC = null;
private boolean debug = true;
public static GnssRemoteControl getInstance() {
if (gnssRC == null){
gnssRC = new GnssRemoteControl();
}
return gnssRC;
}
public TriStatus init() {
return TriStatusImpl.OK;
}
public static void main(String[] args) {
TERFactory.setImpl(new ITERequired() {
@Override
public void logError(String errorMessage) {
System.out.println(errorMessage);
}
@Override
public void logDebug(String debugMessage) {
System.out.println(debugMessage);
}
@Override
public TriStatus getTriStatus(int statusCode, String message) {
return null;
}
@Override
public TriStatus getTriStatus(int statusCode) {
return null;
}
@Override
public TriMessage getTriMessage(byte[] message) {
return null;
}
@Override
public TriAddress getTriAddress(byte[] message) {
return null;
}
@Override
public Value getTaParameter(String param) {
return null;
}
@Override
public TriCommunicationTE getCommunicationTE() {
return null;
}
});
try {
for (int i = 0; i < 1; i++) {
GnssRemoteControl grc = GnssRemoteControl.getInstance();
grc.showMessage("Now controlled by TTwborkbench");
grc.loadScenario(1500);
// grc.getScenarioName();
// grc.getScenarioStatus();
// grc.getScenarioDuration();
grc.startScenario();
grc.awaitTimeInRunningScenario(new AdapterControlPort("test", "TestComp"), 60000);
// grc.awaitDistanceToCover(new AdapterControlPort("test", "TestComp"), 200.0);
grc.getGpsTime();
// grc.getScenarioStatus();
grc.stopScenario();
// grc.getScenarioStatus();
}
} catch (Throwable th) {
th.printStackTrace();
}
}
public void dispose() {
try {
TERFactory.getInstance().logDebug("+++++++++++++++++++++++++++++++++++++++++++++");
TERFactory.getInstance().logDebug("CLOSING");
TERFactory.getInstance().logDebug("+++++++++++++++++++++++++++++++++++++++++++++");
if (soc!=null) {
soc.close();
soc = null;
}
gnssRC = null;
} catch (IOException e) {
e.printStackTrace();
}
}
private String getScenario(int p_id) throws Exception {
String result;
String scenario;
switch (p_id) {
case 0:
// result = "c:\\Program Files (x86)\\Spirent Communications\\Positioning Application\\Scenarios\\C2C\\StaticPos\\StaticPos.scn";
scenario = "GnssScenario_StaticPos";
break;
case 200:
// result = "C:\\Program Files (x86)\\Spirent Communications\\Positioning Application\\Scenarios\\C2C\\DynamicPos1000m\\DynamicPos200m.scn";
scenario = "GnssScenario_DynamicPos200m";
break;
case 1000:
// result = "C:\\Program Files (x86)\\Spirent Communications\\Positioning Application\\Scenarios\\C2C\\DynamicPos1000m\\DynamicPos1000m.scn";
scenario = "GnssScenario_DynamicPos1000m";
break;
case 1500:
// return "C:\\Program Files (x86)\\Spirent Communications\\Positioning Application\\Scenarios\\C2C\\DynamicPos1500m\\DynamicPos1500m.scn";
scenario = "GnssScenario_DynamicPos1500m";
break;
// case 4:
// return "C:\\Program Files (x86)\\Spirent Communications\\Positioning Application\\Scenarios\\C2C\\DynamicPos1500m_straight\\DynamicPos1500m_straight.scn";
default:
throw new Exception(p_id + ": Unknown scenario identification");
}
try {
result = ((CharstringValue)TERFactory.getInstance().getTaParameter(scenario)).getString();
}
catch (Exception e) {
throw new Exception("Could not retrieve TA parameter " + scenario + ". Check configuration.");
}
return result;
}
public int showMessage(String msg) {
return handleCommand("MESSAGE,TTCN-3,Info,"+msg);
}
public boolean loadScenario(int p_id) {
try {
boolean result= 2 == handleCommand("SC,"+getScenario(p_id));
return result && enableDataStreaming();
} catch (Throwable th) {
th.printStackTrace();
return false;
}
}
public int getScenarioName() {
return handleCommand("SC_NAME");
}
public int getScenarioDuration() {
return handleCommand("SC_DURATION");
}
public int getScenarioStatus() {
return handleCommand("NULL");
}
public boolean enableDataStreaming() {
String address;
try {
try {
address = ((CharstringValue)TERFactory.getInstance().getTaParameter("TestSystemIpAddress")).getString();
}
catch (Throwable th) {
address = "10.73.100.28";
}
}
catch (Throwable th) {
th.printStackTrace();
return false;
}
boolean result = 2==handleCommand("DS_ENABLE,1");
handleCommand("DS_INFO");
if (result) {
result = 2==handleCommand("DS_IP,"+address);
}
if (result) {
result = 2==handleCommand("DS_VEH_MOT,v1,1");
}
if (result) {
result = 2==handleCommand("DS_VEH_CMS,v1,0");
}
if (result) {
result = 2==handleCommand("DS_SYNC,0");
}
if (result) {
result = 2==handleCommand("DS_STATUS,0");
}
if (result) {
result = 2==handleCommand("DS_ANT_MOT,v1_a1,0");
}
return result;
}
public boolean startScenario() {
int result = handleCommand("RU");
if (result==4) {
while((result = getScenarioStatus())!=5){
try {
Thread.sleep(50);
} catch (InterruptedException e) {
//ignore
}
};
}
return result==5;
}
public boolean stopScenario() {
stopReceiver();
return 2==handleCommand("-,EN,1");
}
public boolean awaitDistanceToCover(final AdapterPort commPort, final double distance) {
boolean result = false;
try {
final DatagramSocket udpReceiverSoc = new DatagramSocket(15660);
udpReceiverSoc.setSoTimeout(100);
synchronized (receiverRunning) {
receiverRunning = true;
}
new Thread(new Runnable() {
@Override
public void run() {
boolean result = false;
DatagramPacket packet = new DatagramPacket(new byte[1500], 1500);
Double prevLatitude = null;
Double prevLongitude = null;
Double distanceCovered = 0.0;
while(isReceiverRunning()) {
try {
udpReceiverSoc.receive(packet);
byte[] data = packet.getData();
int length = packet.getLength();
byte[] received = new byte[length];
System.arraycopy(data, 0, received, 0, length);
TabularDecoder dec = new TabularDecoder(new BitArrayInputStream(received));
try {
int type = dec.decodeInteger(false, 32, Endianness.LITTLE_ENDIAN);
if (type==0) {
int version = dec.decodeInteger(false, 32, Endianness.LITTLE_ENDIAN);
int time_into_run = dec.decodeInteger(false, 32, Endianness.LITTLE_ENDIAN);
int time_of_validity = dec.decodeInteger(false, 32, Endianness.LITTLE_ENDIAN);
int vehicleNumber = dec.decodeInteger(false, 32, Endianness.LITTLE_ENDIAN);
//skip spare
dec.decodeOctetstring(4);
//skip posn, velocity, acceleration, jerk
dec.decodeOctetstring(96);
BigInteger tmp = dec.decodeBigInteger(false, 64, Endianness.LITTLE_ENDIAN);
double curLatitude = Double.longBitsToDouble(tmp.longValue());
tmp = dec.decodeBigInteger(false, 64, Endianness.LITTLE_ENDIAN);
double curLongitude = Double.longBitsToDouble(tmp.longValue());
if (debug) {
TERFactory.getInstance().logDebug("Type : " + type);
TERFactory.getInstance().logDebug("Version : " + version);
TERFactory.getInstance().logDebug("Time(into run) : " + time_into_run);
TERFactory.getInstance().logDebug("Time(validity) : " + time_of_validity);
TERFactory.getInstance().logDebug("VehicleNumber : " + vehicleNumber);
TERFactory.getInstance().logDebug("Latitude : " + curLatitude);
TERFactory.getInstance().logDebug("Longitude : " + curLongitude);
}
if (prevLatitude != null && prevLongitude != null) {
distanceCovered += computeDistance(prevLatitude, curLatitude, prevLongitude, curLongitude);
}
prevLatitude = curLatitude;
prevLongitude = curLongitude;
if (debug) {
TERFactory.getInstance().logDebug("Distance: " + distanceCovered);
}
if (distanceCovered>=distance) {
result = true;
stopReceiver();
}
}
} catch (TabularException e) {
e.printStackTrace();
}
} catch (SocketTimeoutException ste) {
//nothing received, ignore
} catch (IOException e) {
e.printStackTrace();
result = false;
stopReceiver();
}
}
udpReceiverSoc.close();
commPort.setChanged();
byte[] response = {(byte)AdapterControlPort.AcGnssDistanceCovered, (byte)(result?AdapterControlPort.AcFalse:AdapterControlPort.AcTrue)};
commPort.notifyObservers(new PortEvent(response, commPort.getPortName(), commPort.getComponentName()));
}
}, "AwaitDistanceCovered").start();
} catch (SocketException e) {
e.printStackTrace();
result = false;
}
return result;
}
public boolean awaitTimeInRunningScenario(final AdapterPort commPort, final int time) {
boolean result = false;
try {
final DatagramSocket udpReceiverSoc = new DatagramSocket(15660);
udpReceiverSoc.setSoTimeout(100);
synchronized (receiverRunning) {
receiverRunning = true;
}
Thread receiver =
new Thread(new Runnable() {
@Override
public void run() {
boolean result = false;
DatagramPacket packet = new DatagramPacket(new byte[1500], 1500);
while(isReceiverRunning()) {
try {
udpReceiverSoc.receive(packet);
TabularDecoder dec = new TabularDecoder(new BitArrayInputStream(packet.getData(), packet.getLength()<<3));
try {
int type = dec.decodeInteger(false, 32, Endianness.LITTLE_ENDIAN);
if (type==0) {
int version = dec.decodeInteger(false, 32, Endianness.LITTLE_ENDIAN);
int time_into_run = dec.decodeInteger(false, 32, Endianness.LITTLE_ENDIAN);
if (debug) {
TERFactory.getInstance().logDebug("Version : " + version);
TERFactory.getInstance().logDebug("Time(into run) : " + time_into_run);
}
if (time_into_run>=(time*1000)/*in ms*/) {
result = true;
stopReceiver();
}
}
} catch (TabularException e) {
e.printStackTrace();
}
} catch (SocketTimeoutException ste) {
//nothing received, ignore
} catch (IOException e) {
e.printStackTrace();
result = false;
stopReceiver();
}
}
udpReceiverSoc.close();
commPort.setChanged();
byte[] response = {(byte)AdapterControlPort.AcGnssTimeReached, (byte)(result?AdapterControlPort.AcFalse:AdapterControlPort.AcTrue)};
commPort.notifyObservers(new PortEvent(response, commPort.getPortName(), commPort.getComponentName()));
}
}, "AwaitTimeIntoRun");
receiver.start();
// try {
// receiver.join();
// } catch (InterruptedException e) {
// e.printStackTrace();
// }
} catch (SocketException e) {
e.printStackTrace();
result = false;
}
return result;
}
public boolean changeSpeed(double speed) {
return false;
}
public boolean changeHeading(double heading) {
return false;
}
public BigInteger getGpsTime() {
boolean result = false;
BigInteger now = null;
long tmpNow = 0;
ReferenceObject data = new ReferenceObject(new Double(0));
result = 5==handleCommand("-,GPS_TIME", data);
if (result) {
if (data.get()!=0.0) {
tmpNow = data.get().longValue();
}
else {
result = false;
}
}
//TODO Check if leap seconds should be handled
if (result) {
String gpsdatestr="01/06/1980 00:00:00 +0000";
String itsdatestr="01/01/2004 00:00:00 +0000";
DateFormat formatter = new SimpleDateFormat("MM/dd/yyyy hh:mm:ss Z");
try {
Date gpsInit = formatter.parse(gpsdatestr);
long time = new Date(tmpNow*1000 + gpsInit.getTime()).getTime();
now = new BigInteger(
1,
ByteHelper.longToByteArray(time - ((java.util.Date)formatter.parse(itsdatestr)).getTime(), Long.SIZE / Byte.SIZE)
);
if (debug) {
TERFactory.getInstance().logDebug("timeGps : " + time);
time = System.currentTimeMillis();
TERFactory.getInstance().logDebug("timeSys : " + time);
TERFactory.getInstance().logDebug("now : " + now);
TERFactory.getInstance().logDebug("ref : " +
new BigInteger(
1,
ByteHelper.longToByteArray(time - ((java.util.Date)formatter.parse(itsdatestr)).getTime(), Long.SIZE / Byte.SIZE)
).longValue()
);
TERFactory.getInstance().logDebug("date : " +
new BigInteger(
1,
ByteHelper.longToByteArray(((java.util.Date)formatter.parse("12/15/2015 00:00:00 +0000")).getTime() - ((java.util.Date)formatter.parse(itsdatestr)).getTime(), Long.SIZE / Byte.SIZE)
).longValue()
);
}
} catch (ParseException e) {
now = BigInteger.ZERO;
}
}
return now;
}
private int handleCommand(String command) {
return handleCommand(command, null);
}
private int handleCommand(String command, ReferenceObject data) {
if (soc==null) {
try {
String address;
try {
address = ((CharstringValue)TERFactory.getInstance().getTaParameter("GnssControllerAddress")).getString();
}
catch (Throwable th) {
address = "10.73.224.145";
}
soc = new Socket(address, 15650);
}
catch (Throwable th) {
th.printStackTrace();
return -1;
}
}
try {
command += "\r";
TERFactory.getInstance().logDebug(new Date().toString());
TERFactory.getInstance().logDebug("+++++++++++++++++++++++++++++++++++++++++++++");
TERFactory.getInstance().logDebug(command);
TERFactory.getInstance().logDebug("+++++++++++++++++++++++++++++++++++++++++++++");
soc.getOutputStream().write(command.getBytes());
soc.getOutputStream().flush();
int read=0;
StringBuffer sb = new StringBuffer();
do {
read = soc.getInputStream().read();
sb.append(new String(new byte[] {(byte) read}));
} while(read!='\r');
String response = sb.toString();
TERFactory.getInstance().logDebug(response);
int statusStart = response.indexOf(STATUS_START);
int statusEnd = response.indexOf(STATUS_END, statusStart);
String status = response.substring(statusStart+STATUS_START.length(), statusEnd).trim();
if (data!=null) {
int dataStart = response.indexOf(DATA_START);
if (dataStart!=-1) {
int dataEnd = response.indexOf(DATA_END, dataStart);
if (dataEnd!=-1) {
String dataStr = response.substring(dataStart+DATA_START.length(), dataEnd).trim();
data.set(Double.valueOf(dataStr));
}
}
}
handleResponse(response);
return Integer.valueOf(status);
} catch (Throwable th) {
th.printStackTrace();
return -1;
}
}
private void handleResponse(String response) {
try {
SAXParser parser = SAXParserFactory.newInstance().newSAXParser();
parser.parse(new ByteArrayInputStream(response.getBytes()), new DefaultHandler() {
private boolean inStatus;
private boolean inData;
private boolean inError;
private boolean inFatal;
StringBuffer sb = new StringBuffer();;
/* (non-Javadoc)
* @see org.xml.sax.helpers.DefaultHandler#characters(char[], int, int)
*/
@Override
public void characters(char[] ch, int start, int length)
throws SAXException {
if (inStatus || inData || inError || inFatal) {
sb.append(ch, start, length);
}
super.characters(ch, start, length);
}
/* (non-Javadoc)
* @see org.xml.sax.helpers.DefaultHandler#startElement(java.lang.String, java.lang.String, java.lang.String, org.xml.sax.Attributes)
*/
@Override
public void startElement(String uri, String localName,
String qName, Attributes attributes)
throws SAXException {
localName = getLocalName(localName, qName);
if (localName.equals("status")) {
inStatus=true;
sb.setLength(0);
}
else if (localName.equals("data")) {
inData=true;
sb.setLength(0);
}
else if (localName.equals("error")) {
inError=true;
sb.setLength(0);
}
else if (localName.equals("fatal")) {
inFatal=true;
sb.setLength(0);
}
super.startElement(uri, localName, qName, attributes);
}
@Override
public void endElement(String uri, String localName,
String qName) throws SAXException {
localName = getLocalName(localName, qName);
if (localName.equals("status")) {
inStatus=false;
TERFactory.getInstance().logDebug("status: " + sb.toString());
sb.setLength(0);
}
else if (localName.equals("data")) {
inData=false;
TERFactory.getInstance().logDebug("data: " + sb.toString());
sb.setLength(0);
}
else if (localName.equals("error")) {
inError=false;
TERFactory.getInstance().logDebug("error: " + sb.toString());
sb.setLength(0);
}
else if (localName.equals("fatal")) {
inFatal=false;
TERFactory.getInstance().logDebug("fatal: " + sb.toString());
sb.setLength(0);
}
super.endElement(uri, localName, qName);
}
}, null);
} catch (ParserConfigurationException e) {
e.printStackTrace();
} catch (SAXException e) {
e.printStackTrace();
} catch (IOException e) {
e.printStackTrace();
}
}
private String getLocalName(String localName, String qName) {
if (localName.isEmpty()) {
String[] nameParts = qName.split(":",2);
if (nameParts.length==1) {
localName = nameParts[0];
}
else {
localName = nameParts[1];
}
}
return localName;
}
static boolean isReceiverRunning() {
synchronized (receiverRunning) {
return receiverRunning;
}
}
void stopReceiver() {
synchronized (receiverRunning) {
receiverRunning = false;
}
}
double computeDistance(double p_latitudeA, double p_latitudeB, double p_longitudeA, double p_longitudeB) {
double R = 6371000; // m
double dLat = p_latitudeB-p_latitudeA;
double dLon = p_longitudeB-p_longitudeA;
double lat1 = p_latitudeA;
double lat2 = p_latitudeB;
double a = Math.sin(dLat/2) * Math.sin(dLat/2) +
Math.sin(dLon/2) * Math.sin(dLon/2) * Math.cos(lat1) * Math.cos(lat2);
double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
double d = R * c;
return d;
}
}