GnssRemoteControl.java

/*
 * ----------------------------------------------------------------------------
 *  (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.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.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 = "<status>";
    private final String STATUS_END = "</status>";

    private final String DATA_START = "<data>";
    private final String DATA_END = "</data>";

    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) {
        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.awaitDistanceToCover(new AdapterControlPort("test", "TestComp"), 200.0);
                grc.getGpsTime();
//            grc.getScenarioStatus();
                grc.stopScenario();
//            grc.getScenarioStatus();
            }
        } catch (Throwable th) {
            th.printStackTrace();
        }
    }
    
    public void dispose() {
        try {
            System.out.println("+++++++++++++++++++++++++++++++++++++++++++++");
            System.out.println("CLOSING");
            System.out.println("+++++++++++++++++++++++++++++++++++++++++++++");
            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.38";
            }
        }
        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() {
        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) {
                                        System.out.println("Type            : " + type);
                                        System.out.println("Version         : " + version);
                                        System.out.println("Time(into run)  : " + time_into_run);
                                        System.out.println("Time(validity)  : " + time_of_validity);
                                        System.out.println("VehicleNumber   : " + vehicleNumber);
                                        System.out.println("Latitude        : " + curLatitude);
                                        System.out.println("Longitude       : " + curLongitude);
                                    }
                                    
                                    if (prevLatitude != null && prevLongitude != null) {
                                        distanceCovered += computeDistance(prevLatitude, curLatitude, prevLongitude, curLongitude);
                                    }
                                    prevLatitude = curLatitude;
                                    prevLongitude = curLongitude;
                                    
                                    if (debug) {
                                        System.out.println("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 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<Double> data = new ReferenceObject<Double>(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) {
                    System.out.println("timeGps : " + time);
                    time = System.currentTimeMillis();
                    System.out.println("timeSys : " + time);
                    System.out.println("now : " + now);
                    System.out.println("ref : " + 
                            new BigInteger(
                                    1, 
                                    ByteHelper.longToByteArray(time - ((java.util.Date)formatter.parse(itsdatestr)).getTime(), Long.SIZE / Byte.SIZE)
                                    ).longValue()
                            );
                    System.out.println("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<Double> 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";
            System.out.println(new Date().toString());
            System.out.println("+++++++++++++++++++++++++++++++++++++++++++++");
            System.out.println(command);
            System.out.println("+++++++++++++++++++++++++++++++++++++++++++++");
            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();
            System.out.println(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;
                        System.out.println("status: " + sb.toString());
                        sb.setLength(0);
                    }
                    else if (localName.equals("data")) {
                        inData=false;
                        System.out.println("data: " + sb.toString());
                        sb.setLength(0);
                    }
                    else if (localName.equals("error")) {
                        inError=false;
                        System.out.println("error: " + sb.toString());
                        sb.setLength(0);
                    }
                    else if (localName.equals("fatal")) {
                        inFatal=false;
                        System.out.println("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;
    }
}