/*
 * Nintendo 64 Controller to Arduino sketch
 * By Waffle (http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?action=viewprofile;username=Waffle)
 * See this Arduino forum thread for info: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1261980415
 * 
 * Connect N64 controller +3.3V line to Arduino 3.3V line
 * Connect N64 controller ground line to Arduino ground
 * Connect N64 controller data line to an Arduino I/O pin with a 1K pull-up resistor to 3.3V line 
 *
 * Set Arduino pin used for data line in the follow three #defines:
 * Example for Arduino pin 9: N64_DATA_DDR = DDRB, N64_DATA_PIN = PINB, N64_DATA_PIN_NO = PINB1
 */

#define N64_DATA_DDR    DDRB
#define N64_DATA_PIN	PINB
#define N64_DATA_PIN_NO	PINB1

#include <util/delay.h>

/* based off the 'N64/Gamecube controller to USB adapter' by Raphaël Assénat (http://www.raphnet.net/electronique/gc_n64_usb/index_en.php)
 * modifications/improvements:
 * - adjusted timing for 16 MHz
 * - support writing variable length data
 * - receive variable length data directly packed into destination buffer
 */
uint8_t n64cmd(uint8_t rxdata[], uint8_t rxlen, uint8_t txdata[], uint8_t txlen) {
	uint8_t num = 0;
	uint8_t oldSREG = SREG;

	cli();
	asm volatile(
"nextByte%=:						\n"
"			cpi %[txlen], 0			\n" // 1
"			breq done%=				\n" // 1
"			dec %[txlen]			\n" // 1
"			ld r16, z+				\n" // 2
"			ldi r17, 0x80			\n" // 1
"nextBit%=:			 				\n"
"			mov r18, r16			\n" // 1
"			and r18, r17			\n" // 1
"			breq send0%=			\n" // 2
"			nop						\n"

// 1us low, 1us high
"send1%=:							\n"
"			sbi %[ddr], %[pinNo]	\n" // 2
"				nop\nnop\nnop\nnop	\n" // 4
"				nop\nnop\nnop\nnop	\n" // 4
"				nop\nnop\nnop\nnop	\n" // 4
"				nop\nnop			\n" // 2
"			cbi %[ddr], %[pinNo]	\n" // 2
"				ldi r19, 11			\n"	// 1
"lp1%=:			dec r19				\n"	// 1
"				brne lp1%=			\n"	// 2
"			lsr r17					\n" // 1
"			breq nextByte%=			\n" // 1
"				nop\nnop\nnop\nnop	\n" // 4
"				nop					\n" // 1
"			rjmp nextBit%=			\n" // 2

// 3us low, 1us high
"send0%=:	sbi %[ddr], %[pinNo]	\n"	// 2
"				ldi r19, 15			\n"	// 1
"lp0%=:			dec r19				\n"	// 1
"				brne lp0%=			\n"	// 2
"				nop					\n" // 1
"          	cbi %[ddr], %[pinNo]	\n" // 2
"				nop					\n" // 1
"			lsr r17					\n" // 1
"			breq nextByte%=			\n" // 1
"				nop\nnop\nnop\nnop	\n" // 4
"				nop					\n" // 1
"			rjmp nextBit%=			\n" // 2

// finished sending, sync up to the stop bit time
"done%=:							\n"
"			nop\nnop\nnop\nnop		\n"	// 4
"			nop\nnop\nnop			\n" // 3

// stop bit
"          	sbi %[ddr], %[pinNo]	\n" // 2
"				nop\nnop\nnop\nnop	\n" // 4
"				nop\nnop\nnop\nnop	\n" // 4
"				nop\nnop\nnop\nnop	\n" // 4
"				nop\nnop			\n" // 2
"			cbi %[ddr], %[pinNo]	\n"

// stop now if there's nothing to receive
"			cpi %[rxlen], 0			\n" // 1
"			breq end%=				\n" // 1

// receiving
"			clr r18					\n" // 1  current byte
"			ldi	r17, 0x80			\n" // 1  current bit
"st%=:								\n"
"			ldi r16, 0xff			\n" // 1  setup timeout
"waitFall%=:						\n"
"			dec r16					\n" // 1
"			breq end%=				\n" // 1
"			sbic %[pin], %[pinNo]	\n" // 2
"			rjmp waitFall%=			\n"

// wait about 2us to check the state
"			nop\nnop\nnop\nnop		\n" // 4
"			nop\nnop\nnop\nnop		\n" // 4
"			nop\nnop\nnop\nnop		\n" // 4
"			nop\nnop\nnop\nnop		\n" // 4
"			nop\nnop\nnop\nnop		\n" // 4
"			nop\nnop\nnop\nnop		\n" // 4
"			nop\nnop\nnop\nnop		\n" // 4

"			sbic %[pin], %[pinNo]	\n" // 2
"			or r18, r17				\n"
"			lsr r17					\n" // 1
"			brne nextRxBit%=		\n" // 2

"nextRxByte%=:						\n"
"			st x+, r18				\n" // 2  store the value
"			inc %[num]				\n" // 1  increase number of received bytes
"			cp %[rxlen], %[num]		\n" // 1  check for finish
"			breq end%=				\n" // 1
"			clr r18					\n" // 1
"			ldi	r17, 0x80			\n" // 1

"nextRxBit%=:						\n"
"			ldi r16, 0xff			\n" // 1  setup timeout
"waitHigh%=:						\n"
"			dec r16					\n" // 1  decrement timeout
"			breq end%=				\n" // 1  handle timeout condition
"			sbis %[pin], %[pinNo]	\n" // 2
"			rjmp waitHigh%=			\n"
"			rjmp st%=				\n" // 2

"end%=:								\n"
			: [num] "=r"(num)
			: [ddr] "I"(_SFR_IO_ADDR(N64_DATA_DDR)), [pin] "I"(_SFR_IO_ADDR(N64_DATA_PIN)), [pinNo] "I"(N64_DATA_PIN_NO),
				[rxdata] "x"(rxdata), [rxlen] "r"(rxlen),
				[txdata] "z"(txdata), [txlen] "r"(txlen), "0"(num)
			: "r16", "r17", "r18", "r19"
			);
	SREG = oldSREG;

	_delay_us(100); // some commands leave the controller unresponsive for a while

	return num;
}

uint8_t command[40];
uint8_t result[40];

void setup() {
  Serial.begin(115200);
  command[0] = 0x00;
  n64cmd(result, 3, command, 1);
  // logiquement, faudrait faire des vrais vérifs là maintenant, genre est-ce que le kit vibration est branché, etc…
  // initialisation du rumble pak
  command[0] = 0x03;
  command[1] = 0x80;
  command[2] = 0x01;
  memset(command + 3, 0x80, 32);
  n64cmd(result, 1, command, 35);
  Serial.println(result[0], HEX);
}

void loop() {
  // vibration activée
  command[0] = 0x03;
  command[1] = 0xC0;
  command[2] = 0x1B;
  memset(command + 3, 0x01, 32);
  
  n64cmd(result, 1, command, 35);
  
  delay(1000);
  
  // vibration désactivée
  memset(command + 3, 0x00, 32);
  
  n64cmd(result, 1, command, 35);
  
  delay(1000);
}