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
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373using System.Diagnostics;
using System.IO.Ports;
using System.Text;
using System.Threading.Channels;
using RobotSharp.Models;

namespace RobotSharp.Services;

public record ServoTarget(int Channel, int Position);

public record LogEntry(DateTimeOffset Timestamp, string Direction, string Text);

internal record QueuedCommand(string Text, TimeSpan AckTimeout);

public class RobotSerialService : IDisposable
{
	private static readonly TimeSpan IdleFlushDelay = TimeSpan.FromMilliseconds(150);
	private static readonly TimeSpan WriteGuardTimeout = TimeSpan.FromSeconds(2);
	private static readonly TimeSpan AckTimeoutDefault = TimeSpan.FromSeconds(2);

	private readonly Lock gate = new();
	private readonly List<Channel<LogEntry>> subscribers = [];
	private readonly List<LogEntry> history = [];
	private readonly StringBuilder rxBuffer = new();
	private readonly Timer idleFlushTimer;
	private SerialPort? port;

	private Channel<QueuedCommand>? outgoing;
	private CancellationTokenSource? senderCts;
	private TaskCompletionSource<bool>? ackTcs;

	public RobotSerialService(IConfiguration configuration)
	{
		Settings = configuration.GetSection(RobotSerialSettings.SectionName).Get<RobotSerialSettings>()
				   ?? new RobotSerialSettings();
		idleFlushTimer = new Timer(_ => FlushRxBuffer(), null, Timeout.Infinite, Timeout.Infinite);
	}

	public RobotSerialSettings Settings { get; private set; }

	public bool IsConnected => port is { IsOpen: true };

	public void UpdateSettings(RobotSerialSettings settings) => Settings = settings;

	public IEnumerable<string> GetAvailablePorts() => SerialPort.GetPortNames();

	public void Connect()
	{
		lock (gate)
		{
			if (port is { IsOpen: true }) return;

			port = new SerialPort(Settings.PortName, Settings.BaudRate)
			{
				NewLine = "\n",
				ReadTimeout = 2000,
				WriteTimeout = 2000
			};
			port.DataReceived += OnDataReceived;
			port.Open();

			// Capacity 1 + DropOldest: only the most recent not-yet-sent command is kept.
			// Even with proper OK-waiting, queuing every single slider tick overwhelms
			// the controller after a burst of commands (confirmed on hardware - it drops
			// off the bus). Coalescing to the latest position keeps traffic sane.
			outgoing = Channel.CreateBounded<QueuedCommand>(
				new BoundedChannelOptions(1) { FullMode = BoundedChannelFullMode.DropOldest });
			senderCts = new CancellationTokenSource();
			_ = Task.Run(() => SenderLoopAsync(outgoing.Reader, senderCts.Token));

			AddLog("SYS", $"Connected to {Settings.PortName} @ {Settings.BaudRate}");
		}
	}

	public void Disconnect()
	{
		lock (gate)
		{
			if (port == null) return;
			ClosePort();
			AddLog("SYS", "Disconnected");
		}
	}

	// Tears down the port and sender loop without logging a message; the caller
	// (Disconnect or a failed write) decides what to log. Must hold gate.
	private void ClosePort()
	{
		if (port == null) return;

		port.DataReceived -= OnDataReceived;
		idleFlushTimer.Change(Timeout.Infinite, Timeout.Infinite);
		FlushRxBuffer();

		senderCts?.Cancel();
		senderCts = null;
		outgoing = null;
		ackTcs?.TrySetCanceled();
		ackTcs = null;

		try
		{
			if (port.IsOpen) port.Close();
		}
		catch (Exception)
		{
			// The device may already have dropped off the bus (e.g. after a ~RE restart);
			// closing is best-effort, the port handle is discarded regardless.
		}
		finally
		{
			port.Dispose();
			port = null;
		}
	}

