/*
 * The contents of this file are subject to the Mozilla Public License
 * Version 1.0 (the "License"); you may not use this file except in
 * compliance with the License. You may obtain a copy of the License at
 * http://www.mozilla.org/MPL/
 *
 * Software distributed under the License is distributed on an "AS IS"
 * basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
 * License for the specific language governing rights and limitations
 * under the License.
 *
 * The Initial Developer of this code is David Baum.
 * Portions created by David Baum are Copyright (C) 1998 David Baum.
 * All Rights Reserved.
 */

#include <string.h>
#include <stdio.h>
#include <stdlib.h>

#include "RCX_Link.h"
#include "RCX_Cmd.h"
#include "rcxnub.h"

#define kSerialPortEnv	"RCX_PORT"


#define kMaxTxData	(2*RCX_Link::kMaxCmdLength + 6)
#define kMaxRxData	(kMaxTxData + 2*RCX_Link::kMaxReplyLength + 5)

#define kFragmentChunk	20
#define kFirmwareChunk	200

#define	kDefaultRetryCount 5

#define kFirmwareDelay	250	// milliseconds

#define kNubStart 0x8000

static void DumpData(const UByte *ptr, int length);

static int FindRCXSync(const UByte *data, int length);
static int FindCMSync(const UByte *data, int length);
static RCX_Result VerifyReply(const UByte *data, int length, UByte cmd);
static RCX_Result VerifyFastReply(const UByte *data, int length, UByte cmd);
static int PredictReplyLength(const UByte *data, int length);

// receive states
enum
{
	kReplyState = 0,	// state when rx buffer used for replies
	kIdleState,
	kSync1State,
	kSync2State,
	kDataState
};


RCX_Link::RCX_Link()
{
	fTxData = new UByte[kMaxTxData];
	fRxData = new UByte[kMaxRxData];
	fVerbose = false;
	fSerial = PSerial::NewSerial();
	fTxLastCommand = 0;
	SetTarget(kRCX_RCXTarget);
	fFastMode = false;
}


RCX_Link::~RCX_Link()
{
	Close();
	delete [] fTxData;
	delete [] fRxData;
	delete fSerial;
}


void RCX_Link::SetTarget(RCX_TargetType target)
{
	fTarget = target;
	fPredictive = true;
	fDynamicTimeout = true;
	fRxTimeout = fPredictive ? kMaxTimeout : kMinTimeout;
}


RCX_Result RCX_Link::Open(const char *portName)
{
	// see if an environment variable is set
	if (!portName) portName = getenv(kSerialPortEnv);
	
	// just use the default
	if (!portName) portName = PSerial::GetDefaultName();

	if (! fSerial->Open(portName)) return kRCX_OpenSerialError;
	
	if (fFastMode)
		fSerial->SetSpeed(4800);
	else
		fSerial->SetSpeed(2400, kPSerial_ParityOdd);
	
	if (fTarget == kRCX_CMTarget)
	{
		fSerial->SetDTR(true);
		fSerial->SetRTS(false);
	}
		
	fSynced = false;
	fResult = kRCX_OK;
	
	fRxState = kReplyState;
	return kRCX_OK;
}


void RCX_Link::Close()
{
	fSerial->Close();
}



void RCX_Link::BuildTxData(const UByte *data, int length, bool duplicateReduction)
{
	int i;
	UByte checksum = 0;
	UByte byte;
	
	UByte *ptr = fTxData;
	
	if (fTarget == kRCX_CMTarget)
	{
		// CM sync pattern
		*ptr++ = 0xfe;
		*ptr++ = 0;
		*ptr++ = 0;
		*ptr++ = 0xff;
	}
	else if (fFastMode)
	{
		*ptr++ = 0xff;
	}
	else
	{
		// RCX sync pattern
		*ptr++ = 0x55;
		*ptr++ = 0xff;
		*ptr++ = 0;
	}
	
	// interleaved data & inverse data
	for(i=0; i<length; i++)
	{
		byte = *data++;

		if (i==0)
		{
			if (duplicateReduction && byte==fTxLastCommand)
				byte ^= 8;
			fTxLastCommand = byte;
		}
		
				
		*ptr++ = byte;
		if (!fFastMode)
			*ptr++ = (UByte)~byte;
		checksum += byte;
	}
	
	// checksum
	*ptr++ = checksum;
	if (!fFastMode)
		*ptr++ = (UByte)~checksum;
	
	fTxLength = ptr - fTxData;
}


