#include <SPI.h>
#include <SdFat.h>
#include <SPIFlash.h>
#include "wiring_private.h" // pinPeripheral() function
#define ENABLE_EXTENDED_TRANSFER_CLASS 1
#define CPU_HZ 48000000
#define TIMER_PRESCALER_DIV 16

#define MANID    0x90
#define PAGEPROG     0x02
#define READDATA     0x03
#define FASTREAD   0x0B
#define WRITEDISABLE 0x04
#define READSTAT1    0x05
#define READSTAT2  0x35
#define WRITEENABLE  0x06
#define SECTORERASE  0x20
#define BLOCK32ERASE 0x52
#define CHIPERASE    0x60
#define SUSPEND      0x75
#define ID           0x90
#define RESUME       0x7A
#define JEDECID      0x9f
#define RELEASE      0xAB
#define POWERDOWN    0xB9
#define BLOCK64ERASE 0xD8

//PORTA
#define CARD_CS ( 1 << 11 ) // 0
#define MISO ( 1 << 12 ) // 0
#define SN_WE ( 1 << 13 ) // 1
#define YM_IC ( 1 << 14 ) // 1
#define YM_A0 ( 1 << 15 ) // 0
#define YM_A1 ( 1 << 16 ) // 0
#define YM_RD ( 1 << 17 ) // 1
#define YM_WR ( 1 << 18 ) // 1
#define SN_CS ( 1 << 19 ) // 1
#define YM_CS ( 1 << 20 ) // 1
#define MEM_CS ( 1 << 21 ) // 1
#define SDA ( 1 << 22 ) // 1
#define SCL ( 1 << 23 ) // 1
#define SN_RDY ( 1 << 27 ) // 1
#define YM_IRQ ( 1 << 28 ) // 1

//PORTB
#define BACKBUTTON ( 1 << 2 ) // 1
#define PLAYBUTTON ( 1 << 3 ) // 1
#define OSC_IN ( 1 << 8 ) // 0
#define NEXTBUTTON ( 1 << 9 ) // 1
#define MOSI ( 1 << 10 ) // 0
#define SCK ( 1 << 11 ) // 0
#define MIDI_IN ( 1 << 23 ) // 1

//#define DEBUG
#ifdef DEBUG
#define DEBUG_PRINT(...)    Serial.print(__VA_ARGS__)
#define DEBUG_PRINTLN(...)  Serial.println(__VA_ARGS__)
#else
#define DEBUG_PRINT(...)
#define DEBUG_PRINTLN(...)
#endif
#define PCMSIZE 20000
byte pcmData[ PCMSIZE ];

int numFiles = 0;
SdFat SD;
File vgmFile;
StdioStream vgmStream;
SPIClass mySPI ( &sercom1, 12, 13, 11, SPI_PAD_0_SCK_1, SERCOM_RX_PAD_3 );
volatile byte done = 0;
uint32_t fileSize;
byte readPcmFromFlash = 0;
volatile uint32_t commandPosition;
volatile uint32_t pcmPosition;

void waitForFlashReady() {
  PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
  SPI.transfer( READSTAT1 );
  byte s = SPI.transfer( 0x00 );
  while ( s & 0x01 ) {
    s = SPI.transfer( 0x00 );
    DEBUG_PRINT( "." );
  }
  PORT->Group[PORTA].OUTSET.reg = MEM_CS;
}

void getNumFiles() {
  SD.vwd()->rewind();
  Serial.println( "Looking for files");
  while (vgmFile.openNext(SD.vwd(), O_READ)) {
    numFiles++;
    vgmFile.close();
  }
}

void writeEnable() {
  PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
  SPI.transfer( WRITEENABLE );
  PORT->Group[PORTA].OUTSET.reg = MEM_CS;
  PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
  SPI.transfer( READSTAT1 );
  byte s = SPI.transfer( 0x00 );
  while ( s & 0x01 ) {
    s = SPI.transfer( 0x00 );
    DEBUG_PRINT( "." );
  }
  PORT->Group[PORTA].OUTSET.reg = MEM_CS;
}

