senseless.h


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106

#ifndef SENSELESS_H
#define SENSELESS_H

#include <stddef.h> // size_t
#include <stdio.h>

void _clamp(double lower, double upper, double *value) {
	if (*value < lower) {
		*value = lower;
	} else if (*value > upper) {
		*value = upper;
	}
}

void _wrap(double *degree) {
	do {
		if (*degree < 0)
			*degree += 360;
		
		if (*degree >= 360)
			*degree -= 360;
	} while (*degree < 0 || *degree >= 360);
}

void _min(double *value, double lower) {
	if (*value < lower)
		*value = lower;
}

void _max(double *value, double upper) {
	if (*value > upper)
		*value = upper;
}

double senseless_directions(const char *directions, size_t length) {
	double degrees = 0.0;
	double factor = 90.0;
	size_t consumed = 0;

	do {
		if (length - consumed == 0)
			return degrees;

		char current = directions[consumed];
		if (consumed == 0) {
			switch (current) {
				case 'N': case 'n': degrees = 0; break;
				case 'E': case 'e': degrees = 90; break;
				case 'S': case 's': degrees = 180; break;
				case 'W': case 'w': degrees = 270; break;
			}
			_wrap(&degrees);
			continue;
		}

		factor /= 2;
		switch (current) {
			case 'N': case 'n':
				if (degrees >= 180) {
					degrees += factor;
				} else {
					degrees -= factor;
				}

				_clamp(0, 360, &degrees);
				break;
			case 'S': case 's':
				if (degrees >= 0 && degrees < 180) {
					degrees += factor;

					_clamp(0, 180, &degrees);
				} else {
					degrees -= factor;

					_clamp(180, 360, &degrees);
				}
				break;
			case 'W': case 'w':
				if (degrees >= 90 && degrees < 270) {
					// Covers the bottom-arc
					degrees += factor;
					_clamp(90, 270, &degrees);
				} else if (degrees < 90 && degrees < factor) {
					// Covers the top-right quadrant
					degrees = 360 - (factor - degrees);
				} else {
					degrees -= factor;
					_clamp(0, 360, &degrees);
				}
				break;
			case 'E': case 'e':
				if (degrees >= 270 || degrees < 90) {
					degrees += factor;
					_wrap(&degrees);
				} else {
					degrees -= factor;
					_wrap(&degrees);
				}
				break;
		}
	} while (++consumed);

	return 0.0;
}

#endif // SENSELESS_H