	// Drops the port reference without calling Close()/Dispose(). Use this when a
	// write is known to be stuck: SerialPort.Close() blocks until any pending
	// synchronous Read/Write completes, so calling it on a wedged port would hang
	// the whole app (including shutdown). We accept the handle leak - the OS
	// reclaims it once the process actually exits or the USB device is replugged.
	// Must be called while holding gate.
	private void AbandonPort()
	{
		if (port == null) return;

		try
		{
			port.DataReceived -= OnDataReceived;
		}
		catch (Exception)
		{
		}

		idleFlushTimer.Change(Timeout.Infinite, Timeout.Infinite);
		rxBuffer.Clear();

		senderCts?.Cancel();
		senderCts = null;
		outgoing = null;
		ackTcs?.TrySetCanceled();
		ackTcs = null;
		port = null;
	}

	private void OnDataReceived(object sender, SerialDataReceivedEventArgs e)
	{
		try
		{
			var data = port?.ReadExisting();
			if (string.IsNullOrEmpty(data)) return;

			var needsIdleFallback = true;
			lock (gate)
			{
				rxBuffer.Append(data);
				FlushCompleteLines();

				// Confirmed on the actual hardware: "OK" is sent as bare "OK" (0x4F 0x4B),
				// with no CR/LF terminator at all. Match it immediately instead of waiting
				// for the line to end, so acknowledgement isn't delayed by a guessed timeout.
				if (rxBuffer.ToString().Trim(' ').Equals("OK", StringComparison.OrdinalIgnoreCase))
				{
					rxBuffer.Clear();
					HandleIncomingLine("OK");
					needsIdleFallback = false;
				}
			}

			if (needsIdleFallback)
			{
				// Safety net for any other unterminated reply we don't explicitly
				// recognize: flush whatever is left once traffic goes quiet.
				idleFlushTimer.Change(IdleFlushDelay, Timeout.InfiniteTimeSpan);
			}
		}
		catch (TimeoutException)
		{
		}
	}

	// Must be called while holding gate.
	private void FlushCompleteLines()
	{
		var text = rxBuffer.ToString();
		var lines = text.Split('\n');
		for (var i = 0; i < lines.Length - 1; i++)
		{
			var line = lines[i].Trim('\r', '\n', ' ');
			if (line.Length > 0) HandleIncomingLine(line);
		}
		rxBuffer.Clear();
		rxBuffer.Append(lines[^1]);
	}

	private void FlushRxBuffer()
	{
		lock (gate)
		{
			var line = rxBuffer.ToString().Trim('\r', '\n', ' ');
			rxBuffer.Clear();
			if (line.Length > 0) HandleIncomingLine(line);
		}
	}

	// Must be called while holding gate.
	private void HandleIncomingLine(string line)
	{
		AddLog("RX", line);
		if (line.Equals("OK", StringComparison.OrdinalIgnoreCase))
			ackTcs?.TrySetResult(true);
	}

	public void SendServoCommand(IEnumerable<ServoTarget> targets, int speedMs, int delayMs)
	{
		var sb = new StringBuilder();
		foreach (var t in targets)
			sb.Append('#').Append(t.Channel).Append('P').Append(t.Position);
		sb.Append('T').Append(speedMs).Append('D').Append(delayMs);

		// The controller replies "OK" only after it has actually executed the move,
		// i.e. after roughly Speed + Delay - size the wait to that instead of a
		// generic fixed timeout.
		var marginMs = (int)(speedMs * 0.1);
		var timeoutMs = speedMs + delayMs + marginMs;
		Send(sb.ToString(), TimeSpan.FromMilliseconds(timeoutMs));
	}

	public void SendRestart()
	{
		try
		{
			Send("~RE", AckTimeoutDefault);
		}
		catch (Exception)
		{
			// The interface may drop off the bus as soon as it receives the restart
			// command, before the write can be acknowledged. Still proceed to disconnect.
		}
		finally
		{
			Disconnect();
		}
	}