void dumpToPcm() {
  for ( uint32_t addr = 0; addr < fileSize; addr++ ) {
    byte data = readFromFlash( addr );
    uint32_t s = 0;
    if ( data == 0x67 ) {
      addr++;
      data = readFromFlash( addr ); //0x66
      if ( data == 0x66 ) {
        addr++;
        data = readFromFlash( addr++ ); //Data Block Type
        for ( uint32_t j = 0; j < 4; j++ ) s += ( uint32_t( readFromFlash( addr++ ) ) << ( 8 * j ));
        if ( s > PCMSIZE ) {
          Serial.println( "PCM Data too large!" );
          readPcmFromFlash = 1;
          return;
        }
        readPcmFromFlash = 0;
        for ( uint32_t j = 0; j < s; j++ ) {
          pcmData[ j ] = readFromFlash( addr++ );
          DEBUG_PRINTLN( "Writing PCM Data to " + String( j, HEX ));
        }
      }
    }
  }
}

void dumpToFlash() {
  uint8_t buf[ 256 ];
  uint32_t addr = 0;
  while ( !vgmStream.feof() ) {
    writeEnable();
    DEBUG_PRINTLN( "Erasing 64K Block Starting At " + String( addr, HEX ));
    PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
    SPI.transfer( BLOCK64ERASE );
    SPI.transfer( addr >> 16 );
    SPI.transfer( addr >> 8 );
    SPI.transfer( 0 );
    PORT->Group[PORTA].OUTSET.reg = MEM_CS;
    waitForFlashReady();
    for ( int page = 0; page < 256; page++ ) {
      for ( int i = 0; i < 256; i++ ) {
        byte data = 0;
        if ( !vgmStream.feof() ) data = vgmStream.getc();
        buf[ i ] = data;
      }
      writeEnable();
      PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
      SPI.transfer( 0x02 ); // Pageprog
      SPI.transfer( addr >> 16 );
      SPI.transfer( addr >> 8 );
      SPI.transfer( addr );
      for (int i = 0; i < 256; i++) {
        SPI.transfer( buf[i] );
        addr++;
      }
      PORT->Group[PORTA].OUTSET.reg = MEM_CS;
      waitForFlashReady();
      DEBUG_PRINTLN( "Wrote to flash page " + String( addr, HEX ));
    }
  }
}

void dumpFlash() {
  PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
  SPI.transfer( 0x03 ); // read
  SPI.transfer( 0x00 );
  SPI.transfer( 0x00 );
  SPI.transfer( 0x00 );
  byte dat = 0;
  for ( long i = 0; i < 0xFFFFFF; i++ ) {
    dat = SPI.transfer( 0x00 );
    DEBUG_PRINTLN( "Reading from flash " + String( SPI.transfer( 0x00 ), HEX ));
  }
  PORT->Group[PORTA].OUTSET.reg = MEM_CS;
}

byte readFromFlash( uint32_t addr ) {
  static uint32_t lastAddr = 0xFFFFFFF0;
  if ( addr != ( lastAddr + 1 )) {
    DEBUG_PRINTLN( "Last Address was " + String( lastAddr, HEX ) + " Changing Address to " + String( addr, HEX ));
    PORT->Group[PORTA].OUTSET.reg = MEM_CS;
    PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
    SPI.transfer16(( 0x03 << 8 ) | (addr >> 16)); // read
    SPI.transfer16( addr );
  }
  lastAddr = addr;
  byte data = SPI.transfer( 0x00 );
  return data;
}

void playSN( uint8_t data ) {
  PORT->Group[PORTA].OUTCLR.reg = SN_CS | ((uint32_t)~(data) << 3 ) & 0x7F8;
  PORT->Group[PORTA].OUTSET.reg = ((uint32_t)data << 3 ) & 0x7F8;
  PORT->Group[PORTA].OUTCLR.reg = SN_WE;
  delayMicroseconds( 7 );
  while ( !(PORT->Group[PORTA].IN.reg & SN_RDY ));
  PORT->Group[PORTA].OUTSET.reg = SN_CS | SN_WE;
}

