//Pico-PIO-USB/issues/122 v0.6.0:no mount, needs any setting but -Os (v0.6.0 only uses one PIO)

/*
Serial on GP0/GP1
I2C, inc IC1 DS3231 on GP2/GP3=I2C1 SDA/SCL
SPI devices on SPI1, MISO=GP8, SCK=GP10, MOSI=GP11
IC2 is SPI memory with CS on GP7
IC3 is SPI memory with CS on GP9
IRRX1 on GP22
SD card on SPI0,  MISO=GP16, SCK=GP18, MOSI=GP19
I2S audio: GP4=DIN,GP5=BCK,GP6=LRCK
USB PIO on GP26 D+, GP27 D-
uSD LED on GP15
USB LED on GP20
*/

#include <SdFat.h>
#include <pio_usb.h>
#include <Adafruit_TinyUSB.h>
#include <Wire.h>
#include <SPI.h>
#include "RTClib.h"

#define CONSOLE Serial1 
#define CONSOLE_TX 0
#define CONSOLE_RX 1
#define I2C1 Wire1
#define I2C_SDA 2
#define I2C_SCL 3
#define MISO0 16
#define MOSI0 19
#define SCK0 18
#define SD_CS 21
#define MISO1 8
#define MOSI1 11
#define SCK1 10
#define IC2_CS 7
#define IC3_CS 9
#define I2S_DATA 4
#define I2S_BCK 5
#define USB_PIO 26
#define RTC_ADD 0x68
#define MEM_SPI SPI1
#define USB_LED 20
#define USD_LED 15
#define IR_RX 22
#define LED_ONBOARD 25

#define SD_CONFIG SdSpiConfig(SD_CS,SHARED_SPI,SD_SCK_MHZ(50),&SPI)
SdFat32 sd;
File32 SDdir; //root object on SD
File32 SDWD; //working directory
File32 SDfile;
SPISettings memSettings(20000000,MSBFIRST,SPI_MODE0);

Adafruit_USBH_Host USBHost;
Adafruit_USBH_MSC_BlockDevice msc_block_dev;
FatVolume mscfatfs;
File32 mscDir;  //root object on MSC
File32 mscWD; //working directory
File32 mscFile;
bool mscIsMounted = false;
bool usdMounted = false;
char activeDrive='@';

#include <I2S.h>
I2S i2s(OUTPUT);
const int I2SsampleRate=16000;
#define TONE_FREQ 440
#define TONE_AMP 500
extern const int8_t sineSample[256];

//for I2C devices
#include "util.h"
RTC_DS3231 rtc;
DateTime now; //generic use by rtc, eg now = rtc.now();

//command prompt
#define CONBUFSIZE 256
char conBuffer[CONBUFSIZE]="";
char aPath[CONBUFSIZE]="/";
char bPath[CONBUFSIZE]="/";
char tempPath[CONBUFSIZE]="";
char promptBuffer[CONBUFSIZE]="> ";
int bPtr=0;

//IR on IR_RX pin
#include <IRremote.hpp>