	// Enqueues the command; the sender loop writes it once any previously
	// queued command has been acknowledged with "OK" (or timed out).
	private void Send(string command, TimeSpan ackTimeout)
	{
		ChannelWriter<QueuedCommand> writer;
		lock (gate)
		{
			if (port is not { IsOpen: true } || outgoing is null)
				throw new InvalidOperationException("Serial port is not connected.");
			writer = outgoing.Writer;
		}
		writer.TryWrite(new QueuedCommand(command, ackTimeout));
	}

	private async Task SenderLoopAsync(ChannelReader<QueuedCommand> reader, CancellationToken token)
	{
		try
		{
			await foreach (var queued in reader.ReadAllAsync(token))
			{
				var command = queued.Text;
				var tcs = new TaskCompletionSource<bool>(TaskCreationOptions.RunContinuationsAsynchronously);
				SerialPort? currentPort;
				lock (gate)
				{
					ackTcs = tcs;
					currentPort = port;
				}

				if (currentPort is not { IsOpen: true })
					break;

				// SerialPort.Write() can hang indefinitely on a wedged USB-serial adapter
				// even with WriteTimeout set - a known System.IO.Ports issue. Run it on a
				// worker thread and give up on it after WriteGuardTimeout so the sender
				// loop (and the app) never gets stuck waiting for it.
				var writeTask = Task.Run(() => currentPort.Write(command + "\r\n"), token);
				var writeCompleted = await Task.WhenAny(writeTask, Task.Delay(WriteGuardTimeout, token));

				if (writeCompleted != writeTask)
				{
					AddLog("SYS", "Poort reageert niet meer op verzenden - verbinding wordt losgekoppeld (kabel/interface controleren)");
					lock (gate)
					{
						AbandonPort();
					}
					break;
				}

				Stopwatch ackStopwatch;
				try
				{
					await writeTask;
					AddLog("TX", command);
					ackStopwatch = Stopwatch.StartNew();
				}
				catch (Exception ex)
				{
					// The write actually completed (with an error), so the port isn't
					// stuck - safe to close it normally.
					lock (gate)
					{
						ClosePort();
					}
					AddLog("SYS", $"Verbinding verbroken (verzenden mislukt): {ex.Message}");
					break;
				}

				var completed = await Task.WhenAny(tcs.Task, Task.Delay(queued.AckTimeout, token));
				ackStopwatch.Stop();
				if (completed != tcs.Task)
				{
					// Closing and reopening the port here didn't actually recover the
					// interface in practice. Just log the miss and keep going with the
					// next queued command instead.
					AddLog("SYS", $"Geen OK ontvangen op '{command}' (timeout na {ackStopwatch.ElapsedMilliseconds}ms, verwacht ~{queued.AckTimeout.TotalMilliseconds}ms)");
				}
				else
				{
					AddLog("SYS", $"OK ontvangen na {ackStopwatch.ElapsedMilliseconds}ms");
				}

				lock (gate)
				{
					if (ackTcs == tcs) ackTcs = null;
				}
			}
		}
		catch (OperationCanceledException)
		{
		}
	}

	private void AddLog(string direction, string text)
	{
		var entry = new LogEntry(DateTimeOffset.Now, direction, text);
		lock (gate)
		{
			history.Add(entry);
			if (history.Count > 500) history.RemoveAt(0);
		}
		foreach (var sub in subscribers.ToArray())
			sub.Writer.TryWrite(entry);
	}

	public IEnumerable<LogEntry> GetHistory()
	{
		lock (gate) return [.. history];
	}

	public ChannelReader<LogEntry> Subscribe()
	{
		var channel = Channel.CreateUnbounded<LogEntry>();
		lock (gate) subscribers.Add(channel);
		return channel.Reader;
	}

	public void Unsubscribe(ChannelReader<LogEntry> reader)
	{
		lock (gate) subscribers.RemoveAll(c => c.Reader == reader);
	}

	public void Dispose()
	{
		Disconnect();
		idleFlushTimer.Dispose();
	}
}