/*
* static void Test()
* {
* var ser = new SerialQueue("COM5", 250000);
* ser.Start();
*
* Thread.Sleep(100);
*
* MemoryStream buffer = new MemoryStream();
* int res = 0;
*
* var cq = new KlipperSharp.command_queue();
* MemoryStream msg = new MemoryStream(16);
*
* while (true)
* {
* KlipperSharp.queue_message qm;
* ser.pull(out qm);
*
* msg.Position = 0;
* msg.SetLength(0);
* for (var i = SerialQueue.MESSAGE_HEADER_SIZE;
* i < qm.len - SerialQueue.MESSAGE_TRAILER_SIZE; i++)
* {
* msg.WriteByte(qm.msg[i]);
* }
* msg.Position = 0;
* var code = msg.ReadByte();
*
* Console.WriteLine();
* Console.WriteLine($"MSG code:{code} {(qm.receive_time - qm.sent_time) * 1000}ms");
*
* if (code == 0)
* {
* Console.WriteLine("identify_response");
* var offset = msg.parse(false);
* var len = msg.ReadByte();
* if (len > 0)
* {
* var buf = new byte[len];
* var read = msg.Read(buf, 0, len);
* if (read != len)
* Console.WriteLine("Error");
*
* buffer.Write(buf, 0, read);
* res += read;
*
* Console.WriteLine($"Read bytes {buffer.Length}; {read}/{len}");
* }
* if (len < 40)
* {
* Console.WriteLine("download done");
* res = -1;
*
* //var final = ZlibDecompress(buffer.ToArray());
* //var jsonText = Encoding.ASCII.GetString(final);
* //var xml = XDocument.Load(JsonReaderWriterFactory.CreateJsonReader(Encoding.ASCII.GetBytes(jsonText), new XmlDictionaryReaderQuotas()));
* //xml.Save("identify.xml");
*
* //Assert.AreEqual(PROGMEM, buffer.ToArray());
*
* break;
* }
* }
* else
* {
* //Console.WriteLine($"{qm.receive_time * 1000} code:{code}");
* }
* if (res != -1)
* {
* Console.WriteLine($"identify: get data {res}");
* //{ 1, "identify offset=%u count=%c" }, uint32, byte
* msg.Position = 0;
* msg.SetLength(0);
* msg.WriteByte(1);
* msg.encode(res);
* msg.encode(40);
* ser.send(cq, msg.ToArray(), (int)msg.Length);
* }
*
* }
* Console.ReadKey();
* }
*/
// Return a message read from the serial port (or wait for one if none
// available)
static void serialqueue_pull(out queue_message pqm)
{
//pthread_mutex_lock(&sq->lock);
// Wait for message to be available
while (receive_queue.Count == 0)
{
//if (pollreactor_is_exit(&sq->pr))
// goto exit;
//sq->receive_waiting = 1;
//int ret = pthread_cond_wait(&sq->cond, &sq->lock);
//if (ret)
// report_errno("pthread_cond_wait", ret);
}
// Remove message from queue
queue_message qm;
receive_queue.TryDequeue(out qm);
// Copy message
pqm = qm;
//pqm = new queue_message();
//memcpy(pqm->msg, qm->msg, qm->len);
//pqm.len = qm.len;
//pqm.sent_time = qm.sent_time;
//pqm.receive_time = qm.receive_time;
//debug_queue_add(&sq->old_receive, qm);
//pthread_mutex_unlock(&sq->lock);
return;
//exit:
// pqm = new queue_message();
// pqm.len = -1;
//pthread_mutex_unlock(&sq->lock);
}
static void build_and_send_command(double eventtime)
{
queue_message output = new queue_message();
output.len = MESSAGE_HEADER_SIZE;
while (ready_bytes != 0)
{
// Find highest priority message (message with lowest req_clock)
ulong min_clock = MAX_CLOCK;
command_queue cq = null;
queue_message qm = new queue_message();
foreach (var q in pending_queues)
{
queue_message m;
if (!q.ready_queue.TryPeek(out m) && m.req_clock < min_clock)
{
min_clock = m.req_clock;
cq = q;
qm = m;
}
}
// Append message to outgoing command
if (output.len + qm.len > MESSAGE_MAX - MESSAGE_TRAILER_SIZE)
{
break;
}
cq.ready_queue.TryDequeue(out qm);
if (cq.ready_queue.Count == 0 && cq.stalled_queue.Count == 0)
{
pending_queues.Remove(cq);
}
Buffer.MemoryCopy(qm.msg, &output.msg[output.len], MESSAGE_MAX, qm.len);
//memcpy(output.msg[output.len], qm.msg, qm.len);
output.len += qm.len;
ready_bytes -= qm.len;
}
// Fill header / trailer
output.len += MESSAGE_TRAILER_SIZE;
output.msg[MESSAGE_POS_LEN] = output.len;
output.msg[MESSAGE_POS_SEQ] = (byte)(MESSAGE_DEST | (send_seq & MESSAGE_SEQ_MASK));
int crc = Crc16_ccitt(new ReadOnlySpan <byte>(output.msg, output.len - MESSAGE_TRAILER_SIZE));
output.msg[output.len - MESSAGE_TRAILER_CRC] = (byte)(crc >> 8);
output.msg[output.len - MESSAGE_TRAILER_CRC + 1] = (byte)(crc & 0xff);
output.msg[output.len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC;
// Send message
fixed(byte *psendBuffer = sendBuffer)
{
*((queue_message *)psendBuffer) = output;
}
serialPort.Write(sendBuffer, 0, output.len);
//int ret = write(sq->serial_fd, output->msg, output->len);
//if (ret < 0)
// report_errno("write", ret);
bytes_write += output.len;
if (eventtime > idle_time)
{
idle_time = eventtime;
}
idle_time += output.len * baud_adjust;
output.sent_time = eventtime;
output.receive_time = idle_time;
if (sent_queue.Count == 0)
{
// pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, sq->idle_time + sq->rto);
retransmitTimer = idle_time + rto;
}
if (rtt_sample_seq != 0)
{
rtt_sample_seq = send_seq;
}
send_seq++;
need_ack_bytes += output.len;
sent_queue.Enqueue(output);
}
static void handle_message(int length, double eventtime)
{
//Console.Write(eventtime + " ");
//PrintByteArray(input_buf, length);
// Calculate receive sequence number
ulong rseq = (receive_seq & ~MESSAGE_SEQ_MASK) | (input_buf[MESSAGE_POS_SEQ] & MESSAGE_SEQ_MASK);
if (rseq < receive_seq)
{
rseq += MESSAGE_SEQ_MASK + 1;
}
if (rseq != receive_seq)
{
// New sequence number
update_receive_seq(eventtime, rseq);
}
if (length == MESSAGE_MIN)
{
// Ack/nak message
if (last_ack_seq < rseq)
{
last_ack_seq = rseq;
}
else if (rseq > ignore_nak_seq && sent_queue.Count != 0)
{
// Duplicate Ack is a Nak - do fast retransmit
//pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, PR_NOW);
retransmitTimer = PR_NOW;
}
}
if (length > MESSAGE_MIN)
{
// Add message to receive queue
queue_message qm = new queue_message(input_buf, (byte)length);
qm.sent_time = (rseq > retransmit_seq ? last_receive_sent_time : 0.0);
qm.receive_time = GetTime(); //get_monotonic(); // must be time post read()
qm.receive_time -= baud_adjust * length;
receive_queue.Enqueue(qm);
//check_wake_receive(sq);
Console.WriteLine($"receive time {(qm.receive_time - qm.sent_time) * 1000}ms");
//{ 0, "identify_response offset=%u data=%.*s" }, uint32, buffer
MemoryStream msg = new MemoryStream(length);
for (var i = MESSAGE_HEADER_SIZE; i < length - MESSAGE_TRAILER_SIZE; i++)
{
msg.WriteByte(input_buf[i]);
}
msg.Position = 0;
var code = msg.ReadByte();
if (code == 0)
{
Console.WriteLine("identify_response");
var offset = msg.parse(false);
var len = msg.ReadByte();
if (len > 0)
{
var buf = new byte[len];
var read = msg.Read(buf, 0, len);
if (read != len)
{
Console.WriteLine("Error");
}
buffer.Write(buf, 0, read);
res += read;
Console.WriteLine($"Read bytes {buffer.Length}; {read}/{len}");
}
if (len < 40)
{
Console.WriteLine("download done");
res = -1;
var final = ZlibDecompress(buffer.ToArray());
var jsonText = Encoding.ASCII.GetString(final);
//System.Text.Json.JsonDocument doc = System.Text.Json.JsonDocument.Parse(jsonText);
var xml = XDocument.Load(JsonReaderWriterFactory.CreateJsonReader(Encoding.ASCII.GetBytes(jsonText), new XmlDictionaryReaderQuotas()));
xml.Save("identify.xml");
//Console.WriteLine(Encoding.ASCII.GetString(final));
}
}
else
{
Console.WriteLine($"{eventtime * 1000} code:{code}; seq:{rseq}; len:{length}");
var text = Encoding.ASCII.GetString(input_buf, 1 + MESSAGE_HEADER_SIZE, length - MESSAGE_TRAILER_SIZE);
Console.WriteLine(text);
}
}
if (length > MESSAGE_MIN && res != -1)
{
Console.WriteLine(eventtime + " identify get data " + res + " seq:" + send_seq);
//{ 1, "identify offset=%u count=%c" }, uint32, byte
MemoryStream msg = new MemoryStream(16);
msg.WriteByte(1);
msg.encode(res);
msg.encode(40);
Send(msg);
}
}
