Subversion Repositories Projects

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.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());

        }

}