void setup() {
  int i=0;
  int b=0;
  int a=0;
  Serial.begin(115200);  
  pinMode(LED_ONBOARD,OUTPUT);
  digitalWrite(LED_ONBOARD,HIGH);
  CONSOLE.setRX(CONSOLE_RX);
  CONSOLE.setTX(CONSOLE_TX);
  CONSOLE.begin(115200);  
  pinMode(SD_CS,OUTPUT);
  digitalWrite(SD_CS,HIGH);
  pinMode(IC2_CS,OUTPUT);
  digitalWrite(IC2_CS,HIGH);
  pinMode(IC3_CS,OUTPUT);
  digitalWrite(IC3_CS,HIGH);
  I2C1.setSDA(I2C_SDA);
  I2C1.setSCL(I2C_SCL);
  I2C1.begin();
  I2C1.setClock(100000);
  SPI.setRX(MISO0);
  SPI.setTX(MOSI0);
  SPI.setSCK(SCK0);
  SPI.begin();
  MEM_SPI.setRX(MISO1);
  MEM_SPI.setTX(MOSI1);
  MEM_SPI.setSCK(SCK1);
  MEM_SPI.begin();
  pinMode(USB_LED,OUTPUT);
  digitalWrite(USB_LED,LOW);
  pinMode(USD_LED,OUTPUT);
  digitalWrite(USD_LED,LOW);
  while ((!Serial)&&(millis()<1500)){}
  delay(500);
  CONSOLE.print("\x1B[2J\x1B[H"); //clear and home
  Serial.println("Starting Pico Digital Video Terminal BackPack");  
  CONSOLE.println("Starting Pico Digital Video Terminal BackPack");  
  if (sd.begin(SD_CONFIG)){
    usdMounted=true;
    digitalWrite(USD_LED,HIGH);
    Serial.println("SD OK");  
    CONSOLE.println("SD OK");  
    SDdir.open("/");
    SDdir.rewindDirectory();
    i=0;
    b=0;
    SDfile=SDdir.openNextFile();
    while(SDfile){
      i=i+1;
      b=b+SDfile.fileSize();
      SDfile.close();
      SDfile=SDdir.openNextFile();
    }
    SDfile.close();
    Serial.printf("A: SD card root has %d files totalling %d bytes.\r\n",i,b);
    CONSOLE.printf("A: SD card root has %d files totalling %d bytes.\r\n",i,b);
    SDWD.open("/");
    activeDrive='A';    
  }else{
    Serial.println("SD init error");
    CONSOLE.println("SD init error");
  }
  if(mscIsMounted){
    Serial.println("USB MSC OK"); 
    CONSOLE.println("USB MSC OK"); 
    mscDir.open(&mscfatfs,"/");
    mscDir.rewindDirectory();
    i=0;
    b=0;
    mscFile=mscDir.openNextFile();
    while(mscFile){
      i=i+1;
      b=b+mscFile.fileSize();
      mscFile.close();
      mscFile=mscDir.openNextFile();
    }
    mscFile.close();
    Serial.printf("B: USB MSC card root has %d files totalling %d bytes.\r\n",i,b);
    CONSOLE.printf("B: USB MSC card root has %d files totalling %d bytes.\r\n",i,b);
    mscWD.open(&mscfatfs,"/");
    if(activeDrive<'A'){activeDrive='B';}
    mscDir.rewindDirectory();  
  }else{
    Serial.println("USB MSC not found");
    CONSOLE.println("USB MSC not found");
  }
  I2C1.beginTransmission(RTC_ADD);
  if (I2C1.endTransmission()==0){
    Serial.println("RTC found");
    CONSOLE.println("RTC found");
  }else{
    Serial.println("RTC not found");
    CONSOLE.println("RTC not found");
  }
  if (rtc.begin(&I2C1)){
    Serial.println("RTC started OK");
    CONSOLE.println("RTC started OK");
    if(rtc.lostPower()){
      Serial.println("RTC needs to be set");
      CONSOLE.println("RTC needs to be set");
    }else{
      now=rtc.now();  
      Serial.printf("Time is %2d:%02d:%02d on %2d/%2d/%4d\r\n",now.hour(),now.minute(),now.second(),now.day(),now.month(),now.year());
      CONSOLE.printf("Time is %2d:%02d:%02d on %2d/%2d/%4d\r\n",now.hour(),now.minute(),now.second(),now.day(),now.month(),now.year());
    }
  }else{
    Serial.println("RTC not started");
    CONSOLE.println("RTC not started");
  }
  Serial.printf("IC2 ID is 0x%x.\r\n",getMemID(IC2_CS));
  Serial.printf("IC3 ID is 0x%x.\r\n",getMemID(IC3_CS));
  CONSOLE.printf("IC2 ID is 0x%x.\r\n",getMemID(IC2_CS));
  CONSOLE.printf("IC3 ID is 0x%x.\r\n",getMemID(IC3_CS));
  i2s.setBCLK(I2S_BCK);
  i2s.setDATA(I2S_DATA);
  i2s.setBuffers(3,128,0);
  i2s.setBitsPerSample(16);
  if (i2s.begin(I2SsampleRate)){
    Serial.println("Audio started OK");
    CONSOLE.println("Audio started OK");
    //Serial.printf("%d bytes available in I2S buffers\r\n",i2s.availableForWrite());
    //CONSOLE.printf("%d bytes available in I2S buffers\r\n",i2s.availableForWrite());
    i2s.end();
  }else{
    Serial.println("Audio start failed");
    CONSOLE.println("Audio start failed");
  }
  Serial.println("I2C scan:");
  CONSOLE.println("I2C scan:");
  for (a=8;a<120;a++){
    if(checkDevice(a)){
      Serial.print(" 0x");
      Serial.print(a,HEX);
      CONSOLE.print(" 0x");
      CONSOLE.print(a,HEX);
    }
  }
  Serial.println();
  Serial.println("I2C scan done.");
  CONSOLE.println();
  CONSOLE.println("I2C scan done.");
  getPrompt();
  Serial.print(promptBuffer);
  CONSOLE.print(promptBuffer);
}