void playYM( uint8_t port, uint8_t addr, uint8_t data ) {
  PORT->Group[PORTA].OUTCLR.reg = YM_CS | YM_A0 | ((uint32_t)~(addr) << 3 ) & 0x7F8;
  PORT->Group[PORTA].OUTSET.reg = ((uint32_t)addr << 3 ) & 0x7F8;
  if ( port ) PORT->Group[PORTA].OUTSET.reg = YM_A1;
  else PORT->Group[PORTA].OUTCLR.reg = YM_A1;
  PORT->Group[PORTA].OUTCLR.reg = YM_WR;
  delayMicroseconds( 1 );
  PORT->Group[PORTA].OUTSET.reg = YM_WR;
  PORT->Group[PORTA].OUTSET.reg = YM_A0 | ((uint32_t)data << 3 ) & 0x7F8;
  PORT->Group[PORTA].OUTCLR.reg = ((uint32_t)~(data) << 3 ) & 0x7F8;
  PORT->Group[PORTA].OUTCLR.reg = YM_WR;
  PORT->Group[PORTA].OUTSET.reg = YM_CS | YM_WR;
}

void silence() {
  playSN( 0x9F ); //4 byte silence sn sequence
  playSN( 0xBF );
  playSN( 0xDF );
  playSN( 0xFF );
  playYM(0, 0x22, 0x00); // LFO off
  playYM(0, 0x27, 0x00); // Note off (channel 0)
  playYM(0, 0x28, 0x01); // Note off (channel 1)
  playYM(0, 0x28, 0x02); // Note off (channel 2)
  playYM(0, 0x28, 0x04); // Note off (channel 3)
  playYM(0, 0x28, 0x05); // Note off (channel 4)
  playYM(0, 0x28, 0x06); // Note off (channel 5)
  playYM(0, 0x2B, 0x00); // DAC off
}

