From a9607ba09068c1e4248e011cd6a3a15a437a8ebb Mon Sep 17 00:00:00 2001
From: Knut Ahlers <knut@ahlers.me>
Date: Wed, 6 Nov 2019 14:51:29 +0100
Subject: [PATCH] Add placement reading

Signed-off-by: Knut Ahlers <knut@ahlers.me>
---
 datatypes.go    | 59 ++++++++++++++++++++++++++++++++++++++++++++-----
 helpers.go      | 51 ++++++++++++++++++++++++++++++++++++++++++
 helpers_test.go | 51 ++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 155 insertions(+), 6 deletions(-)
 create mode 100644 helpers.go
 create mode 100644 helpers_test.go

diff --git a/datatypes.go b/datatypes.go
index 570fe2f..a1ed09c 100644
--- a/datatypes.go
+++ b/datatypes.go
@@ -1,6 +1,12 @@
 package sii
 
-import "strings"
+import (
+	"bytes"
+	"regexp"
+	"strings"
+
+	"github.com/pkg/errors"
+)
 
 // See https://modding.scssoft.com/wiki/Documentation/Engine/Units
 
@@ -10,12 +16,50 @@ import "strings"
 
 // float2-4 => [2]float - [4]float
 
-type Placement struct {
-	X, Y, Z       float64
-	W, X2, Y2, Z2 float64
+var placementRegexp = regexp.MustCompile(`^\(([0-9.]+|&[0-9a-f]+), ([0-9.]+|&[0-9a-f]+), ([0-9.]+|&[0-9a-f]+)\) \(([0-9.]+|&[0-9a-f]+); ([0-9.]+|&[0-9a-f]+), ([0-9.]+|&[0-9a-f]+), ([0-9.]+|&[0-9a-f]+)\)$`)
+
+// Placement contains 7 floats: (x, y, z) (w; x, y, z)
+type Placement [7]float32
+
+func (p Placement) MarshalSII() ([]byte, error) {
+	var siiFloats = [][]byte{}
+
+	for _, f := range p {
+		b, err := float2sii(f)
+		if err != nil {
+			return nil, errors.Wrap(err, "Unable to encode float")
+		}
+		siiFloats = append(siiFloats, b)
+	}
+
+	var buf = new(bytes.Buffer)
+
+	buf.Write([]byte("("))
+	bytes.Join(siiFloats[0:3], []byte(", "))
+	buf.Write([]byte(") ("))
+	buf.Write(siiFloats[3])
+	buf.Write([]byte("; "))
+	bytes.Join(siiFloats[4:7], []byte(", "))
+	buf.Write([]byte(")"))
+
+	return buf.Bytes(), nil
 }
 
-// TODO: Implement marshalling for Placement
+func (p *Placement) UnmarshalSII(in []byte) error {
+	if !placementRegexp.Match(in) {
+		return errors.New("Input data does not match expected format")
+	}
+
+	grps := placementRegexp.FindSubmatch(in)
+	var err error
+	for i := 0; i < 7; i++ {
+		if p[i], err = sii2float(grps[i+1]); err != nil {
+			return errors.Wrap(err, "Unable to decode float")
+		}
+	}
+
+	return nil
+}
 
 // fixed => native type int
 
@@ -38,8 +82,10 @@ type Ptr struct {
 	unit   *Unit
 }
 
-func (p Ptr) CanResolve() bool   { return strings.HasPrefix(p.Target, ".") }
+func (p Ptr) CanResolve() bool { return strings.HasPrefix(p.Target, ".") }
+
 func (p Ptr) MarshalSII() []byte { return []byte(p.Target) }
+
 func (p Ptr) Resolve() Block {
 	if p.Target == "null" {
 		return nil
@@ -52,6 +98,7 @@ func (p Ptr) Resolve() Block {
 	}
 	return nil
 }
+
 func (p *Ptr) UnmarshalSII(in []byte) error {
 	p.Target = string(in)
 	return nil
diff --git a/helpers.go b/helpers.go
new file mode 100644
index 0000000..aacf5c6
--- /dev/null
+++ b/helpers.go
@@ -0,0 +1,51 @@
+package sii
+
+import (
+	"bytes"
+	"encoding/binary"
+	"encoding/hex"
+	"strconv"
+
+	"github.com/pkg/errors"
+)
+
+func float2sii(f float32) ([]byte, error) {
+	var (
+		buf = new(bytes.Buffer)
+		err error
+	)
+
+	err = binary.Write(buf, binary.BigEndian, f)
+	if err != nil {
+		return nil, errors.Wrap(err, "Unable to encode float")
+	}
+
+	dst := make([]byte, hex.EncodedLen(buf.Len()))
+	hex.Encode(dst, buf.Bytes())
+
+	return append([]byte("&"), dst...), nil
+}
+
+func sii2float(f []byte) (float32, error) {
+	if f[0] != '&' {
+		out, err := strconv.ParseFloat(string(f), 32)
+		return float32(out), err
+	}
+
+	// Strip leading '&'
+	f = f[1:]
+
+	var (
+		err error
+		out float32
+	)
+
+	dst := make([]byte, hex.DecodedLen(len(f)))
+	_, err = hex.Decode(dst, f)
+	if err != nil {
+		return 0, errors.Wrap(err, "Unable to read hex format")
+	}
+
+	err = binary.Read(bytes.NewReader(dst), binary.BigEndian, &out)
+	return out, errors.Wrap(err, "Unable to decode hex notation")
+}
diff --git a/helpers_test.go b/helpers_test.go
new file mode 100644
index 0000000..c68c01a
--- /dev/null
+++ b/helpers_test.go
@@ -0,0 +1,51 @@
+package sii
+
+import "testing"
+
+func TestSii2FloatConversion(t *testing.T) {
+	var (
+		err error
+		f   float32
+	)
+
+	for b, exp := range map[string]float32{
+		"0.00250711967": 0.00250711967,
+		"1.0":           1.0,
+		"&3b244e7d":     0.00250711967,
+		"&3f46da61":     0.7767697,
+		"&47135818":     37720.0938,
+	} {
+		f, err = sii2float([]byte(b))
+		if err != nil {
+			t.Errorf("Conversion of %q failed: %s", b, err)
+			continue
+		}
+
+		if f != exp {
+			t.Errorf("Conversion of %q has unxpected result: %f != %f", b, f, exp)
+		}
+	}
+}
+
+func TestFloat2SiiConversion(t *testing.T) {
+	var (
+		err error
+		f   []byte
+	)
+
+	for b, exp := range map[float32]string{
+		0.00250711967: "&3b244e7d",
+		0.7767697:     "&3f46da61",
+		37720.0938:    "&47135818",
+	} {
+		f, err = float2sii(b)
+		if err != nil {
+			t.Errorf("Conversion of %f failed: %s", b, err)
+			continue
+		}
+
+		if string(f) != exp {
+			t.Errorf("Conversion of %f has unxpected result: %s != %s", b, f, exp)
+		}
+	}
+}