//
//  WiiFlashPacket.m
//  WiimoteServer
//
//  Created by hiko on 07/05/04.
//  Copyright 2007 __MyCompanyName__. All rights reserved.
//

#import "WiiFlashPacket.h"

static char WIIFLASH_CODE[] = {
	0x02,
	0x03,
	0x00,
	0x01,
	0x05,
	0x06,
	0x04,
	0x07,
	0x08,
	0x0a,
	0x09,
	
	0x0b,
	0x0c
};


@implementation WiiFlashPacket

- init {

	if ((self = [super init])) {
		int i;
		for (i=0; i<16; i++) {
			buttonState[i] = NO;
		}
		accX = accY = accZ = 0.0;
		
		hasNunchuk = NO;
		nunchukButtonState[0] = NO;
		nunchukButtonState[1] = NO;
		stickX = stickY = 0.0;
		nunchukAccX = nunchukAccY = nunchukAccZ = 0.0;
		
		calib.accX_zero = calib.accY_zero = calib.accZ_zero = 128;
		calib.accX_1g= calib.accY_1g = calib.accZ_1g = 156;
		
		ir1found = NO;
		ir2found = NO;
	}
	
	return self;
}

- (void) setButtonState:(WiiButtonType)button isPressed:(BOOL)isPressed
{
	if (button < 16) {
		buttonState[WIIFLASH_CODE[button]] = isPressed;
	} else {
	}
}

- (void) setAcceleration:(WiiAccelerationSensorType)type accX:(unsigned char)ax accY:(unsigned char)ay accZ:(unsigned char)az
{
	switch (type) {
		case WiiRemoteAccelerationSensor:
			accX = (float)(ax - calib.accX_zero) / (calib.accX_1g - calib.accX_zero);
			accY = (float)(ay - calib.accY_zero) / (calib.accY_1g - calib.accY_zero);
			accZ = (float)(az - calib.accZ_zero) / (calib.accZ_1g - calib.accZ_zero);
			break;
	}
	//NSLog(@"%f,%f,%f", accX,accY,accZ);
}

- (void) setIRRawData:(IRData[4])irData
{
	if (irData[0].x == 0x3ff && irData[0].y == 0x3ff) {
		ir1found = NO;
	} else {
		ir1found = YES;
		irX1 = (float)irData[0].x / 0x3ff * 2 - 1;
		irY1 = (float)irData[0].y / 0x2ff * 2 - 1;
	}

	if (irData[1].x == 0x3ff && irData[1].y == 0x3ff) {
		ir2found = NO;
	} else {
		ir2found = YES;
		irX2 = (float)irData[1].x / 0x3ff * 2 - 1;
		irY2 = (float)irData[1].y / 0x2ff * 2 - 1;
	}
}

- (NSData *) currentStateAsData {
	unsigned char data[57];
	
	memset(data, 0, 57);
	
	unsigned short state = 0;
	int i;
	for (i=0; i<16; i++) {
		if (buttonState[i]) {
			state |= (0x01 << (0x0f - i));
		}
	}
	*(unsigned short *)(data + 1) = CFSwapInt16HostToBig(state);
	*(CFSwappedFloat32 *)(data + 3) = CFConvertFloatHostToSwapped(accX);
	*(CFSwappedFloat32 *)(data + 7) = CFConvertFloatHostToSwapped(accY);
	*(CFSwappedFloat32 *)(data + 11) = CFConvertFloatHostToSwapped(accZ);
	
	*(unsigned char *)(data + 37) = ir1found ? 1 : 0;
	if (ir1found) {
		*(CFSwappedFloat32 *)(data + 38) = CFConvertFloatHostToSwapped(irX1);
		*(CFSwappedFloat32 *)(data + 42) = CFConvertFloatHostToSwapped(irY1);
	}
	*(unsigned char *)(data + 46) = ir2found ? 1 : 0;
	if (ir2found) {
		*(CFSwappedFloat32 *)(data + 47) = CFConvertFloatHostToSwapped(irX2);
		*(CFSwappedFloat32 *)(data + 51) = CFConvertFloatHostToSwapped(irY2);
	}

	return [NSData dataWithBytes:data length:57];
}

- (void) setCalibData:(WiiAccCalibData)data forType:(WiiAccelerationSensorType)type
{
	calib = data;
}

- (void) dump
{
	NSData *data = [self currentStateAsData];
	char *ptr = (char *)[data bytes];
	
	int n = [data length];
	int i;
	for (i=0; i<n; i++) {
		NSLog(@"%02d(0x%02x): 0x%02x", i, i, *(ptr + i));
	}
}

@end
