From ff1f4cc5a0ef001b104fb1e36a4e7e660b555910 Mon Sep 17 00:00:00 2001
From: Anton Tananaev <anton.tananaev@gmail.com>
Date: Thu, 3 May 2018 06:21:42 +1200
Subject: [PATCH] Support Castel response messages

---
 .../protocol/CastelProtocolDecoder.java       | 31 ++++++++++++++++---
 .../protocol/CastelProtocolDecoderTest.java   |  3 ++
 2 files changed, 30 insertions(+), 4 deletions(-)

diff --git a/src/org/traccar/protocol/CastelProtocolDecoder.java b/src/org/traccar/protocol/CastelProtocolDecoder.java
index 44a5e213c9b..db6f24eeb73 100644
--- a/src/org/traccar/protocol/CastelProtocolDecoder.java
+++ b/src/org/traccar/protocol/CastelProtocolDecoder.java
@@ -91,6 +91,7 @@ public CastelProtocolDecoder(CastelProtocol protocol) {
     public static final short MSG_SC_GPS_SLEEP = 0x4009;
     public static final short MSG_SC_FUEL = 0x400E;
     public static final short MSG_SC_AGPS_REQUEST = 0x5101;
+    public static final short MSG_SC_QUERY_RESPONSE = (short) 0xA002;
     public static final short MSG_SC_CURRENT_LOCATION = (short) 0xB001;
 
     public static final short MSG_CC_LOGIN = 0x4001;
@@ -272,7 +273,7 @@ private void decodeAlarm(Position position, int alarm) {
 
     private Object decodeSc(
             Channel channel, SocketAddress remoteAddress, ChannelBuffer buf,
-            int version, ChannelBuffer id, int type, DeviceSession deviceSession) {
+            int version, ChannelBuffer id, short type, DeviceSession deviceSession) {
 
         if (type == MSG_SC_HEARTBEAT) {
 
@@ -395,6 +396,28 @@ private Object decodeSc(
 
             return position;
 
+        } else if (type == MSG_SC_QUERY_RESPONSE) {
+
+            Position position = createPosition(deviceSession);
+
+            buf.readUnsignedShort(); // index
+            buf.readUnsignedByte(); // response count
+            buf.readUnsignedByte(); // response index
+
+            int failureCount = buf.readUnsignedByte();
+            for (int i = 0; i < failureCount; i++) {
+                buf.readUnsignedShort(); // tag
+            }
+
+            int successCount = buf.readUnsignedByte();
+            for (int i = 0; i < successCount; i++) {
+                buf.readUnsignedShort(); // tag
+                position.set(Position.KEY_RESULT,
+                        buf.readBytes(buf.readUnsignedShort()).toString(StandardCharsets.US_ASCII));
+            }
+
+            return position;
+
         }
 
         return null;
@@ -402,7 +425,7 @@ private Object decodeSc(
 
     private Object decodeCc(
             Channel channel, SocketAddress remoteAddress, ChannelBuffer buf,
-            int version, ChannelBuffer id, int type, DeviceSession deviceSession) {
+            int version, ChannelBuffer id, short type, DeviceSession deviceSession) {
 
         if (type == MSG_CC_HEARTBEAT) {
 
@@ -459,7 +482,7 @@ private Object decodeCc(
 
     private Object decodeMpip(
             Channel channel, SocketAddress remoteAddress, ChannelBuffer buf,
-            int version, ChannelBuffer id, int type, DeviceSession deviceSession) {
+            int version, ChannelBuffer id, short type, DeviceSession deviceSession) {
 
         if (type == 0x4001) {
 
@@ -513,7 +536,7 @@ protected Object decode(
         }
 
         ChannelBuffer id = buf.readBytes(20);
-        int type = ChannelBuffers.swapShort(buf.readShort());
+        short type = ChannelBuffers.swapShort(buf.readShort());
 
         DeviceSession deviceSession = getDeviceSession(
                 channel, remoteAddress, id.toString(StandardCharsets.US_ASCII).trim());
diff --git a/test/org/traccar/protocol/CastelProtocolDecoderTest.java b/test/org/traccar/protocol/CastelProtocolDecoderTest.java
index cb44087a169..2baee549473 100644
--- a/test/org/traccar/protocol/CastelProtocolDecoderTest.java
+++ b/test/org/traccar/protocol/CastelProtocolDecoderTest.java
@@ -12,6 +12,9 @@ public void testDecode() throws Exception {
 
         CastelProtocolDecoder decoder = new CastelProtocolDecoder(new CastelProtocol());
 
+        verifyAttributes(decoder, binary(ByteOrder.LITTLE_ENDIAN,
+                "40403a00043231335750323031373030363135360000000000a00200000100000101201100344a474446364545374a4230373632363056ff0d0a"));
+
         verifyAttributes(decoder, binary(ByteOrder.LITTLE_ENDIAN,
                 "4040560004323133474c3230313630303033363400000000004002a122a05a5423a05abe0f2a000000000007f1f90014000000040001640011170003001e000505210b210c210d210f2101062b58ef02001a25950d0a"));