#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <sys/time.h>
#include <errno.h>
#include <unistd.h>
#include <zlib.h>
#include <wiringPiSPI.h>

static const int CHANNEL = 0;
const uint8_t state = 0xCF;
const uint8_t YM_CS = ( 1 << 0 );
const uint8_t YM_IC = ( 1 << 1 );
const uint8_t YM_RD = ( 1 << 2 );
const uint8_t YM_WR = ( 1 << 3 );
const uint8_t YM_A0 = ( 1 << 4 );
const uint8_t YM_A1 = ( 1 << 5 );
const uint8_t SN_CS = ( 1 << 6 );
const uint8_t SN_WE = ( 1 << 7 );

uint32_t getValue( uint8_t *buf, uint32_t i ) {
  return buf[ i ] + ( buf[ i + 1 ] << 8 ) + ( buf[ i + 2 ] << 16 ) + ( buf[ i + 3 ] << 24 );
}

void pushToChips( uint8_t addr, uint8_t data ) {
	char buf[2] = { addr, data };
  wiringPiSPIDataRW( CHANNEL, buf, 2 );
}
   

void playSN( uint8_t data ) {
   pushToChips((( state & ~( SN_CS )) & ~( SN_WE )), data );
   pushToChips( state, data );
}

void playYM( uint8_t port, uint8_t addr, uint8_t data ) {
	uint8_t newState = state;
	if ( port == 0 )
		newState = ((newState & ~( YM_CS )) & ~( YM_A0 )) & ~( YM_A1 );
	else
		newState = ((newState & ~( YM_CS )) & ~( YM_A0 )) | ( YM_A1 );
	newState &= ~( YM_WR );
	pushToChips( newState, addr );
	
	newState |= YM_WR;
	pushToChips( newState, addr );
	
	newState = ( newState | YM_A0 );
	pushToChips( newState, data );
	
	newState &= ~( YM_WR );
	pushToChips( newState, data );
	
	newState = newState | YM_WR;
	pushToChips( newState, data );
	
	pushToChips( state, data );
}