RCX_Result RCX_Link::Sync()
{
	RCX_Cmd cmd;
	RCX_Result result;

	if (fSynced) return kRCX_OK;
	
	// always start with a ping
	result = Send(cmd.MakePing());
	// if error, try a second time
	if (result == kRCX_ReplyError)
		result = Send(&cmd);
	if (RCX_ERROR(result)) return result;

	// cybermaster requires an unlock also
	if (fTarget == kRCX_CMTarget)
	{
		result = Send(cmd.MakeUnlockCM());
		if (RCX_ERROR(result)) return result;
	}
	else if (fTarget == kRCX_ScoutTarget)
	{
		result = Send(cmd.MakeBoot());
		if (RCX_ERROR(result)) return result;
		
		result = Send(cmd.Set(0x47, 0x80));
		if (RCX_ERROR(result)) return result;
	}

	fSynced = true;
	return kRCX_OK;
}


RCX_Result RCX_Link::SendFragment(RCX_FragmentType type, UByte number, const UByte *data, int length, int total)
{
	RCX_Cmd cmd;
	RCX_Result result;
	
	result = Sync();
	if (RCX_ERROR(result)) return result;
		
	result = Send(cmd.MakeBegin(type, number, (UShort)length));
	if (RCX_ERROR(result)) return result;

	// make sure we have enough room
	
	if (result != 1 ||
		GetReplyByte(0) != 0) return kRCX_MemFullError;	
	
	if (total == 0)
		total = length;
	
	if (total > 0)
		BeginProgress(total);

	result = Download(data, length, kFragmentChunk);
	return result;
}


RCX_Result RCX_Link::GetVersion(ULong &rom, ULong &ram)
{
	RCX_Cmd cmd;
	RCX_Result result;
	UByte reply[8];
	
	result = Sync();
	if (RCX_ERROR(result)) return result;
	
	result = Send(cmd.MakeUnlock());
	if (RCX_ERROR(result)) return result;
	
	if (result != 8) return kRCX_ReplyError;
	
	GetReply(reply,8);
	
	rom =	((ULong)reply[0] << 24) |
			((ULong)reply[1] << 16) |
			((ULong)reply[2] << 8) |
			((ULong)reply[3]);
	
	ram =	((ULong)reply[4] << 24) |
			((ULong)reply[5] << 16) |
			((ULong)reply[6] << 8) |
			((ULong)reply[7]);

	return kRCX_OK;
}


RCX_Result RCX_Link::GetValue(RCX_Value value)
{
	RCX_Cmd cmd;
	RCX_Result result;
	
	result = Sync();
	if (RCX_ERROR(result)) return result;
	
	result = Send(cmd.MakeRead(value));
	if (RCX_ERROR(result)) return result;
	if (result != 2) return kRCX_ReplyError;

	result = (int)GetReplyByte(0) + ((int)GetReplyByte(1) << 8);
	return result;	
}


RCX_Result RCX_Link::GetBatteryLevel()
{
	RCX_Cmd cmd;
	RCX_Result result;

	result = Sync();
	if (RCX_ERROR(result)) return result;
	
	if (fTarget == kRCX_ScoutTarget)
	{
		result = Send(cmd.Set(kRCX_PollMemoryOp, 0x3a, 0x01, 0x01));
		if (result != 1) return kRCX_ReplyError;
		result = (int)GetReplyByte(0) * 109;
	}
	else
	{
		result = Send(cmd.Set(kRCX_BatteryLevelOp));
		if (result != 2) return kRCX_ReplyError;
		result = (int)GetReplyByte(0) + ((int)GetReplyByte(1) << 8);
	}
	
	return result;
}


RCX_Result RCX_Link::SendFirmware(const UByte *data, int length, int start, bool fast)
{
	RCX_Result result;
	bool wasPredictive = fPredictive;
	
	if (fast)
	{
		// send the nub first
		result = _SendFirmware(rcxnub, sizeof(rcxnub), kNubStart, false);
		if (RCX_ERROR(result)) return result;
		
		// switch to fast mode
		fFastMode = true;
		fPredictive = false;
		fSerial->SetSpeed(4800);		
	}
	
	result = _SendFirmware(data, length, start, true);
	
	if (fast)
	{
		fFastMode = false;
		fSerial->SetSpeed(2400, kPSerial_ParityOdd);
		fPredictive = wasPredictive;
	}
	
	return result;
}


