1////////////////////////////////////////////////////////////////////////////////
2// //
3// This software is supplied under the terms of a license agreement or //
4// nondisclosure agreement with Mitov Software and may not be copied //
5// or disclosed except in accordance with the terms of that agreement. //
6// Copyright(c) 2002-2016 Mitov Software. All Rights Reserved. //
7// //
8////////////////////////////////////////////////////////////////////////////////
9#ifndef _MITOV_PACKET_h
11#define _MITOV_PACKET_h
12#include <Mitov.h>
14namespace Mitov
16{
17//---------------------------------------------------------------------------
18 class TPacketAccumulator : public Mitov::SimpleList<unsigned char>
19 {
20 public:
21 bool SkipBytes( int ACount )
22 {
23 if( size() < ACount )
24 return false;
25 pop_front( ACount );
27 return true;
28 }
29 bool FindBytes( int ASize, unsigned char *ABytesPtr, int AStartPos, int ASearchSize, int &APosition )
31 {
32 for( APosition = AStartPos; APosition < MitovMin( (int)( _size - ASize ), ASearchSize ); ++APosition )
33 if( memcmp( ABytesPtr, _list + APosition, ASize ) == 0 )
34 {
35 APosition += ASize;
36 return true;
37 }
38 return false;
40 }
41 bool FindBytes( int ASize, unsigned char *ABytesPtr, int &APosition )
43 {
44 return FindBytes( ASize, ABytesPtr, 0, _size, APosition );
45 }
46 bool ExtractBytes( int ASize, void *ABytesPtr )
48 {
49 if( size() < ASize )
50 return false;
51 memcpy( ABytesPtr, _list, ASize );
53 pop_front( ASize );
54 return true;
55 }
56 bool PeekBytes( int ASize, void *ABytesPtr )
58 {
59 if( size() < ASize )
60 return false;
61 memcpy( ABytesPtr, _list, ASize );
63 return true;
64 }
65 };
67//---------------------------------------------------------------------------
68 class BasicPacketElement : public OpenWire::Component
69 {
70 public:
71 virtual int GetSize( bool &Alligned ) { Alligned = true; return 0; }
72 virtual bool GetIsPopulated() { return true; }
73 virtual bool GetIsModified() { return false; }
74 virtual void ClearModified() {}
75 virtual void GetData( unsigned char *ADataStart, unsigned char *&AData, unsigned char &AOffset ) {}
76 virtual bool Expand( unsigned char *AInBuffer, unsigned char *AOutBuffer, int &ASize ) { return false; }
77 };
78//---------------------------------------------------------------------------
79 class BasicUnpacketElement : public OpenWire::Component
80 {
81 public:
82 virtual int Start( bool &AAllign ) { AAllign = false; return 0; }
83 virtual bool ExpandBuffers( TPacketAccumulator *AInAccumulator, TPacketAccumulator *AOutAccumulator, int &ASize, int &APosition, bool &AIsValid, bool &AIsCorruptedPacket ) { return false; }
84 virtual bool Process( TPacketAccumulator *AAccumulator, unsigned char &AOffset ) { return true; }
85 };
87//---------------------------------------------------------------------------
88 class BasicPacketSinkElement : public BasicPacketElement
89 {
90 public:
91 OpenWire::SinkPin InputPin;
92 protected:
94 virtual void DoReceive( void *_Data ) = 0;
95 public:
97 BasicPacketSinkElement()
98 {
99 InputPin.SetCallback( this, (OpenWire::TOnPinReceive)&BasicPacketSinkElement::DoReceive );
100 }
101 };
103//---------------------------------------------------------------------------
104 template<typename T> class BasicTypedPacketSinkElement : public BasicPacketSinkElement
105 {
106 protected:
107 T FValue;
108 bool FPopulated : 1;
109 bool FModified : 1;
110/*
112 public:
113 T InitialValue;
114 virtual void SystemInit()
116 {
117 inherited::SystemInit();
118 FValue = InitialValue;
119 }
120*/
121 public:
122 virtual int GetSize( bool &Alligned ) override
123 {
124 Alligned = true;
125// Serial.println( sizeof( double ) );
126 return sizeof( T ) * 8;
127 }
128 virtual bool GetIsPopulated() override
130 {
131 return FPopulated;
132 }
133 virtual bool GetIsModified() override
135 {
136 return FModified;
137 }
138 virtual void ClearModified() override
140 {
141 FModified = false;
142 }
143 virtual void GetData( unsigned char *ADataStart, unsigned char *&AData, unsigned char &AOffset ) override
145 {
146 if( AOffset )
147 {
148 ++AData;
149 AOffset = 0;
150 }
151 *(T*)AData = FValue;
153// memcpy( AData, &FValue, sizeof( T ));
154 AData += sizeof( T );
155 }
156 protected:
158 virtual void DoReceive( void *_Data ) override
159 {
160 T AValue = *(T*)_Data;
161 FPopulated = true;
162 if( AValue == FValue )
163 return;
164 FValue = AValue;
166 FModified = true;
167 }
168 public:
170 BasicTypedPacketSinkElement() :
171 FPopulated( false ),
172 FModified( false )
173 {
174 }
175 };
177//---------------------------------------------------------------------------
178 template<typename T> class BasicTypedUnpacketSourceElement : public BasicUnpacketElement
179 {
180 public:
181 OpenWire::SourcePin OutputPin;
182 public:
184 virtual int Start( bool &AAllign ) override
185 {
186 AAllign = true;
187 return sizeof( T ) * 8;
188 }
189 virtual bool Process( TPacketAccumulator *AAccumulator, unsigned char &AOffset ) override
191 {
192 if( AOffset )
193 {
194 if( ! AAccumulator->SkipBytes( 1 ))
195 return false;
196 AOffset = 0;
198 }
199 T AValue;
201 if( ! AAccumulator->ExtractBytes( sizeof( T ), &AValue ))
202 return false;
203 OutputPin.Notify( &AValue );
205 return true;
207 }
208 virtual bool ExpandBuffers( TPacketAccumulator *AInAccumulator, TPacketAccumulator *AOutAccumulator, int &ASize, int &APosition, bool &AIsValid, bool &AIsCorruptedPacket ) override
210 {
211 APosition = ( APosition + 7 ) / 8;
212 APosition *= 8;
213 APosition += sizeof( T ) * 8;
214 return false;
215 }
216 };
217//---------------------------------------------------------------------------
218 class UnpacketDigitalBinaryElement : public BasicTypedUnpacketSourceElement<bool>
219 {
220 public:
221 virtual int Start( bool &AAllign ) override
222 {
223 AAllign = false;
224 return 1;
225 }
226 virtual bool Process( TPacketAccumulator *AAccumulator, unsigned char &AOffset ) override
228 {
229 unsigned char AValue;
230 if( ! AAccumulator->PeekBytes( 1, &AValue ))
231 return false;
232 bool ABoolValue = (( AValue & ( 1 << AOffset )) != 0 );
234 ++AOffset;
235 if( AOffset == 8 )
237 {
238 AOffset = 0;
239 if( ! AAccumulator->SkipBytes( 1 ))
240 return false;
241 }
243 OutputPin.Notify( &ABoolValue );
245 return true;
247 }
248 virtual bool ExpandBuffers( TPacketAccumulator *AInAccumulator, TPacketAccumulator *AOutAccumulator, int &ASize, int &APosition, bool &AIsValid, bool &AIsCorruptedPacket ) override
250 {
251 ++ APosition;
252 return false;
253 }
254 };
255//---------------------------------------------------------------------------
256 class PacketBasicMarkerBinaryElement : public BasicPacketElement
257 {
258 public:
259 Mitov::Bytes Bytes;
260 };
262//---------------------------------------------------------------------------
263 class UnpacketBasicMarkerBinaryElement : public BasicUnpacketElement
264 {
265 public:
266 Mitov::Bytes Bytes;
267 public:
269 virtual int Start( bool &AAllign ) override
270 {
271 AAllign = true;
272 return Bytes._BytesSize * 2 * 8;
273 }
274/*
276 virtual bool ExpandBuffers( TPacketAccumulator *AInAccumulator, TPacketAccumulator *AOutAccumulator, int &ASize, int &APosition, bool &AIsValid, bool &AIsCorruptedPacket ) override
277 {
278 return false;
279 }
280 virtual bool Process( TPacketAccumulator *AAccumulator, unsigned char &AOffset ) override { return true; }
282*/
283 };
285//---------------------------------------------------------------------------
286 class UnpacketMarkerBinaryElement : public UnpacketBasicMarkerBinaryElement
287 {
288 public:
289 virtual bool ExpandBuffers( TPacketAccumulator *AInAccumulator, TPacketAccumulator *AOutAccumulator, int &ASize, int &APosition, bool &AIsValid, bool &AIsCorruptedPacket ) override
290 {
291 APosition = ( APosition + 7 ) / 8;
292 APosition *= 8;
293 int ABytesSize = Bytes._BytesSize;
295 int ASearchPos;
296 if( ! AInAccumulator->FindBytes( ABytesSize, Bytes._Bytes, APosition / 8, Bytes._BytesSize, ASearchPos ))
297 AIsCorruptedPacket = true;
298 else if( ASearchPos != APosition / 8 + ABytesSize )
300 AIsCorruptedPacket = true;
301 APosition += ABytesSize * 8;
303 return false;
304 }
305 virtual bool Process( TPacketAccumulator *AAccumulator, unsigned char &AOffset ) override
307 {
308 if( AOffset != 0 )
309 {
310 if( !AAccumulator->SkipBytes( 1 ))
311 return false;
312 AOffset = 0;
314 }
315 if( ! AAccumulator->SkipBytes( Bytes._BytesSize ))
317 return false;
318 return true;
320 }
321 };
322//---------------------------------------------------------------------------
323 class UnpacketChecksumElement : public BasicUnpacketElement
324 {
325 virtual int Start( bool &AAllign ) override
326 {
327 AAllign = true;
328 return 8;
329 }
330 virtual bool ExpandBuffers( TPacketAccumulator *AInAccumulator, TPacketAccumulator *AOutAccumulator, int &ASize, int &APosition, bool &AIsValid, bool &AIsCorruptedPacket ) override
332 {
333 APosition = ( APosition + 7 ) / 8;
334 APosition *= 8;
335 unsigned char *ABytes = new unsigned char[ APosition / 8 + 1 ];
337// SetLength( ABytes, APosition + 1 );
338 if( ! AInAccumulator->PeekBytes( APosition / 8 + 1, ABytes ))
340 {
341 AIsValid = false;
342 delete [] ABytes;
343 return false;
344 }
345 unsigned char AChecksum = 0;
347// for AByte in ABytes do
349// AChecksum ^= AByte;
350 for( int i = 0; i < APosition / 8 + 1; ++i )
351 AChecksum ^= ABytes[ i ];
352 delete [] ABytes;
354 if( AChecksum )
356 {
357 AIsCorruptedPacket = true;
358 return false;
359 }
360 APosition += 8;
362 return false;
363 }
364 virtual bool Process( TPacketAccumulator *AAccumulator, unsigned char &AOffset ) override
366 {
367 if( AOffset )
368 {
369 if( ! AAccumulator->SkipBytes( 1 ))
370 return false;
371 AOffset = 0;
373 }
374 AAccumulator->SkipBytes( 1 );
376 return true;
377 }
378 };
379//---------------------------------------------------------------------------
380 class PacketMarkerBinaryElement : public PacketBasicMarkerBinaryElement
381 {
382 public:
383 virtual int GetSize( bool &Alligned ) override
384 {
385 Alligned = true;
386 return Bytes._BytesSize * 8;
387 }
388 virtual void GetData( unsigned char *ADataStart, unsigned char *&AData, unsigned char &AOffset ) override
390 {
391 if( Bytes._BytesSize == 0 )
392 return;
393 if( AOffset )
395 {
396 ++AData;
397 AOffset = 0;
398 }
399 memcpy( AData, Bytes._Bytes, Bytes._BytesSize );
401 AData += Bytes._BytesSize;
402 }
403 };
405//---------------------------------------------------------------------------
406 class PacketChecksumElement : public BasicPacketElement
407 {
408 public:
409 bool Enabled = true;
410 public:
412 virtual int GetSize( bool &Alligned ) override
413 {
414 if( Enabled )
415 {
416 Alligned = true;
417 return 8;
418 }
419 return 0;
421 }
422 virtual void GetData( unsigned char *ADataStart, unsigned char *&AData, unsigned char &AOffset ) override
424 {
425 if( !Enabled )
426 return;
427 if( AOffset )
429 {
430 ++AData;
431 AOffset = 0;
432 }
433 unsigned char AChecksum = 0;
435 for( ;ADataStart < AData; ++ADataStart )
436 AChecksum ^= *ADataStart;
437 *AData = AChecksum;
439 ++AData;
440 }
441 };
443//---------------------------------------------------------------------------
444 class UnpacketHeadMarkerBinaryElement : public UnpacketBasicMarkerBinaryElement
445 {
446 protected:
447 Mitov::SimpleList<unsigned char> FDoubledBytes;
448 public:
450 virtual int Start( bool &AAllign ) override
451 {
452 for( int i = 0; i < Bytes._BytesSize; ++i )
453 FDoubledBytes.push_back( Bytes._Bytes[ i ] );
454 for( int i = 0; i < Bytes._BytesSize; ++i )
456 FDoubledBytes.push_back( Bytes._Bytes[ i ] );
457 AAllign = true;
459 return Bytes._BytesSize * 2 * 8;
460 }
461 virtual bool Process( TPacketAccumulator *AAccumulator, unsigned char &AOffset ) override
463 {
464 if( AOffset )
465 {
466 if( ! AAccumulator->SkipBytes( 1 ))
467 return false;
468 AOffset = 0;
470 }
471 // AOldDebugBuf := AAccumulator.GetAsBuffer();
473 AAccumulator->SkipBytes( FDoubledBytes.size() );
474 return true;
475 }
476 virtual bool ExpandBuffers( TPacketAccumulator *AInAccumulator, TPacketAccumulator *AOutAccumulator, int &ASize, int &APosition, bool &AIsValid, bool &AIsCorruptedPacket ) override
478 {
479 APosition = ( APosition + 7 ) / 8;
480 APosition *= 8;
481 int ABytesSize = Bytes._BytesSize;
483 if( !ABytesSize )
484 return false;
485 unsigned char *ABytes = Bytes._Bytes;
487 // if( not AInAccumulator.FindSkipBytes( ABytesSize * 2, @FDoubledBytes[ 0 ], ASkipped )) then
488 // Exit( False );
489 int AFindPosition;
491 if( !AInAccumulator->FindBytes( ABytesSize * 2, FDoubledBytes.begin(), AFindPosition ))
493 {
494 AIsValid = false;
495 return false;
496 }
497 AFindPosition -= ABytesSize * 2;
499 AInAccumulator->SkipBytes( AFindPosition );
500 APosition += ABytesSize * 2 * 8;
502 // Dec( ASize, AFindPosition );
504 // if( ASize <= ABytesSize * 2 ) then
506 // Exit( False );
507 // ANewDebugBuf := AInAccumulator.GetAsBuffer();
509 if( !AInAccumulator->FindBytes( ABytesSize, ABytes, ABytesSize * 2, ASize - ABytesSize * 2, AFindPosition ))
511 return false;
512 // ANewDebugBuf := AInAccumulator.GetAsBuffer();
514// AMarkerBuffer := TSLBlockBuffer.CreateData( ABytes, ABytesSize );
515 // if( not AInAccumulator.FindBytes( ABytesSize, ABytes, ASize - ABytesSize * 2, AFindPosition )) then
517 // Exit( False );
518 AOutAccumulator->append( ABytes, ABytesSize );
520 AOutAccumulator->append( ABytes, ABytesSize );
521 for(;;)
523 {
524 Mitov::SimpleList<unsigned char> ABuffer;
525 ABuffer.AddCount( AFindPosition );
526// ABuffer := TSLBlockBuffer.CreateSize( AFindPosition );
527 AInAccumulator->ExtractBytes( AFindPosition, ABuffer );
528 AOutAccumulator->append( ABuffer, AFindPosition );
529 unsigned char ACount;
531 AInAccumulator->ExtractBytes( 1, &ACount );
532 for( int i = 0; i <= ACount; ++i )
534 AOutAccumulator->append( ABytes, ABytesSize );
535 ASize = ABytesSize * ACount + 1;
537 if( ! AInAccumulator->FindBytes( ABytesSize, ABytes, AFindPosition, ASize - AFindPosition, AFindPosition ))
539 return true;
540 }
542 return true;
544 }
545 public:
547 void SkipHeader( TPacketAccumulator *AAccumulator )
548 {
549 AAccumulator->SkipBytes( Bytes._BytesSize * 2 );
550 }
551 };
553//---------------------------------------------------------------------------
554 class PacketHeadMarkerBinaryElement : public PacketMarkerBinaryElement
555 {
556 public:
557 virtual int GetSize( bool &Alligned ) override
558 {
559 Alligned = true;
560 return Bytes._BytesSize * 2 * 8;
561 }
562 virtual void GetData( unsigned char *ADataStart, unsigned char *&AData, unsigned char &AOffset ) override
564 {
565 if( Bytes._BytesSize == 0 )
566 return;
567 if( AOffset )
569 {
570 ++AData;
571 AOffset = 0;
572 }
573 memcpy( AData, Bytes._Bytes, Bytes._BytesSize );
575 AData += Bytes._BytesSize;
576 memcpy( AData, Bytes._Bytes, Bytes._BytesSize );
578 AData += Bytes._BytesSize;
579 }
580 virtual bool Expand( unsigned char *AInBuffer, unsigned char *AOutBuffer, int &ASize ) override
582 {
583 if( Bytes._BytesSize == 0 )
584 return false;
585// memcpy( AOutBuffer, AInBuffer, ASize );
587// return true;
588 unsigned char *ABytes = Bytes._Bytes;
590 int ABytesSize = Bytes._BytesSize;
591 int AOutSize = ABytesSize * 2;
593 memcpy( AOutBuffer, AInBuffer, AOutSize );
595 AOutBuffer += AOutSize;
596 AInBuffer += AOutSize;
597 int i = AOutSize;
599 for( ; i < ASize - ABytesSize * 2 + 1; ++i )
600 {
601 if( memcmp( AInBuffer, ABytes, ABytesSize ) == 0 )
602 if( memcmp( AInBuffer + ABytesSize, ABytes, ABytesSize ) == 0 )
603 {
604 memcpy( AOutBuffer, AInBuffer, ABytesSize );
605 AInBuffer += ABytesSize * 2;
606// Serial.println( "START" );
608 AOutBuffer += ABytesSize;
609 unsigned char ACount = 0;
610 i += ABytesSize;
611 for( ; i < ASize - ABytesSize + 1; i += ABytesSize )
612 {
613 if( memcmp( AInBuffer, ABytes, ABytesSize ) != 0 )
614 break;
615// Serial.println( "INC" );
617 ++ACount;
618 if( ACount == 255 )
619 break;
620 AInBuffer += ABytesSize;
622 }
623 if( ACount == *ABytes )
625 {
626 if( ACount > 0 )
627 {
628// Serial.println( "DEC" );
629 --ACount;
630 AInBuffer -= ABytesSize;
631 i -= ABytesSize;
632 }
633 }
634 *AOutBuffer++ = ACount;
636 AOutSize += ABytesSize + 1;
637 continue;
638 }
639 *AOutBuffer++ = *AInBuffer++;
641//Serial.print( "++AOutSize" );
642 ++AOutSize;
643 }
644 int ACorrection = ASize - i;
646 memcpy( AOutBuffer, AInBuffer, ACorrection );
647// *AOutBuffer = 5;
648// Serial.print( "ADebugCount : " );
650// Serial.println( ADebugCount );
651// Serial.print( "AOutSize : " );
653// Serial.println( AOutSize );
654// Serial.print( "TEST : " );
656// Serial.println( ACorrection );
657// Serial.println( i );
658 ASize = AOutSize + ACorrection;
660 return true;
661 }
662 };
664//---------------------------------------------------------------------------
665 class PacketDigitalBinaryElement : public BasicTypedPacketSinkElement<bool>
666 {
667 public:
668 virtual int GetSize( bool &Alligned ) override
669 {
670 Alligned = false;
671 return 1;
672 }
673 virtual void GetData( unsigned char *ADataStart, unsigned char *&AData, unsigned char &AOffset ) override
675 {
676 *AData &= 0xFF >> ( 8 - AOffset ); // Zero the upper bits
677 if( FValue )
678 *AData |= 1 << AOffset; // Set the bit
679 ++AOffset;
681 if( AOffset == 8 )
682 {
683 AOffset = 0;
684 ++AData;
685 }
686 }
687 };
688//---------------------------------------------------------------------------
689 class Packet : public Mitov::CommonSource, public Mitov::ClockingSupport
690 {
691 typedef Mitov::CommonSource inherited;
692 public:
694 Mitov::SimpleObjectList<BasicPacketElement*> Elements;
695 bool OnlyModified = false;
697 PacketHeadMarkerBinaryElement HeadMarker;
699 PacketChecksumElement Checksum;
700 protected:
702 int FBufferSize;
703 unsigned char *FBuffers[ 2 ];
705 Mitov::SimpleList<BasicPacketElement*> FAllElements;
706 protected:
708 virtual void DoClockReceive( void *_Data ) override
709 {
710// Serial.println( "++++++" );
711 int AElementCount = FAllElements.size();
712 if( OnlyModified )
713 {
714 bool AModified = false;
715 for( int i = 0; i < AElementCount; ++i )
716 if( FAllElements[ i ]->GetIsModified() )
717 {
718 AModified = true;
719 break;
720 }
721 if( !AModified )
723 return;
724 }
725 else
726 {
727 for( int i = 0; i < AElementCount; ++i )
728 if( ! FAllElements[ i ]->GetIsPopulated() )
729 return;
730 }
732 unsigned char *ADataPtr = FBuffers[ 0 ];
734 unsigned char AOffset = 0;
735 for( int i = 0; i < AElementCount; ++i )
736 {
737 FAllElements[ i ]->GetData( FBuffers[ 0 ], ADataPtr, AOffset );
738// while( AElementSize-- )
739// OutputPin.Notify( AData++ );
740 }
743 int ASize = FBufferSize;
745// Serial.println( ASize );
746 int ABufferIndex = 0;
747// Serial.println( "--------" );
748 for( int i = 0; i < AElementCount; ++i )
749 {
750// Serial.println( ABufferIndex & 1 );
751// Serial.println( 1 ^ ( ABufferIndex & 1 ) );
752 unsigned char *AInBuffer = FBuffers[ ABufferIndex & 1 ];
753 unsigned char *AOutBuffer = FBuffers[ 1 ^ ( ABufferIndex & 1 )];
754 if( FAllElements[ i ]->Expand( AInBuffer, AOutBuffer, ASize ))
755 ++ ABufferIndex;
756 }
757 ADataPtr = FBuffers[ ABufferIndex & 1 ];
759 OutputPin.SendValue( Mitov::TDataBlock( ASize, ADataPtr ));
760// Serial.println( ASize );
762// while( ASize-- )
763// OutputPin.Notify( ADataPtr++ );
764 }
766 protected:
768 virtual void SystemLoopBegin( unsigned long currentMicros )
769 {
770 if( !ClockInputPin.IsConnected() )
771 DoClockReceive( NULL );
772// delay( 1000 );
774 inherited::SystemLoopBegin( currentMicros );
775 }
776 virtual void SystemStart()
778 {
779 FBufferSize = 0;
780 FAllElements.push_back( &HeadMarker );
781 for( int i = 0; i < Elements.size(); ++i )
782 FAllElements.push_back( Elements[ i ] );
783 FAllElements.push_back( &Checksum );
785 for( int i = 0; i < FAllElements.size(); ++i )
787 {
788 bool AAlligned = false;
789 FBufferSize += FAllElements[ i ]->GetSize( AAlligned );
790// Serial.println( FBufferSize );
791 if( AAlligned )
792 {
793 FBufferSize = ( FBufferSize + 7 ) / 8;
794 FBufferSize *= 8;
795 }
796 }
798// Serial.println( FBufferSize );
800 FBufferSize = ( FBufferSize + 7 ) / 8;
802 FBuffers[ 0 ] = new unsigned char[ FBufferSize * 2 ];
803 FBuffers[ 1 ] = new unsigned char[ FBufferSize * 2 ];
804 inherited::SystemStart();
806 }
807/*
809 virtual ~Packet()
810 {
811 delete []FBuffers[ 1 ];
812 delete []FBuffers[ 0 ];
813 }
814*/
815 };
816//---------------------------------------------------------------------------
817 class Unpacket : public Mitov::CommonSink
818 {
819 typedef Mitov::CommonSink inherited;
820 public:
822 Mitov::SimpleObjectList<BasicUnpacketElement*> Elements;
823 UnpacketHeadMarkerBinaryElement HeadMarker;
825 UnpacketChecksumElement Checksum;
826 protected:
828 TPacketAccumulator FAccumulator;
829 int FMinSize = 0;
830 Mitov::SimpleList<BasicUnpacketElement*> FAllElements;
831 protected:
833 virtual void DoReceive( void *_Data ) override
834 {
835 if( FMinSize == 0 )
836 return;
837// Serial.println( "FMinSize" );
839// Serial.println( FMinSize );
840/// Serial.println( "T0:" );
841/// Serial.println( FAccumulator.size() );
842 Mitov::TDataBlock ABlock = *(Mitov::TDataBlock *)_Data;
843 while( ABlock.Size-- )
844 FAccumulator.push_back( *ABlock.Data++ );
845/// Serial.println( FAccumulator.size() );
847// Serial.println( "T1" );
849 TPacketAccumulator AOldAccumulator;
851 TPacketAccumulator *AInAccumulator;
852 TPacketAccumulator ARealOutAccumulator;
853 TPacketAccumulator *AOutAccumulator = &ARealOutAccumulator;
854 TPacketAccumulator *AAccumulator;
855// while( FAccumulator.size() >= FMinSize )
857 while( FAccumulator.size() >= FMinSize )
858 {
859// Serial.println( "FMinSize" );
860// Serial.println( FMinSize );
861/// Serial.println( "T2" );
862//AOldAccumulator.clear();
863//AOldAccumulator.append( FAccumulator, FAccumulator.size() );
864//return;
865 AOldAccumulator = FAccumulator;
866//return;
867 AInAccumulator = &FAccumulator;
868 AOutAccumulator->clear();
870 AAccumulator = &FAccumulator;
871 int ASize = FMinSize;
872 bool AIsValid = true;
873 bool AIsCorruptedPacket = false;
875 int APosition = 0;
876//return;
877 for( int i = 0; i < FAllElements.size(); ++i )
878 {
879// Serial.println( "enter" );
880// Serial.println( FAccumulator.size() );
881 if( FAllElements[ i ]->ExpandBuffers( AInAccumulator, AOutAccumulator, ASize, APosition, AIsValid, AIsCorruptedPacket ))
882 {
883// Serial.println( "ExpandBuffers" );
884//return;
885 if( AIsCorruptedPacket )
886 {
887 FAccumulator = AOldAccumulator;
888 HeadMarker.SkipHeader( &FAccumulator );
889 break;
890 }
891 AAccumulator = AOutAccumulator;
893 if( AAccumulator == &FAccumulator )
894 {
895// Serial.println( "AAccumulator == &FAccumulator" );
896 AInAccumulator = AOutAccumulator;
897 AOutAccumulator->clear();
898 }
899 else
901 swap( AInAccumulator, AOutAccumulator );
902 }
904// Serial.println( "Step1" );
906 if( AIsCorruptedPacket )
907 {
908// Serial.println( "AIsCorruptedPacket" );
909 FAccumulator = AOldAccumulator;
910 HeadMarker.SkipHeader( &FAccumulator );
911 break;
912 }
913 if( ! AIsValid )
915 {
916 FAccumulator = AOldAccumulator;
917 return;
918 }
919 }
921// Serial.println( "ExpandBuffers OUT" );
923 if( AIsCorruptedPacket )
924 {
925// Serial.println( "AIsCorruptedPacket SKIPPING" );
926//return;
927// if( ! HeadMarker.Bytes._BytesSize )
928 FAccumulator.SkipBytes( 1 );
929 continue;
931 }
932 if( AAccumulator->size() < FMinSize )
934 {
935 FAccumulator = AOldAccumulator;
936 return;
937 }
938 if( ASize < FMinSize )
940 // for AItem in FAllElements do
941 // if( not AItem.CanProcess( AAccumulator )) then
942 {
943 FAccumulator = AOldAccumulator;
944 return;
945 }
946// Serial.println( "PROCESS" );
948 unsigned char AOffset = 0;
949 for( int i = 0; i < FAllElements.size(); ++i )
950 {
951 if( ! FAllElements[ i ]->Process( AAccumulator, AOffset ))
952 break;
953//break;
955 }
956// Serial.println( "exit" );
959// Serial.println( FAllElements.size() );
960// return;
961 }
963 }
964 virtual void SystemStart() override
966 {
967// Serial.println( "FMinSize" );
968 FAllElements.push_back( &HeadMarker );
969 for( int i = 0; i < Elements.size(); ++i )
971 FAllElements.push_back( Elements[ i ] );
972 FAllElements.push_back( &Checksum );
974 FMinSize = 0;
976 for( int i = 0; i < FAllElements.size(); ++i )
977 {
978 bool AAlligned = false;
979 FMinSize += FAllElements[ i ]->Start( AAlligned );
980 if( AAlligned )
981 {
982 FMinSize = ( FMinSize + 7 ) / 8;
983 FMinSize = FMinSize * 8;
984 }
985 }
986 FMinSize = ( FMinSize + 7 ) / 8;
988// Serial.println( "FMinSize" );
990// Serial.println( FMinSize );
991 inherited::SystemStart();
993 }
994 };
995//---------------------------------------------------------------------------
996}
998#endif