void loop() {
  int d=-1;
  int i;
  char outBuffer[256]="";
  if(Serial.available()){ //process one or the other at a time
    d=Serial.read();
  }else{
    if(CONSOLE.available()){
      d=CONSOLE.read();
    }
  }
  if(d==13){
    Serial.println();
    CONSOLE.println();
    checkCMD();
    bPtr=0;
    for(i=0;i<CONBUFSIZE;i++){conBuffer[i]=0;}//erase
    getPrompt();
    Serial.print(promptBuffer);
    CONSOLE.print(promptBuffer);
  }else if(d==8){ //backspace
    if(bPtr){
      bPtr--;
      conBuffer[bPtr]=0;  //erase
      Serial.print("\x1B[D \x1B[D"); //backup, draw space and backup
      CONSOLE.print("\x1B[D \x1B[D"); //backup, draw space and backup
    }
  }else if(d==27){ //escape
    bPtr=0;
    for(i=0;i<CONBUFSIZE;i++){conBuffer[i]=0;}//erase
    Serial.println();
    CONSOLE.println();
    Serial.print(promptBuffer);
    CONSOLE.print(promptBuffer);
  }else if(d>31){ //normal character
    if(bPtr<64){
      conBuffer[bPtr]=d;
      bPtr++;
      Serial.write(d);
      CONSOLE.write(d);
    }
  }
  if(usdMounted){
    digitalWrite(USD_LED,HIGH);
  }else{
    digitalWrite(USD_LED,LOW);
  }
  if(mscIsMounted){
    digitalWrite(USB_LED,HIGH);
  }else{
    digitalWrite(USB_LED,LOW);
  }
}


void getPrompt(void){
  char t[64]=""; 
  if(activeDrive=='A'){
    //SDWD.getName(t,64);
    //sprintf(promptBuffer,"%c:%s> ",'A',t);
    sprintf(promptBuffer,"%c:%s> ",'A',aPath);
  }else if(activeDrive=='B'){
    //mscWD.getName(t,64);
    //sprintf(promptBuffer,"%c:%s> ",'B',t);
    sprintf(promptBuffer,"%c:%s> ",'B',bPath);
  }else{
    sprintf(promptBuffer,"?:> ");  
  }
}

void checkCMD(void){
    const char* t;
    conBuffer[bPtr]=0;  //ensure null term
    t=toUpperNew(conBuffer);  //use a temp so we don't mangle parameters
    //Serial.println(conBuffer);
    //CONSOLE.println(conBuffer);
    if(matchCMD(t,"REBOOT",6)){rp2040.reboot();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"BOOTLOAD",8)){rp2040.rebootToBootloader();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"CLS",3)){SandCprint("\x1B[2J\x1B[H");conBuffer[0]=0;}  //clear
    if(matchCMD(t,"DEL",3)){doDELCMD();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"CD",2)){doCDCMD();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"DIR",3)){doDIRCMD();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"A:",2)){doAdriveCMD();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"B:",2)){doBdriveCMD();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"TIME",4)){doTimeCMD();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"HELP",4)){doHelpCMD();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"SETTIME",7)){doSettimeCMD();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"IR",2)){doIRCMD();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"TONE",4)){doTONECMD();conBuffer[0]=0;}  //clear
    if(matchCMD(t,"MEM",3)){doMEMcmd();conBuffer[0]=0;}  //clear
    if(conBuffer[0]!=0){
      Serial.print("Unknown command:");
      CONSOLE.print("Unknown command:");
      Serial.println(conBuffer);
      CONSOLE.println(conBuffer);
    }
}

void doMEMcmd(void){
    const char* t;
    t=toUpperNew(conBuffer);  //use a temp so we don't mangle parameters
    if(matchCMD(&t[3]," IC2",4)){dumpMem(IC2_CS);}
    else if(matchCMD(&t[3]," IC3",4)){dumpMem(IC3_CS);}
    else{SandCprint("Device unavailable\r\n");}
}