RCX_Result RCX_Link::_SendFirmware(const UByte *data, int length, int start, bool progress)
{
	RCX_Cmd cmd;
	RCX_Result result;
	int check;
	
	result = Sync();
	if (RCX_ERROR(result)) return result;
	
	result = Send(cmd.MakeUnlock());
	if (RCX_ERROR(result)) return result;

	result = Send(cmd.Set(kRCX_BootModeOp, 1, 3, 5, 7, 0xb));
	if (RCX_ERROR(result)) return result;
	
	check = 0;
	for(int i=0; i<length && i<0x4c00; i++)
		check += data[i];
	result = Send(cmd.Set(kRCX_BeginFirmwareOp, (UByte)(start), (UByte)(start>>8),
		(UByte)check, (UByte)(check>>8), 0));
	if (RCX_ERROR(result)) return result;

	BeginProgress(progress ? length : 0);
	result = Download(data, length, kFirmwareChunk);
	if (RCX_ERROR(result)) return result;

	// last packet is no-retry with an extra long delay
	// this gives the RCX time to respond and makes sure response doesn't get trampled
	result = Send(cmd.MakeBoot(), false, kFirmwareDelay);

	if (fFastMode)
	{
		// for some reason the response doesn't checksum in fast mode
		// just assume it was ok
		result = kRCX_OK;
	}

	return result;
}


RCX_Result RCX_Link::Download(const UByte *data, int length, int chunk)
{
	RCX_Cmd cmd;
	RCX_Result result;
	UShort seq;
	int remain = length;
	int n;
	
	seq = 1;
	while(remain > 0)
	{
		if (remain <= chunk)
		{
			seq = 0;
			n = remain;
		}
		else
			n = chunk;

		result = Send(cmd.MakeDownload(seq++, data, (UByte)n));
		if (result < 0) return result;
		
		remain -= n;
		data += n;
		if (!IncrementProgress(n)) return kRCX_AbortError;
	}

	return kRCX_OK;
}

RCX_Result RCX_Link::Send1(const UByte *data, int length)
{
	if (length > kMaxCmdLength) return kRCX_RequestError;

	// format the command
	BuildTxData(data, length, false);

	fSerial->Write(fTxData, fTxLength);

	return 0;
}


RCX_Result RCX_Link::Send(const RCX_Cmd *cmd, bool retry, int timeout)
{
	return Send(cmd->GetBody(), cmd->GetLength(), retry, timeout);
}


RCX_Result RCX_Link::Send(const UByte *data, int length, bool retry, int timeout)
{
	if (length > kMaxCmdLength) return kRCX_RequestError;

	// format the command
	BuildTxData(data, length, retry);

	// determine reply length
	fRxExpected = PredictReplyLength(data, length);
	
	// try sending
	int tries = retry ? kDefaultRetryCount : 1;
	int originalTimeout = fRxTimeout;
	bool dynamic = (timeout==0) && fDynamicTimeout && !fPredictive;
	int i;
	
	for(i=0; i<tries; i++)
	{
		fResult = _SendFromTxBuffer(timeout ? timeout : fRxTimeout);
		
		if (!RCX_ERROR(fResult))
		{
			// success
			if (i==0)
			{
				// worked on first try, lets see if we can go faster next time
				if (dynamic && fRxTimeout > kMinTimeout)
				{
					int newTimeout = fRxTimeout - (fRxTimeout / 10);
					if (newTimeout < kMinTimeout)
						newTimeout = kMinTimeout;
					fRxTimeout = newTimeout;
					#ifdef DEBUG_TIMEOUT
						printf("Timeout = %d\n", fRxTimeout);
					#endif
				}
			}
			return fResult;
		}

		// only the second kRCX_IREchoError is catastrophic
		// this is somewhat of a hack - I really should keep track
		// of the echo, but for now this makes sure that a serial
		// level failure on a single packet doesn't kill the entire
		// send
		if (fResult == kRCX_IREchoError && i > 0) break;

		if (dynamic && i>0)
		{
			// increase retry time
			fRxTimeout *= 2;
			if (fRxTimeout > kMaxTimeout) fRxTimeout = kMaxTimeout;
			#ifdef DEBUG_TIMEOUT
				printf("Timeout = %d\n", fRxTimeout);
			#endif
		}
	}
	
	// retries exceeded, restore original timeout and lose the sync
	if (retry)
	{
		if (dynamic)
			fRxTimeout = originalTimeout;
		fSynced = false;
	}
	return fResult;
}


