From baa842556c85f24e0cc1b4fd07da03b6609160c1 Mon Sep 17 00:00:00 2001
From: John Tsiombikas <nuclear@mutantstargoat.com>
Date: Mon, 17 Oct 2016 22:49:36 +0300
Subject: [PATCH] foo

---
 .gitignore        |    4 ++++
 src/dev_smag.c    |   18 ++++----------
 src/serial.h      |    1 +
 src/unix/serial.c |   24 +++++++++----------
 src/vmath.inl     |   68 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 90 insertions(+), 25 deletions(-)
 create mode 100644 .gitignore
 create mode 100644 src/vmath.inl

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..a2793a8
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,4 @@
+*.o
+*.swp
+*.d
+smouse
diff --git a/src/dev_smag.c b/src/dev_smag.c
index 40f5046..f96ae5d 100644
--- a/src/dev_smag.c
+++ b/src/dev_smag.c
@@ -76,21 +76,13 @@ static int opendev(const char *dev)
 		return -1;	/* failed to get a response */
 	}
 
-	while(ser_getline(fd, buf, sizeof buf)) {
-		printf("magellan open(%s): %s\n", dev, buf);
-	}
-
 	ser_printf(fd, "vQ\r");
-	ser_wait(fd, 250);
 
-	while(ser_getline(fd, buf, sizeof buf)) {
-		if(buf[0] == 'v') {
-			break;
-		}
-	}
-	printf("magellan open(%s): got version string: \"%s\"\n", dev, buf + 1);
-	if(!strstr(buf, "v Magellan")) {
-		fprintf(stderr, "unknown device\n");
+	while(ser_getline_block(fd, buf, sizeof buf) && buf[0] != 'v');
+
+	printf("DBG: \"%s\"\n", buf);
+	if(buf[0] != 'v' || !strstr(buf, "MAGELLAN")) {
+		fprintf(stderr, "unknown device: \"%s\"\n", buf + 1);
 		ser_close(fd);
 		return -1;
 	}
diff --git a/src/serial.h b/src/serial.h
index e4a8670..f920461 100644
--- a/src/serial.h
+++ b/src/serial.h
@@ -17,5 +17,6 @@ int ser_read(int fd, char *buf, int count);
 
 void ser_printf(int fd, const char *fmt, ...);
 char *ser_getline(int fd, char *buf, int bsz);
+char *ser_getline_block(int fd, char *buf, int bsz);
 
 #endif	/* SERIAL_H_ */
diff --git a/src/unix/serial.c b/src/unix/serial.c
index fe8aeb3..cdcdb88 100644
--- a/src/unix/serial.c
+++ b/src/unix/serial.c
@@ -137,7 +137,6 @@ char *ser_getline(int fd, char *buf, int bsz)
 	static char linebuf[512];
 	static int widx;
 	int i, rd, size;
-	char *ptr;
 
 	size = sizeof linebuf - widx;
 	while(size && (rd = read(fd, linebuf + widx, size)) > 0) {
@@ -145,27 +144,28 @@ char *ser_getline(int fd, char *buf, int bsz)
 		size -= rd;
 	}
 
-	ptr = linebuf;
 	for(i=0; i<widx; i++) {
-		if(*ptr == '\r' || *ptr == '\n') {
-			*ptr++ = '\n';
-			if(i < widx - 1 && ptr[1] == '\n') {
-				*ptr++ = 0;
-			}
-
-			size = widx >= bsz ? bsz - 1 : widx;
+		if(linebuf[i] == '\r' || linebuf[i] == '\n') {
+			size = i >= bsz ? bsz - 1 : i;
 			memcpy(buf, linebuf, size);
 			buf[size] = 0;
 
-			memmove(linebuf, linebuf + widx, sizeof linebuf - widx);
+			memmove(linebuf, linebuf + i + 1, sizeof linebuf - i - 1);
+			printf("ser_getline-> \"%s\"\n", buf);
 			return buf;
-		} else {
-			++ptr;
 		}
 	}
 	return 0;
 }
 
+char *ser_getline_block(int fd, char *buf, int bsz)
+{
+	if(!ser_wait(fd, -1)) {
+		return 0;
+	}
+	return ser_getline(fd, buf, bsz);
+}
+
 static int baud_id(int baud)
 {
 	switch(baud) {
diff --git a/src/vmath.inl b/src/vmath.inl
new file mode 100644
index 0000000..49b5a2f
--- /dev/null
+++ b/src/vmath.inl
@@ -0,0 +1,68 @@
+/* vector functions */
+static inline vec3_t v3_cons(float x, float y, float z)
+{
+	vec3_t res;
+	res.x = x;
+	res.y = y;
+	res.z = z;
+	return res;
+}
+
+static inline vec3_t quat_vec(quat_t q)
+{
+	vec3_t v;
+	v.x = q.x;
+	v.y = q.y;
+	v.z = q.z;
+	return v;
+}
+
+static inline float v3_dot(vec3_t v1, vec3_t v2)
+{
+	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
+}
+
+/* quaternion functions */
+static inline quat_t quat_cons(float s, float x, float y, float z)
+{
+	quat_t q;
+	q.x = x;
+	q.y = y;
+	q.z = z;
+	q.w = s;
+	return q;
+}
+
+static inline quat_t quat_mul(quat_t q1, quat_t q2)
+{
+	quat_t res;
+	vec3_t v1 = quat_vec(q1);
+	vec3_t v2 = quat_vec(q2);
+
+	res.w = q1.w * q2.w - v3_dot(v1, v2);
+	res.x = v2.x * q1.w + v1.x * q2.w + (v1.y * v2.z - v1.z * v2.y);
+	res.y = v2.y * q1.w + v1.y * q2.w + (v1.z * v2.x - v1.x * v2.z);
+	res.z = v2.z * q1.w + v1.z * q2.w + (v1.x * v2.y - v1.y * v2.x);
+	return res;
+}
+
+static inline void quat_to_mat(mat4_t res, quat_t q)
+{
+	m4_cons(res,	1.0 - 2.0 * q.y*q.y - 2.0 * q.z*q.z,	2.0 * q.x * q.y + 2.0 * q.w * q.z,		2.0 * q.z * q.x - 2.0 * q.w * q.y, 0,
+					2.0 * q.x * q.y - 2.0 * q.w * q.z,		1.0 - 2.0 * q.x*q.x - 2.0 * q.z*q.z,	2.0 * q.y * q.z + 2.0 * q.w * q.x, 0,
+					2.0 * q.z * q.x + 2.0 * q.w * q.y,		2.0 * q.y * q.z - 2.0 * q.w * q.x,		1.0 - 2.0 * q.x*q.x - 2.0 * q.y*q.y, 0,
+					0, 0, 0, 1);
+}
+
+/* matrix functions */
+static inline void m4_cons(mat4_t m,
+		float m11, float m12, float m13, float m14,
+		float m21, float m22, float m23, float m24,
+		float m31, float m32, float m33, float m34,
+		float m41, float m42, float m43, float m44)
+{
+	m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
+	m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
+	m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
+	m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
+}
-- 
1.7.10.4