package dongfang.mkt.comm;
import java.io.IOException;
import java.io.OutputStream;
import dongfang.mkt.RequestFrameVisitor;
import dongfang.mkt.frames.AllDisplaysRequestFrame;
import dongfang.mkt.frames.AnalogDebugLabelRequestFrame;
import dongfang.mkt.frames.AttitudeDataRequestFrame;
import dongfang.mkt.frames.ChangeParameterSetRequestFrame;
import dongfang.mkt.frames.CompassHeadingRequestFrame;
import dongfang.mkt.frames.DebugRequestFrame;
import dongfang.mkt.frames.ExternalControlRequestFrame;
import dongfang.mkt.frames.LoopbackTestRequestFrame;
import dongfang.mkt.frames.MotorTestRequestFrame;
import dongfang.mkt.frames.OSDDataRequestFrame;
import dongfang.mkt.frames.ReadExternalControlRequestFrame;
import dongfang.mkt.frames.ReadIMUConfigurationRequestFrame;
import dongfang.mkt.frames.ReadMotorMixerRequestFrame;
import dongfang.mkt.frames.ReadParamSetRequestFrame;
import dongfang.mkt.frames.ReadRCChannelsRequestFrame;
import dongfang.mkt.frames.ReadVariablesRequestFrame;
import dongfang.mkt.frames.RequestFrame;
import dongfang.mkt.frames.ResetRequestFrame;
import dongfang.mkt.frames.SetCompassHeadingRequestFrame;
import dongfang.mkt.frames.SingleDisplayRequestFrame;
import dongfang.mkt.frames.VersionRequestFrame;
import dongfang.mkt.frames.WriteIMUConfigurationRequestFrame;
import dongfang.mkt.frames.WriteMotorMixerRequestFrame;
import dongfang.mkt.frames.WriteParamSetRequestFrame;
public class MKOutputStream
extends OutputStream implements RequestFrameVisitor
{
public class MKDataOutputStream
{
int[] inbuf =
new int[3];
int[] outbuf =
new int[4];
int inbufptr =
0;
void writeByte
(int b
) throws IOException {
if(inbufptr == inbuf.
length) flush
();
inbuf
[inbufptr++
] = b
;
}
void writeSignedByte
(int b
) throws IOException {
if (b
<0) b+=
256;
writeByte
(b
);
}
public void writeBytes
(int[] bs
) throws IOException {
for (int i=
0; i
<bs.
length; i++
)
writeByte
(bs
[i
]);
}
void writeWord
(int w
) throws IOException {
writeByte
(w
& 0xff
);
writeByte
(w
>>> 8);
}
void writeChars
(char[] s
) throws IOException {
for (int i=
0; i
<s.
length; i++
) {
// Here, a 1:1 mapping between byte values and char codes is assumed.
// That means we're assuming ISO-8859-1 (= the first 256 code points
// of Unicode, which Java uses for chars)
writeByte
(s
[i
]);
}
}
void flush
() throws IOException {
if (inbufptr ==
0)
return;
while(inbufptr
< inbuf.
length) {
// add padding .. well just clear it, for tidyness.
inbuf
[inbufptr++
] =
0;
}
MKOutputStream.
this.
writeByte((inbuf
[0] >>> 2) +
'=');
MKOutputStream.
this.
writeByte(( ((inbuf
[0] & 0x03
) << 4) |
(inbuf
[1] >>> 4) ) +
'=');
MKOutputStream.
this.
writeByte(( ((inbuf
[1] & 0x0f
) << 2) |
(inbuf
[2] >>> 6)) +
'=');
MKOutputStream.
this.
writeByte(((inbuf
[2] & 0x3f
) +
'='));
inbufptr =
0;
}
}
final OutputStream os
;
MKDataOutputStream base64OutputStream =
new MKDataOutputStream
();
int crc
;
public MKOutputStream
(OutputStream os
) {
this.
os = os
;
}
@
Override
public void write
(int b
) throws IOException {
os.
write(b
);
// System.out.println("Writing: " + b);
}
public void writeByte
(int b
) throws IOException {
crc += b
;
write
(b
);
}
public void write
(RequestFrame f
) throws IOException {
write
(crc =
'#');
int address = f.
getAddress() +
'a';
writeByte
(address
);
// Will cause one of the "visit" methods below
// to be called, depending on the type of f.
f.
accept(this);
base64OutputStream.
flush();
crc
%=
4096;
write
((crc
>>> 6) +
'=');
write
((crc
& 0x3f
) +
'=');
write
('\r');
}
/*
public void visit(RequestFrame f) {
throw new RuntimeException("Unbound RequestFrame type: "
+ f.getClass().getSimpleName()
+ ". Don't know how to output.");
}
*/
public void visit
(AnalogDebugLabelRequestFrame f
) throws IOException {
writeByte
('a');
base64OutputStream.
writeByte(f.
getChannel());
}
public void visit
(AttitudeDataRequestFrame f
) throws IOException {
writeByte
('c');
base64OutputStream.
writeByte(f.
getAutoSendInterval());
}
public void visit
(DebugRequestFrame f
) throws IOException {
writeByte
('d');
base64OutputStream.
writeByte(f.
getAutoSendInterval());
}
public void visit
(ChangeParameterSetRequestFrame f
) throws IOException {
writeByte
('f');
base64OutputStream.
writeByte(f.
getParameterSetNumber());
}
public void visit
(VersionRequestFrame f
) throws IOException {
writeByte
('v');
}
public void visit
(ResetRequestFrame f
) throws IOException {
writeByte
('R');
}
public void visit
(MotorTestRequestFrame f
) throws IOException {
writeByte
('t');
base64OutputStream.
writeBytes(f.
getMotorValues());
}
public void visit
(SingleDisplayRequestFrame f
) throws IOException {
writeByte
('l');
base64OutputStream.
writeByte(f.
getMenuItemCode()); // In 0.74 there is not the all in one mode.
}
public void visit
(AllDisplaysRequestFrame f
) throws IOException {
writeByte
('h');
base64OutputStream.
writeByte(f.
getPageOrder().
getRemoteKeys());
// mdo.writeByte(f.getAutoSendInterval());
}
public void visit
(ReadMotorMixerRequestFrame f
) throws IOException {
writeByte
('n');
}
public void visit
(WriteMotorMixerRequestFrame f
) throws IOException {
writeByte
('m');
base64OutputStream.
writeByte(f.
getMotorMixerVersionNumber());
base64OutputStream.
writeByte(f.
getDataLength());
base64OutputStream.
writeChars(f.
getName());
for(int i=
0; i
<f.
getMatrix().
length; i++
) {
int[] row = f.
getMatrix()[i
];
for(int j=
0; j
<row.
length; j++
) {
base64OutputStream.
writeSignedByte(row
[j
]);
//System.out.print(row[j] + " ");
}
//System.out.println();
}
/*
for(int i=0; i<f.getOppositeMotors().length; i++) {
base64OutputStream.writeByte(f.getOppositeMotors()[i]);
}
*/
}
public void visit
(ReadVariablesRequestFrame f
) throws IOException {
writeByte
('x');
}
public void visit
(ExternalControlRequestFrame f
) throws IOException {
writeByte
('b');
base64OutputStream.
writeByte(f.
getDigital()[0]);
base64OutputStream.
writeByte(f.
getDigital()[1]);
base64OutputStream.
writeByte(f.
getRemoteButtons());
base64OutputStream.
writeByte(f.
getPitch());
base64OutputStream.
writeByte(f.
getRoll());
base64OutputStream.
writeByte(f.
getYaw());
base64OutputStream.
writeByte(f.
getThrottle());
base64OutputStream.
writeByte(f.
getHeight());
base64OutputStream.
writeByte(f.
getCommand());
base64OutputStream.
writeByte(f.
getFrameNum());
base64OutputStream.
writeByte(f.
getArgument());
}
public void visit
(ReadExternalControlRequestFrame f
) throws IOException {
writeByte
('g');
}
public void visit
(LoopbackTestRequestFrame f
) throws IOException {
writeByte
('0');
base64OutputStream.
writeByte(f.
getByte());
base64OutputStream.
writeWord(f.
getWord());
base64OutputStream.
writeChars(f.
getChararray());
}
public void visit
(ReadParamSetRequestFrame f
) throws IOException {
writeByte
('q');
base64OutputStream.
writeByte(f.
getConfigurationSetNumber());
}
public void visit
(WriteParamSetRequestFrame f
) throws IOException {
writeByte
('s');
base64OutputStream.
writeByte(f.
getConfigurationSetNumber());
base64OutputStream.
writeByte(f.
getConfigurationVersionNumber());
base64OutputStream.
writeByte(f.
getDataLength() +
12);
base64OutputStream.
writeBytes(f.
getData());
base64OutputStream.
writeChars(f.
getName());
}
public void visit
(SetCompassHeadingRequestFrame f
) throws IOException {
writeByte
('K');
}
public void visit
(CompassHeadingRequestFrame f
) throws IOException {
writeByte
('w');
}
public void visit
(OSDDataRequestFrame f
) throws IOException {
writeByte
('o');
base64OutputStream.
writeByte(f.
getAutoSendInterval());
}
public void visit
(ReadIMUConfigurationRequestFrame f
) throws IOException {
writeByte
('i');
}
public void visit
(WriteIMUConfigurationRequestFrame f
) throws IOException {
writeByte
('j');
base64OutputStream.
writeByte(f.
getConfigurationVersionNumber());
base64OutputStream.
writeByte(f.
getDataLength());
base64OutputStream.
writeBytes(f.
getData());
}
public void visit
(ReadRCChannelsRequestFrame f
) throws IOException {
writeByte
('p');
}
}