RCX_Result RCX_Link::_SendFromTxBuffer(int timeout)
{
	int offset;
	RCX_Result result;
	
	// drain serial rx buffer
	fSerial->SetTimeout(0);
	while(fSerial->Read(fRxData, kMaxRxData) > 0)
		;

	// send command	
	fSerial->Write(fTxData, fTxLength);
	fSerial->FlushWrite();
	if (fVerbose)
	{
		printf("Tx: ");
		DumpData(fTxData, fTxLength);
	}

	// get the reply
	fSerial->SetTimeout(timeout);
	fRxState = kReplyState;
	fRxLength = 0;
	while(fRxLength < kMaxRxData)
	{
		if (fSerial->Read(fRxData+fRxLength, 1) != 1) break;
		fRxLength++;
		
		// check for predictive replies...doesn't seem to work with firmware download
		if (fPredictive)
		{
			result = FindReply(offset);
			if (result == fRxExpected) break;
		}
	}
	
	if (fVerbose)
	{
		printf("Rx: ");
		DumpData(fRxData, fRxLength);
	}
	
	if (fRxLength == 0)
		return fTarget==kRCX_CMTarget ? kRCX_ReplyError : kRCX_IREchoError;
	
	// find reply
	result = FindReply(offset);
	
	if (!RCX_ERROR(result))
	{
		AdjustReply(offset + 2, result);
	}
	
	return result;
}


RCX_Result RCX_Link::Receive(bool echo)
{	
	if (fRxState == kReplyState)
		SetRxState(kIdleState);
	
	while(true)
	{
		UByte b;
		if (fSerial->Read(&b, 1) != 1)
		{
			RCX_Result result;
			
			if (fRxState == kDataState)
				result = ValidateRxData();
			else
				result = 0;
							
			SetRxState(kIdleState);
			return result;
		}
		
		if (echo) fSerial->Write(&b, 1);
		
		ProcessRxByte(b);
	}
}


int RCX_Link::ValidateRxData()
{
	// validate complements
	int count = fRxLength / 2;
	for(int i=0; i<count; ++i)
	{
		UByte d = fRxData[2*i];
		
		if (d + fRxData[2*i+1] != 0xff) return 0;
		
		fRxData[i] = d;
	}
	
	// compute checksum
	--count;
	UByte checksum = 0;
	for(int i=0; i<count; ++i)
		checksum += fRxData[i];
	
	if (checksum != fRxData[count]) return 0;
	
	return count;
}


void RCX_Link::ProcessRxByte(UByte b)
{
	int newState = fRxState;
	bool reset = false;
			
	switch(fRxState)
	{
		case kIdleState:
			if (b == 0x55)
				newState = kSync1State;
			break;
		case kSync1State:
			if (b == 0xff)
				newState = kSync2State;
			else
				reset = true;
			break;
		case kSync2State:
			if (b == 0x00)
			{
				fRxLength = 0;
				newState = kDataState;
			}
			else
				reset = true;
			break;
		case kDataState:
			if (fRxLength < kMaxRxData)
			{
				fRxData[fRxLength++] = b;
			}
			break;
	}
	
	if (reset)
		newState = (b==0x55) ? kSync1State : kIdleState;

	SetRxState(newState);
}
		

void RCX_Link::SetRxState(int state)
{
	if (fRxState == state) return;
	
	if (state == kIdleState)
		fSerial->SetTimeout(200);
	else if (fRxState == kIdleState)
		fSerial->SetTimeout(30);

	fRxState = state;
}



RCX_Result RCX_Link::FindReply(int &offset)
{
	RCX_Result result;
	
	offset = 0;
	while(1)
	{
		if (fTarget == kRCX_CMTarget)
			result = FindCMSync(fRxData + offset, fRxLength - offset);
		else
			result = FindRCXSync(fRxData + offset, fRxLength - offset);
				
		if (!result) return kRCX_ReplyError;
	
		offset += result;
		
		if (fFastMode)
			result = VerifyFastReply(fRxData + offset, fRxLength - offset, fTxLastCommand);
		else
			result = VerifyReply(fRxData + offset, fRxLength - offset, fTxLastCommand);

		if (result >= 0) return result;
	}
}