void TC3_Handler(void) {
  TcCount16* TC = (TcCount16*) TC3;
  if (( TC->INTFLAG.bit.MC0 == 1 ) && ( done == 0 )) {
    TC->INTFLAG.bit.MC0 = 1;
    static uint32_t pause = 0;
    uint8_t bufferByte;
    if ( pause > 0 ) {
      pause--;
      DEBUG_PRINTLN( "Pausing for " + String( pause ));
    }
    else {
      byte command = readFromFlash( commandPosition );
      DEBUG_PRINT( "Addr:0x" + String( ( commandPosition ), HEX ) + " Cmd:0x" + String( command, HEX ) + " | " );
      commandPosition++;
      uint32_t addr;
      uint32_t data;
      uint32_t temp;
      uint32_t s = 0;
      switch ( command ) {
        case 0x4F: // should I just write this straight to the SN?
          data = readFromFlash( commandPosition++ );
          DEBUG_PRINTLN( "W - SN? Data 0x" + String( data, HEX ));
          playSN( data );
          break;
        case 0x50:
          data = readFromFlash( commandPosition++ );
          DEBUG_PRINTLN( "W - SN Data 0x" + String( data, HEX ));
          playSN( data );
          break;
        case 0x52:
          addr = readFromFlash( commandPosition++ );
          data = readFromFlash( commandPosition++ );
          DEBUG_PRINTLN( "W - YM 0 Address 0x" + String( addr, HEX ) + " Data 0x" + String( data, HEX ) );
          playYM( 0, addr, data );
          break;
        case 0x53:
          addr = readFromFlash( commandPosition++ );
          data = readFromFlash( commandPosition++ );
          DEBUG_PRINTLN( "W - YM 1 Address 0x" + String( addr, HEX ) + " Data 0x" + String( data, HEX ) );
          playYM( 1, addr, data );
          break;
        case 0x61:
          pause = readFromFlash( commandPosition++ );
          pause += (((uint16_t)readFromFlash( commandPosition++ )) << 8 );
          DEBUG_PRINTLN( "W - Delay " + String( pause ));
          break;
        case 0x62:
          DEBUG_PRINTLN( "W - Delay 735" );
          pause = 735;
          break;
        case 0x63:
          DEBUG_PRINTLN( "W - Delay 882" );
          pause = 882;
          break;
        case 0x66:
          done = 1;
          DEBUG_PRINTLN( "End of Sound Data" );
          break;
        case 0x67:
          temp = readFromFlash( commandPosition++ ); //0x66
          temp = readFromFlash( commandPosition++ ); //Data Block Type
          for ( uint32_t j = 0; j < 4; j++ ) s += ( uint32_t( readFromFlash( commandPosition++ ) ) << ( 8 * j ));
          pcmPosition = commandPosition;
          commandPosition += s;
          DEBUG_PRINTLN( "Data Block Type " + String( temp, HEX ) + " Space Needed: " + s + " Set Command Pointer to " + commandPosition );
          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:
          DEBUG_PRINTLN( "W - Delaying " + String( ( command & 0x0F ) + 1 ));
          pause = ( 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:
          addr = 0x2A;
          if ( readPcmFromFlash ) data = readFromFlash( pcmPosition++ );
          else data = pcmData[ pcmPosition ];
          DEBUG_PRINTLN( "W - YM Play 0x" + String( pcmPosition, HEX ) + " > 0x" + String ( data, HEX ));
          playYM( 0, addr, data );
          pause = (command & 0x0F);
          if ( pause > 0 ) pause--;
          break;
        case 0xE0:
          for ( int j = 0; j < 4; j++ ) s += ( readFromFlash( commandPosition++ ) << ( 8 * ( j )));
          pcmPosition = s; // are these addresses offset relative to the data block's position in memory, or relative to the data block's 0th index?
          DEBUG_PRINTLN( "Seeking PCM Pointer to Offset " + String( pcmPosition, HEX ));
          break;
        default:
          DEBUG_PRINTLN( "Unknown File Command: 0x" + String( command, HEX ));
          //while ( 1 );
          break;
      }
    }
  }
}

void startTimer(int frequencyHz) {
  REG_GCLK_CLKCTRL = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID (GCM_TCC2_TC3)) ;
  while ( GCLK->STATUS.bit.SYNCBUSY == 1 );
  TcCount16* TC = (TcCount16*) TC3;
  TC->CTRLA.reg &= ~TC_CTRLA_ENABLE;
  TC->CTRLA.reg |= TC_CTRLA_MODE_COUNT16;// Use the 16-bit timer
  while (TC->STATUS.bit.SYNCBUSY == 1);
  TC->CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ;// Use match mode so that the timer counter resets when the count matches the compare register
  while (TC->STATUS.bit.SYNCBUSY == 1);
  TC->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV16; // Set prescaler to 16
  while (TC->STATUS.bit.SYNCBUSY == 1);
  int compareValue = ( CPU_HZ / ( TIMER_PRESCALER_DIV * frequencyHz )) - 1;
  TC->COUNT.reg = map( TC->COUNT.reg, 0, TC->CC[ 0 ].reg, 0, compareValue ); // Make sure the count is in a proportional position to where it was to prevent any jitter or disconnect when changing the compare value.
  TC->CC[ 0 ].reg = compareValue;
  while ( TC->STATUS.bit.SYNCBUSY == 1 );
  TC->INTENSET.reg = 0; // Enable the compare interrupt
  TC->INTENSET.bit.MC0 = 1;
  NVIC_EnableIRQ(TC3_IRQn);
  TC->CTRLA.reg |= TC_CTRLA_ENABLE;
  while (TC->STATUS.bit.SYNCBUSY == 1);
}

void setTimerFrequency(int frequencyHz) {
  int compareValue = ( CPU_HZ / ( TIMER_PRESCALER_DIV * frequencyHz )) - 1;
  TcCount16* TC = ( TcCount16* ) TC3;
  TC->COUNT.reg = map( TC->COUNT.reg, 0, TC->CC[ 0 ].reg, 0, compareValue ); // Make sure the count is in a proportional position to where it was to prevent any jitter or disconnect when changing the compare value.
  TC->CC[ 0 ].reg = compareValue;
  while ( TC->STATUS.bit.SYNCBUSY == 1 );
}