void dumpMem(uint8_t csPin){
  char n[256]="";
  int i;
  SandCprint("    ");
  for(i=0;i<16;i++){
    sprintf(n," x%X",i);
    SandCprint(n);
  }
  for(i=0;i<256;i++){
    if((i&0xF)==0){ //16 per line
      SandCprint("\r\n");
      sprintf(n," %Xx ",i/16);
      SandCprint(n);
    }
    sprintf(n," %02X",readMEM(csPin,i));
    SandCprint(n);
  }
  SandCprint("\r\n");  
}


void doTONECMD(void){
  unsigned int f,d,s,i,p,incr;  //freq, duration, sample count,index, pointer, increment
  int n[3]={0,0,0};
  char c;
  int8_t samp;  
  p=0;
  for(i=5;i<bPtr;i++){      //scan command string for parameters
    c=conBuffer[i];
    if((c>='0')&&(c<='9')){n[p]=n[p]*10+c-'0';}
    if((c==',')&&(p<2)){p=p+1;}
  }
  if((n[0]<10)||(n[0]>8000)){  //sanity check, default if no argument given
    f=1000;
  }else{
    f=n[0];
  }
  if((n[1]<10)||(n[1]>10000)){  //sanity check, default if no argument given
    d=1000;
  }else{
    d=n[1];
  }
  s=(d*I2SsampleRate)/1000; //1000ms in a second
  p=0;
  incr=(I2SsampleRate*f)/3906;  //3906=1000000/256
  //Serial.printf("%d samples, %dHz, %dms,(INC=%d)\r\n",s,f,d,incr);
  if (i2s.begin(I2SsampleRate)){
    SandCprint("Playing tone. ");
    for(i=0;i<s;i++){
      samp=sineSample[(p/256)%256];    
      i2s.write16(samp*256,samp*256);  //this is blocking as required
      p=(p+incr)%65536;
    }
    //Serial.printf("%d samples written\r\n",i);
    i2s.flush();  //wait for playback    
    SandCprint("Done.\r\n");
  }else{
    SandCprint("Could not start audio\r\n");
  }
  i2s.end();
}

void doHelpCMD(void){
  SandCprint("Command listing:\r\n");
  SandCprint("TIME, HELP, CLS\r\n");
  SandCprint("SETTIME yyyy,mm,dd,hh,mm,ss\r\n");
  SandCprint("A: or B: switch drive\r\n");
  SandCprint("CD change dirctory (CD .. to go up)\r\n");
  SandCprint("DIR directory listing\r\n");
  SandCprint("DEL delete file in current directory\r\n");
  SandCprint("IR monitor IR signals\r\n");
  SandCprint("TONE freq,dur\r\n");
  SandCprint("MEM IC2, MEM IC3\r\n");
  SandCprint("REBOOT, BOOTLOAD\r\n");
}

bool wcMatch(const char *pattern, const char *candidate, int p, int c) { //wildcard match, see https://stackoverflow.com/questions/23457305/compare-strings-with-wildcard
  //not like the DOS pattern matcher, but helpful
  if (pattern[p] == '\0') {
    return candidate[c] == '\0';
  } else if (pattern[p] == '*') {
    for (; candidate[c] != '\0'; c++) {
      if (wcMatch(pattern, candidate, p+1, c))
        return true;
    }
    return wcMatch(pattern, candidate, p+1, c);
  } else if (pattern[p] != '?' && pattern[p] != candidate[c]) {
    return false;
  }  else {
    return wcMatch(pattern, candidate, p+1, c+1);
  }
}

void doIRCMD(void){
  SandCprint("Waiting for IR signals\r\n");
  SandCprint("Press any key to exit\r\n");
  IrReceiver.begin(IR_RX,false); //no feedback LED
  while(1){
    if(IrReceiver.decode()){    
      if(IrReceiver.decodedIRData.protocol==UNKNOWN){
        SandCprint("Unknown IR Signal\r\n");      
      }else{
        IrReceiver.printIRResultShort(&Serial);
        IrReceiver.printIRResultShort(&CONSOLE);
      }
      IrReceiver.resume();
    }
    if(Serial.available()){
      Serial.read();
      IrReceiver.end();
      return;
    }
    if(CONSOLE.available()){
      CONSOLE.read();
      IrReceiver.end();
      return;
    }
  }
}