void RCX_Link::AdjustReply(int offset, RCX_Result length)
{
	UByte *src = fRxData + offset;
	UByte *dst = fRxData;
	
	while(length>0)
	{
		*dst++ = *src++;
		if (!fFastMode) src++;
		length--;
	}
}


RCX_Result VerifyReply(const UByte *data, int length, UByte cmd)
{
	UByte checksum;
	const UByte *ptr;
	const UByte *end;
	const UByte *match;
	
	// always need a cmd and a checksum
	if (length < 4) return kRCX_ReplyError;
	
	// check the cmd
	if ((data[0] != (~cmd & 0xff)) ||
		(data[1] != cmd)) return kRCX_ReplyError;

	ptr = data + 2;
	end = data + length - 1;
	checksum = data[0];
	match = nil;
	
	while(ptr < end)
	{
		if (ptr[0] != (~ptr[1] & 0xff)) break;
		
		if (ptr[0] == checksum)
			match = ptr;
		
		checksum += ptr[0];
		ptr += 2;
	}
	
	if (!match) return kRCX_ReplyError;
	
	return ((match - data) / 2) - 1;
}


RCX_Result VerifyFastReply(const UByte *data, int length, UByte cmd)
{
	UByte checksum;
	const UByte *ptr;
	const UByte *end;
	const UByte *match;
	
	// always need a cmd and a checksum
	if (length < 2) return kRCX_ReplyError;
	
	// check the cmd
	if ((data[0] != (~cmd & 0xff))) return kRCX_ReplyError;

	ptr = data + 1;
	end = data + length;
	checksum = data[0];
	match = nil;
	
	while(ptr < end)
	{
		if (ptr[0] == checksum)
			match = ptr;
		
		checksum += ptr[0];
		ptr ++;
	}
	
	if (!match) return kRCX_ReplyError;
	
	return ((match - data)) - 1;
}


RCX_Result	RCX_Link::GetReply(UByte *data, int maxLength)
{
	if (fResult < 0) return fResult;
	
	RCX_Result length;
	const UByte *src = fRxData;
	
	for(length=0; (length<maxLength) && (length<fResult); length++)
	{
		*data++ = *src++;
	}
	
	return length;
}


void RCX_Link::BeginProgress(int total)
{
	fDownloadTotal = total;
	fDownloadSoFar = 0;
}


bool RCX_Link::IncrementProgress(int delta)
{
	fDownloadSoFar += delta;
	return fDownloadTotal ? DownloadProgress(fDownloadSoFar, fDownloadTotal) : true;
}


bool RCX_Link::DownloadProgress(int /* soFar */, int /* total */)
{
	return true;
}


int FindCMSync(const UByte *data, int length)
{
	const UByte *end = data + length;
	const UByte *ptr;

	for(ptr=data; ptr<end; ptr++)
		if (*ptr == 0xff) return ptr-data+1;

	return 0;
}


int FindRCXSync(const UByte *data, int length)
{
	const UByte *end = data + length - 2;
	const UByte *ptr;
	
	for(ptr=data; ptr<end; ptr++)
	{
		if (ptr[0]==0x55 &&
			ptr[1]==0xff &&
			ptr[2]==0x00) return ptr-data+3;
	}
	
	return 0;
}


int PredictReplyLength(const UByte *data, int /* length */)
{
	switch(data[0] & 0xf7)
	{
		case 0x10:	// ping
		case 0x50:	// stop all
		case 0x40:	// delt all
		case 0x70:	// dels all
		case 0x51:	// plays
		case 0x65:
			return 0;
		case 0x25:	// __task
		case 0x45:	// __dl
		case 0x35:
		case 0x75:
			return 1;
		case 0x30:	// pollb
			return 2;
		case 0x15:
			return 8;
		case 0xa5:
			return 25;
		default:
			return 1000;	// ridiculous large number that will never happen
	}
}



void DumpData(const UByte *ptr, int length)
{
	int i;
	
	for(i=0; i<length; i++)
	{
		printf("%02x ", *ptr++);
//		if ((i%16)==15) putchar('\n');
	}

	putchar('\n');
}