int main() {
  char filename[] = "Sonic the Hedgehog - Staff Roll.vgz";
  int fd = wiringPiSPISetup(CHANNEL, 100000000);
  if ( fd < 0 ) {
    printf("Pi SPI Setup Failed!\n");
    return (1);
  }
  FILE *vgzFile = fopen( filename, "rb" );
  if ( vgzFile == NULL ) {
    printf("Couldn't open input file for reading!\n");
    return (1);
  }
  fseek( vgzFile, -4, SEEK_END );
  uLongf vgmLen = ( uint32_t )getc( vgzFile ) + (( uint32_t )getc( vgzFile ) << 8 ) + (( uint32_t )getc( vgzFile ) << 16 ) + (( uint32_t )getc( vgzFile ) << 24 );
  uint8_t *vgmBuf = ( uint8_t* )malloc( vgmLen );
  gzFile vgzFile2 = gzopen( filename, "rb" );
  gzread( vgzFile2, vgmBuf, vgmLen );
  if ( vgmBuf[0] != 'V' || vgmBuf[1] != 'g' || vgmBuf[2] != 'm' ) {
    printf("Uncompressed file does not have valid VGM header!\n");
    return (1);
  }
  uint32_t ident = getValue( vgmBuf, 0 );
  uint32_t eofOffset = getValue( vgmBuf, 4 );
  uint32_t version = getValue( vgmBuf, 8 );
  uint32_t sn76489Clock = getValue( vgmBuf, 0x0C );
  uint32_t ym2413Clock = getValue( vgmBuf, 0x10 );
  uint32_t gd3Offset = getValue( vgmBuf, 0x14 ) + 0x14;
  uint32_t numSamples = getValue( vgmBuf, 0x18 );
  uint32_t loopOffset = getValue( vgmBuf, 0x1C );
  uint32_t loopSamples = getValue( vgmBuf, 0x20 );
  uint32_t rate = getValue( vgmBuf, 0x24 );
  uint32_t sn76489feedback = (( uint32_t )vgmBuf[ 0x28 ] << 8 ) + vgmBuf[ 0x29 ];
  uint32_t sn76489srw = vgmBuf[ 0x2A ];
  uint32_t sn76489f = vgmBuf[ 0x2B ];
  uint32_t ym2612Clock = getValue( vgmBuf, 0x2C );
  uint32_t ym2151Clock = getValue( vgmBuf, 0x30 );
  uint32_t vgmDataOffset = getValue( vgmBuf, 0x34 );
  if ( vgmDataOffset == 0x0C || vgmDataOffset == 0 ) vgmDataOffset = 0x40;
  // There's a lot more to add... todo haha
  char gd3String[5] = { vgmBuf[ gd3Offset ], vgmBuf[ gd3Offset + 1 ], vgmBuf[ gd3Offset + 2 ], vgmBuf[ gd3Offset + 3 ], '\0' };
  uint32_t gd3Version = getValue( vgmBuf, gd3Offset + 4 );
  uint32_t gd3Len = getValue( vgmBuf, gd3Offset + 8 );
  uint32_t gd3Pos = gd3Offset + 12;
  uint32_t gd3End = gd3Pos + gd3Len;
  uint32_t gd3Indices[ 15 ];
  uint8_t gd3Index = 0;
  for ( gd3Pos; gd3Pos < gd3End; gd3Pos ) {
    uint8_t byte1 = vgmBuf[ gd3Pos++ ];
    uint8_t byte2 = vgmBuf[ gd3Pos++ ];
    if ( byte1 == 0 && byte2 == 0 ) gd3Indices[ gd3Index++ ] = gd3Pos - 2;
  }
  uint32_t trackNameEnStringLen = ( gd3Indices[ 0 ] - ( gd3Offset + 12 )) / 2;
  uint32_t trackNameJpStringLen = gd3Indices[ 1 ] - gd3Indices[ 0 ];
  uint32_t gameNameEnStringLen = ( gd3Indices[ 2 ] - gd3Indices[ 1 ]) / 2;
  uint32_t gameNameJpStringLen = ( gd3Indices[ 3 ] - gd3Indices[ 2 ]);
  uint32_t systemNameEnStringLen = ( gd3Indices[ 4 ] - gd3Indices[ 3 ]) / 2;
  uint32_t systemNameJpStringLen = ( gd3Indices[ 5 ] - gd3Indices[ 4 ]);
  uint32_t authorNameEnStringLen = ( gd3Indices[ 6 ] - gd3Indices[ 5 ]) / 2;
  uint32_t authorNameJpStringLen = ( gd3Indices[ 7 ] - gd3Indices[ 6 ]);
  uint32_t dateStringLen = ( gd3Indices[ 8 ] - gd3Indices[ 7 ]) / 2;
  uint32_t ripperStringLen = ( gd3Indices[ 9 ] - gd3Indices[ 8 ]) / 2;
  uint32_t notesStringLen = ( gd3Indices[ 10 ] - gd3Indices[ 9 ]) / 2;
  char *trackNameEnString = ( char* )malloc( trackNameEnStringLen );
  char *trackNameJpString = ( char* )malloc( trackNameJpStringLen );
  char *gameNameEnString = ( char* )malloc( gameNameEnStringLen );
  char *gameNameJpString = ( char* )malloc( gameNameJpStringLen );
  char *systemNameEnString = ( char* )malloc( systemNameEnStringLen );
  char *systemNameJpString = ( char* )malloc( systemNameJpStringLen );
  char *authorNameEnString = ( char* )malloc( authorNameEnStringLen );
  char *authorNameJpString = ( char* )malloc( authorNameJpStringLen );
  char *dateString = ( char* )malloc( dateStringLen );
  char *ripperString = ( char* )malloc( ripperStringLen );
  char *notesString = ( char* )malloc( notesStringLen );
  int i = 0;
  for ( i = 0; i < trackNameEnStringLen; i++ ) trackNameEnString[ i ] = vgmBuf[ ( gd3Offset + 12 ) + ( i * 2 )];
  strncpy( trackNameJpString, vgmBuf + gd3Indices[ 0 ] + 1, sizeof( trackNameJpString ));
  for ( i = 0; i < gameNameEnStringLen; i++ ) gameNameEnString[ i ] = vgmBuf[ ( gd3Indices[ 1 ] ) + ( i * 2 ) + 2];
  strncpy( gameNameJpString, vgmBuf + gd3Indices[ 2 ], sizeof( gameNameJpString ));
  for ( i = 0; i < systemNameEnStringLen; i++ ) systemNameEnString[ i ] = vgmBuf[ gd3Indices[ 3 ] + ( i * 2 ) + 2];
  strncpy( systemNameJpString, vgmBuf + gd3Indices[ 4 ], sizeof( systemNameJpString ));
  for ( i = 0; i < authorNameEnStringLen; i++ ) authorNameEnString[ i ] = vgmBuf[ gd3Indices[ 5 ] + ( i * 2 ) + 2];
  strncpy( authorNameJpString, vgmBuf + gd3Indices[ 6 ], sizeof( authorNameJpString ));
  for ( i = 0; i < dateStringLen; i++ ) dateString[ i ] = vgmBuf[ gd3Indices[ 7 ] + ( i * 2 ) + 2];
  for ( i = 0; i < ripperStringLen; i++ ) ripperString[ i ] = vgmBuf[ gd3Indices[ 8 ] + ( i * 2 ) + 2];
  for ( i = 0; i < notesStringLen; i++ ) notesString[ i ] = vgmBuf[ gd3Indices[ 9 ] + ( i * 2 ) + 2];


  printf( "Reading %s\n", filename );
  printf( "Uncompressed Size: %u\n", vgmLen );
  printf( "Ident: %c%c%c%c\n", vgmBuf[0], vgmBuf[1], vgmBuf[2], vgmBuf[3] );
  printf( "EOF Offset: %#08x\n", eofOffset );
  printf( "Version: %d%d%d%d\n", vgmBuf[ 11 ], vgmBuf[ 10 ], vgmBuf[ 9 ], vgmBuf[ 8 ] );
  printf( "SN76489 Clock: %u\n", sn76489Clock );
  printf( "YM2413 Clock: %u\n", ym2413Clock );
  printf( "GD3 Offset: %#08x\n", gd3Offset );
  printf( "Number of Samples: %u\n", numSamples );
  printf( "Loop Offset: %#08x\n", loopOffset );
  printf( "Number of Loop Samples: %u\n", loopSamples );
  printf( "Rate: %u\n", rate );
  printf( "SN76489 Feedback: %u\n", sn76489feedback );
  printf( "SN76489 Shift Register Width: %u\n", sn76489srw );
  printf( "SN76489 Flags: %u\n", sn76489f );
  printf( "YM2612 Clock: %u\n", ym2612Clock );
  printf( "YM2151 Clock: %u\n", ym2151Clock );
  printf( "VGM Data Offset: %#08x\n", vgmDataOffset );
  printf( "GD3 Header: %s\n", gd3String );
  printf( "GD3 Version: %d%d%d%d\n", vgmBuf[ gd3Offset + 7 ], vgmBuf[ gd3Offset + 6 ], vgmBuf[ gd3Offset + 5 ], vgmBuf[ gd3Offset + 4 ] );
  printf( "GD3 Length: %u\n", gd3Len );
  printf( "Track Name (EN): %s\n", trackNameEnString );
  printf( "Track Name (JP): %s\n", trackNameJpString );
  printf( "Game name (EN): %s\n", gameNameEnString );
  printf( "Game name (JP): %s\n", gameNameJpString );
  printf( "System name (EN): %s\n", systemNameEnString );
  printf( "System name (JP): %s\n", systemNameJpString );
  printf( "Author name (EN): %s\n", authorNameEnString );
  printf( "Author name (JP): %s\n", authorNameJpString );
  printf( "Date Ripped: %s\n", dateString );
  printf( "Ripped By: %s\n", ripperString );
  printf( "Notes: %s\n", notesString );
  
  uint8_t done = 0;
  uint32_t commandOffsetPosition = vgmDataOffset;
  uint32_t commandPosition = commandOffsetPosition;
  uint32_t pcmOffsetPosition = 0;
  uint32_t pcmPosition = 0;
  while ( !done ) {
    uint32_t addr = 0;
    uint32_t data = 0;
    uint32_t temp = 0;
	uint32_t j = 0;
    uint8_t command = vgmBuf[ commandPosition++ ];
	printf( "command %#08x\n", command);
    switch ( command ) {
      case 0x4F:
        data = vgmBuf[ commandPosition++ ];
        playSN( data );
        break;
      case 0x50:
        data = vgmBuf[ commandPosition++ ];
        playSN( data );
        break;
      case 0x52:
        addr = vgmBuf[ commandPosition++ ];
        data = vgmBuf[ commandPosition++ ];
        playYM( 0, addr, data );
        break;
      case 0x53:
        addr = vgmBuf[ commandPosition++ ];
        data = vgmBuf[ commandPosition++ ];
        playYM( 1, addr, data );
        break;
      case 0x61:
        waitSamples = vgmBuf[ commandPosition++ ];
        waitSamples += ( uint16_t )vgmBuf[ commandPosition++ ] << 8;
        break;
      case 0x62:
        waitSamples = 735;
        break;
      case 0x63:
        waitSamples = 882;
        break;
      case 0x66:
        if ( loopOffset ) {
          commandPosition = loopOffset + vgmDataOffset;
          loopOffset = 0;
		  //cheating a bit, need to work on the loop part
        }
        else done = 1;
        break;
      case 0x67:
        addr = vgmBuf[ commandPosition++ ]; //0x66
        data = vgmBuf[ commandPosition++ ]; //Data Block Type
        if ( data < 0x40 ) {
          for ( j = 0; j < 4; j++ ) temp += ( uint32_t )vgmBuf[ commandPosition++ ]  << ( 8 * j );
          pcmOffsetPosition = commandPosition;
		  commandPosition += temp;
        }
        else {
          printf( "Error: Unsupported PCM Type %#08x", data );
          return( 1 );
        }
        break;
      case 0x70:
      case 0x71:
      case 0x72:
      case 0x73:
      case 0x74:
      case 0x75:
      case 0x76:
      case 0x77:
      case 0x78:
      case 0x79:
      case 0x7A:
      case 0x7B:
      case 0x7C:
      case 0x7D:
      case 0x7E:
      case 0x7F:
        waitSamples = ( command & 0x0F ) + 1;
        break;
      case 0x80:
      case 0x81:
      case 0x82:
      case 0x83:
      case 0x84:
      case 0x85:
      case 0x86:
      case 0x87:
      case 0x88:
      case 0x89:
      case 0x8A:
      case 0x8B:
      case 0x8C:
      case 0x8D:
      case 0x8E:
      case 0x8F:
        waitSamples = ( command & 0x0F );
        addr = 0x2A;
        //waitSamples = ((( command | 0x01 ) - 1 ) & 0x0F ); //super weird but if you try it without this you'll see
        data = vgmBuf[ pcmPosition++ ];
        playYM( 0, addr, data );
        break;
      case 0xE0:
        for ( j = 0; j < 4; j++ ) temp += ((uint32_t)vgmBuf[ commandPosition++ ] << ( 8 * j ));
        pcmPosition = temp + pcmOffsetPosition;
        break;
      default:
        printf( "Unknown Command %#08x", command );
        return( 1 );
        break;
    }
  }
	
  free( vgzFile );
  free( vgzFile2 ); 
  free( vgmBuf );
  printf( "\nDone\n" );
}