void doDELCMD(void){
  char n[256]="";
  if(strlen(conBuffer)<5){
    SandCprint("No file specified\r\n");
    return;
  }
  if(activeDrive=='A'){    
    digitalWrite(USD_LED,LOW);  //flicker LED
    if(SDWD.remove(&conBuffer[4])){
      SandCprint("File deleted\r\n");
    }else{
      SandCprint("Delete failed\r\n");
    }
  }else if(activeDrive=='B'){
    digitalWrite(USB_LED,LOW);  //flicker LED
    if(mscWD.remove(&conBuffer[4])){
      SandCprint("File deleted\r\n");
    }else{
      SandCprint("Delete failed\r\n");
    }
  }else{
    SandCprint("No drive selected\r\n");
  }
}

void doDIRCMD(void){
  char t[64]=""; 
  char n[256]="";
  int f=0;  //files
  int d=0;  //directories
  if(activeDrive=='A'){
    digitalWrite(USD_LED,LOW);  //flicker LED
    SDWD.rewindDirectory();
    SDfile=SDWD.openNextFile();
    while(SDfile){
      SDfile.getName(t,64);      
      if((strlen(conBuffer)==3)||(wcMatch(&conBuffer[4],t,0,0))){   //no parameters or wildcard match
        if(SDfile.isDirectory()){
          sprintf(n,"[DIR]     %s\r\n",t);
          d=d+1;
        }else{
          sprintf(n,"%9d %s\r\n",SDfile.fileSize(),t);
          f=f+1;
        }      
        SandCprint(n);
      }
      SDfile.close();
      SDfile=SDWD.openNextFile();
    }
    SDfile.close();
    sprintf(n,"%d files and %d directories\r\n",f,d);
    SandCprint(n);
  }else if(activeDrive=='B'){
    digitalWrite(USB_LED,LOW);  //flicker LED
    mscWD.rewindDirectory();
    mscFile=mscWD.openNextFile();
    while(mscFile){
      mscFile.getName(t,64);
      if((strlen(conBuffer)==3)||(wcMatch(&conBuffer[4],t,0,0))){   //no parameters or wildcard match
        if(mscFile.isDirectory()){
          sprintf(n,"[DIR]     %s\r\n",t);
          d=d+1;
        }else{
          sprintf(n,"%9d %s\r\n",mscFile.fileSize(),t);
          f=f+1;
        }      
        SandCprint(n);
      }
      mscFile.close();
      mscFile=mscWD.openNextFile();
    }
    mscFile.close();
    sprintf(n,"%d files and %d directories\r\n",f,d);
    SandCprint(n);
  }else{
    SandCprint("No drive selected\r\n");
  }
}

char * parentDir(char* a){  //up a level
  static char r[256]="";
  int i;
  strcpy(r,a);  
  i=strlen(r);
  if(i>1){
    i=i-1;
    r[i]=0;
    while((r[i]!='/')&&(i>0)){  //strip back to next /
      r[i]=0;
      i--;
    }
  }else{  //empty, return something
    r[0]='/';
    r[1]=0;
  }
  return r;
}

void doCDCMD(void){
  char t[64]=""; 
  char n[256]="";
  if(activeDrive=='A'){
    digitalWrite(USD_LED,LOW);  //flicker LED
    if(matchCMD(&conBuffer[3],"..",3)){
      //go up a level
      strcpy(tempPath,parentDir(aPath));
    }else{
      sprintf(tempPath,"%s%s/",aPath,&conBuffer[3]);
    }    
    SDfile.close();
    SDfile.open(tempPath); //test
    if(SDfile && SDfile.isDirectory()){ //valid
      SDfile.close();
      SDWD.close();
      SDWD.open(tempPath);
      strcpy(aPath,tempPath);
      if((!SDWD) || (!SDWD.isDirectory())){ //invalid
        SandCprint("Failed to CD\r\n");
        SDWD.close();
        SDWD.open("/");//try default
        aPath[0]='/';
        aPath[1]=0;
      }
    }else{
      SandCprint("Directory does not exist\r\n");
    }
    SDfile.close();
  }else if(activeDrive=='B'){
    digitalWrite(USB_LED,LOW);  //flicker LED
    if(matchCMD(&conBuffer[3],"..",3)){
      //go up a level
      strcpy(tempPath,parentDir(bPath));
    }else{
      sprintf(tempPath,"%s%s/",bPath,&conBuffer[3]);
    }    
    mscFile.close();
    mscFile.open(&mscfatfs,tempPath); //test
    if(mscFile && mscFile.isDirectory()){ //valid
      mscFile.close();
      mscWD.close();
      mscWD.open(&mscfatfs,tempPath);
      strcpy(bPath,tempPath);
      if((!mscWD) || (!mscWD.isDirectory())){ //invalid
        SandCprint("Failed to CD\r\n");
        mscWD.close();
        mscWD.open(&mscfatfs,"/");//try default
        bPath[0]='/';
        bPath[1]=0;
      }
    }else{
      SandCprint("Directory does not exist\r\n");
    }
    mscFile.close();
  }else{
    SandCprint("No drive selected\r\n");
  }
}