void setup() {
  Serial.begin( 115200 );
  Serial.println( "Serial Online" );
  randomSeed( micros() / millis() );
  PORT->Group[PORTA].DIRSET.reg = ( 0xFF << 3 ) | CARD_CS | SN_WE | YM_IC | YM_A0 | YM_A1 | YM_RD | YM_WR | SN_CS | YM_CS | MEM_CS | SDA | SCL;
  PORT->Group[PORTA].DIRCLR.reg = SN_RDY | YM_IRQ;
  PORT->Group[PORTA].OUTSET.reg = YM_IC | YM_RD | YM_WR | YM_CS | MEM_CS | SN_WE | SN_CS;

  PORT->Group[PORTB].DIRSET.reg = MOSI | SCK;
  PORT->Group[PORTB].DIRCLR.reg = OSC_IN | BACKBUTTON | PLAYBUTTON | NEXTBUTTON | MIDI_IN;
  PORT->Group[PORTB].OUTSET.reg = BACKBUTTON | PLAYBUTTON | NEXTBUTTON;

  delay( 10 );
  PORT->Group[PORTA].OUTCLR.reg = YM_IC;
  delay( 10 );
  PORT->Group[PORTA].OUTSET.reg = YM_IC;
  delay( 10 );
  silence();
  SPI.begin();
  SPI.beginTransaction(SPISettings(50000000, MSBFIRST, SPI_MODE0));
  PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
  SPI.transfer( 0xab ); //release
  PORT->Group[PORTA].OUTSET.reg = MEM_CS;
  delayMicroseconds( 5 );
  DEBUG_PRINTLN( "Flash Chip Released" );
  PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
  SPI.transfer( 0x9f ); //read jedec
  uint8_t manuf = SPI.transfer(0x00);
  uint16_t id = ((uint16_t)SPI.transfer(0x00)) << 8;
  id |= SPI.transfer(0x00);
  PORT->Group[PORTA].OUTSET.reg = MEM_CS;
  DEBUG_PRINTLN("MANUF=0x" + String(manuf, HEX) + ",ID=0x" + String(id, HEX));
  PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
  SPI.transfer( 0x06 ); // write enable
  PORT->Group[PORTA].OUTSET.reg = MEM_CS;
  PORT->Group[PORTA].OUTCLR.reg = MEM_CS;
  SPI.transfer( 0x98 ); // write enable
  PORT->Group[PORTA].OUTSET.reg = MEM_CS;
  SD.begin( 0, SPI_FULL_SPEED );

  DEBUG_PRINTLN( "Setup Done" );
  delay( 5000 );
}

void loop() {
  getNumFiles();
  static int fileCount = 0;
  SD.vwd()->rewind();
  int randomFile = fileCount;
  fileCount++;
  char buf[256];
  for ( int i = 0; i < randomFile; i++ ) {
    vgmFile.openNext(SD.vwd(), O_READ);
    vgmFile.getName( buf, sizeof( buf ) );
    Serial.println( String( buf ));
    vgmFile.close();
  }
  Serial.println("try");
  if ( vgmStream.fopen( buf , "r" ) ) {
    Serial.println("fileopen");
    DEBUG_PRINTLN( "VGM File Open" );
    uint32_t ident = 0;
    for ( int i = 0; i < 4; i++ ) ident += uint32_t( vgmStream.getc() ) << ( 8 * i );
    if ( ident != 0x206d6756 ) {
      DEBUG_PRINTLN( "Ident Match Failed!" );
      return;
    }
    fileSize = 0;
    for ( int i = 0; i < 4; i++ ) fileSize += uint32_t( vgmStream.getc() ) << ( 8 * i );
    DEBUG_PRINTLN( "EOF:" + String( fileSize ) );
    vgmStream.fseek( 0x40, SEEK_SET );// cheat for now, need to read real VGM offset in header
    DEBUG_PRINTLN( "--------------VGM File Data Start--------------" );
    Serial.println("dumping");
    dumpToFlash();
    dumpToPcm();
    Serial.println("done");
    vgmStream.fclose();
  }
  else Serial.println(" File read error" );
  DEBUG_PRINTLN( "Closing VGM File" );
  done = 0;
  startTimer(44100);
  interrupts();
  while ( done == 0 ) DEBUG_PRINTLN( "Doing" );
  noInterrupts();
  DEBUG_PRINTLN( "Done" );
  PORT->Group[PORTA].OUTSET.reg = MEM_CS;
  silence();
  fileSize = 0;
  commandPosition = 0;
  pcmPosition  = 0;
  Serial.println("redo");
}