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.DCMMatrixRequestFrame;

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(DCMMatrixRequestFrame f) throws IOException {

		writeByte('e');

	}

	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');

	}

}