void doAdriveCMD(void){
    if(usdMounted){
      activeDrive='A';
    }else{
      SandCprint("A: drive is not available\r\n");
    }
}

void doBdriveCMD(void){
    if(mscIsMounted){
      activeDrive='B';
    }else{
      SandCprint("B: drive is not available\r\n");
    }
}

void SandCprint(const char* s){
  Serial.print(s);
  CONSOLE.print(s);
}

void doSettimeCMD(void){
  int n[7]={0,0,0,0,0,0};
  int i=0;
  int p=0;
  char c;
  for(i=8;i<bPtr;i++){
    c=conBuffer[i];
    if((c>='0')&&(c<='9')){n[p]=n[p]*10+c-'0';}
    if((c==',')&&(p<6)){p=p+1;}
  }
  if(p==5){ //correct number of params
    rtc.adjust(DateTime(n[0],n[1],n[2],n[3],n[4],n[5]));
    doTimeCMD();
  }else{
    Serial.printf("Wrong number (%d) of parameters (should be 6)\r\n",p+1);
    CONSOLE.printf("Wrong number (%d) of parameters (should be 6)\r\n",p+1);
  }
}

void doTimeCMD(void){
  now=rtc.now();  
  Serial.printf("Time is %2d:%02d:%02d on %2d/%2d/%4d\r\n",now.hour(),now.minute(),now.second(),now.day(),now.month(),now.year());
  CONSOLE.printf("Time is %2d:%02d:%02d on %2d/%2d/%4d\r\n",now.hour(),now.minute(),now.second(),now.day(),now.month(),now.year());
}

char* toUpperNew(char* a){
  static char r[CONBUFSIZE]="";
  strcpy(r,a);  //copy
  toUpper(r);   //change buffer
  return r; 
}

void toUpper(char* a){  //to upper in place
  int i,j;
  j=strlen(a);
  for(i=0;i<j;i++){
    if((a[i]>='a')&&(a[i]<='z')){a[i]=a[i]+'A'-'a';}
  }
}

char matchCMD(const char* a, const char* b,int n){  //if first n characters match
  int i;
  for(i=0;i<n;i++){
    if(a[i]!=b[i]){return 0;}
  }
  return 1;
}

int readMEM(uint8_t p,unsigned int a){ //read from device,address
  int r=0;
  MEM_SPI.beginTransaction(memSettings);
  digitalWrite(p,LOW);
  MEM_SPI.transfer(0x03);   //read
  MEM_SPI.transfer((a>>16)&0xFF); //MSB
  MEM_SPI.transfer((a>> 8)&0xFF);
  MEM_SPI.transfer((a>> 0)&0xFF); //LSB
  r=MEM_SPI.transfer(0);
  digitalWrite(p,HIGH);
  MEM_SPI.endTransaction();
  return r;
}

int getMemID(uint8_t p){
  int r=0;
  MEM_SPI.beginTransaction(memSettings);
  digitalWrite(p,LOW);
  //MEM_SPI.transfer(0x90); //?
  MEM_SPI.transfer(0x9F); //JEDEC
  r=MEM_SPI.transfer(0);
  r=(r<<8)|MEM_SPI.transfer(0);
  r=(r<<8)|MEM_SPI.transfer(0);
  r=(r<<8)|MEM_SPI.transfer(0);
  digitalWrite(p,HIGH);
  MEM_SPI.endTransaction();
  return r;
}

static void rp2040_configure_pio_usb(void) {
  // Check for CPU frequency, must be multiple of 120Mhz for bit-banging USB
  pio_usb_configuration_t pio_cfg = PIO_USB_DEFAULT_CONFIG;
  pio_cfg.pin_dp = USB_PIO;
#if defined(ARDUINO_RASPBERRY_PI_PICO_W)
  // For pico-w, PIO is also used to communicate with cyw43
  // Therefore we need to alternate the pio-usb configuration
  // details https://github.com/sekigon-gonnoc/Pico-PIO-USB/issues/46
  pio_cfg.sm_tx      = 3;
  pio_cfg.sm_rx      = 2;
  pio_cfg.sm_eop     = 3;
  pio_cfg.pio_rx_num = 0;
  pio_cfg.pio_tx_num = 1;
  pio_cfg.tx_ch      = 9;
#endif
  USBHost.configure_pio_usb(1, &pio_cfg);
}

//------------- Core1 -------------//
void setup1() {
  // configure pio-usb: see above
  rp2040_configure_pio_usb();
  //run on Core 1
  USBHost.begin(1);
}

void loop1() {  //run tasks on core 1
  USBHost.task();
}

//--------------------------------------------------------------------+
// TinyUSB Host callbacks
//--------------------------------------------------------------------+
extern "C"
{

// Invoked when device is mounted (configured)
void tuh_mount_cb(uint8_t daddr) {
  (void) daddr;
}

/// Invoked when device is unmounted (bus reset/unplugged)
void tuh_umount_cb(uint8_t daddr) {
  (void) daddr;
}

// Invoked when a device with MassStorage interface is mounted
void tuh_msc_mount_cb(uint8_t dev_addr) {
  //Serial.printf("Device attached, address = %d\r\n", dev_addr);
  // Initialize block device with MSC device address
  msc_block_dev.begin(dev_addr);
  // For simplicity this example only support LUN 0
  msc_block_dev.setActiveLUN(0);
  mscIsMounted = mscfatfs.begin(&msc_block_dev);
}

// Invoked when a device with MassStorage interface is unmounted
void tuh_msc_umount_cb(uint8_t dev_addr) {
  //Serial.printf("Device removed, address = %d\r\n", dev_addr);
  // unmount file system
  mscIsMounted = false;
  mscfatfs.end();
  // end block device
  msc_block_dev.end();
}

}

int checkPIO(){ //bitmap of PIO SMs that have been claimed
  int r=0;
  if(pio_sm_is_claimed(pio0,0)){r=r|1;}
  if(pio_sm_is_claimed(pio0,1)){r=r|2;}
  if(pio_sm_is_claimed(pio0,2)){r=r|4;}
  if(pio_sm_is_claimed(pio0,3)){r=r|8;}
  if(pio_sm_is_claimed(pio1,0)){r=r|16;}
  if(pio_sm_is_claimed(pio1,1)){r=r|32;}
  if(pio_sm_is_claimed(pio1,2)){r=r|64;}
  if(pio_sm_is_claimed(pio1,3)){r=r|128;}
  return r;
}

const int8_t sineSample[256]={
  0,2,5,8,11,14,17,20,23,26,29,32,34,37,40,43,45,48,51,53,56,59,61,64,66,69,71,73,76,
  78,80,82,84,86,88,90,92,94,96,98,99,101,102,104,105,107,108,109,110,111,112,113,114,
  115,116,117,117,118,118,119,119,119,119,119,120,119,119,119,119,119,118,118,117,117,
  116,115,114,113,112,111,110,109,108,107,105,104,102,101,99,98,96,94,92,90,88,86,84,
  82,80,78,76,73,71,69,66,64,61,59,56,53,51,48,45,43,40,37,34,32,29,26,23,20,17,14,11,
  8,5,2,0,-3,-6,-9,-12,-15,-18,-21,-24,-27,-30,-33,-35,-38,-41,-44,-46,-49,-52,-54,-57,
  -60,-62,-65,-67,-70,-72,-74,-77,-79,-81,-83,-85,-87,-89,-91,-93,-95,-97,-99,-100,-102,
  -103,-105,-106,-108,-109,-110,-111,-112,-113,-114,-115,-116,-117,-118,-118,-119,-119,
  -120,-120,-120,-120,-120,-120,-120,-120,-120,-120,-120,-119,-119,-118,-118,-117,-116,
  -115,-114,-113,-112,-111,-110,-109,-108,-106,-105,-103,-102,-100,-99,-97,-95,-93,-91,
  -89,-87,-85,-83,-81,-79,-77,-74,-72,-70,-67,-65,-62,-60,-57,-54,-52,-49,-46,-44,-41,
  -38,-35,-33,-30,-27,-24,-21,-18,-15,-12,-9,-6,-3
};