/* ArduinoFloppyDump
* Copyright (C) 2019-2020 Denis Dratov (@Dexus)
* 
* Derived from ArduinoFloppyReader by Robert Smith (@RobSmithDev)
* http://amiga.robsmithdev.co.uk
*
* This sketch is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public 
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
* Library General Public License for more details.
*
* You should have received a copy of the GNU Library General Public
* License along with this sketch; if not, see http://www.gnu.org/licenses
*/

#define BAUDRATE 2000000                 // The baudrate that we want to communicate over (2M)
#define BAUD_PRESCALLER_NORMAL_MODE      (((F_CPU / (BAUDRATE * 16UL))) - 1)
#define BAUD_PRESCALLER_DOUBLESPEED_MODE (((F_CPU / (BAUDRATE * 8UL))) - 1)
#define UART_USE_DOUBLESPEED_MODE        // We're using double speed mode

#define MOTOR_TRACK_DECREASE   HIGH      // Stepper motor directions
#define MOTOR_TRACK_INCREASE   LOW

// PIN 2 - INDEX - Used to detect a specific point on the track for sync. Not used by standard Amiga disks but some copy protection uses it.
#define PIN_INDEX_DETECTED       2          // PD2
#define PIN_INDEX_PORT           PIND
#define PIN_INDEX_MASK           B00000100

// PIN 4 - TRACK0 - Used to detect track 0 while moving the head
#define PIN_DETECT_TRACK_0       4        // PD4
#define DETECT_TRACK_0_MASK      B00010000
#define DETECT_TRACK_0_PORT      PIND

// PIN 5, 6 and 7 - DRIVE, HEAD MOTOR DIRECTION and CONTROL
#define PIN_DRIVE_ENABLE_MOTOR   5        // Turn on and off the motor on the drive
#define PIN_MOTOR_DIR            6        // Stepper motor to choose the direction the head should move
#define PIN_MOTOR_STEP           7        // Stepper motor step impulse to move the head position

// PIN 8 - RDATA - Used to read RAW floppy data
#define PIN_READ_DATA            8        // PB0
#define PIN_READ_DATA_MASK       B00000001
#define PIN_READ_DATA_PORT       PINB

// PIN 9 - HEAD - Used to choose upper or lower head to work with
#define PIN_HEAD_SELECT          9

// PIN 13 - Standard acvivity LED on Arduinos
#define PIN_ACTIVITY_LED         13

// PIN A2 - CTS Pin from UART
#define PIN_CTS                  A2         // Pin linked to the CTS pin
#define PIN_CTS_PORT             PORTC      // Port the CTS pin is on
#define PIN_CTS_MASK             B00000100  // Binary mask to control it with


// The current track that the head is over
byte currentTrack = 0;
// If the drive has been switched on or not
bool driveEnabled  = 0;
// Current MFM threshold values
byte z1;//=40/80;
byte z2;//=56/112; 

// Static memory buffer for everything (as big as possible) (#0x780 = 1920 bytes)
#define _BUFSIZE (0x100*7+0x80)
volatile byte _Buf[_BUFSIZE];


// We are using interruptless version of delay
#define smalldelay _delay_ms

// Step the head once. 6ms per step is standard speed period
void stepDirectionHead() {
    digitalWrite(PIN_MOTOR_STEP,LOW);
    smalldelay(1);
    digitalWrite(PIN_MOTOR_STEP,HIGH);
    smalldelay(5);
}

// Prepare serial port - We dont want to use the arduino serial library as we want to use faster speeds and no serial interrupts
void prepSerialInterface() { 
#ifdef UART_USE_DOUBLESPEED_MODE
    UBRR0H = (uint8_t)(BAUD_PRESCALLER_DOUBLESPEED_MODE>>8);
    UBRR0L = (uint8_t)(BAUD_PRESCALLER_DOUBLESPEED_MODE);
    UCSR0A |= 1<<U2X0;
#else
    UBRR0H = (uint8_t)(BAUD_PRESCALLER_NORMAL_MODE>>8);
    UBRR0L = (uint8_t)(BAUD_PRESCALLER_NORMAL_MODE);
    UCSR0A &= ~(1<<U2X0);
#endif 

    // UCSROA is a status register only (apart from U2Xn):
    //  * Bit 7 – RXCn: USART Receive Complete 
    //  * Bit 6 – TXCn: USART Transmit Complete
    //  * Bit 5 – UDREn: USART Data Register Empty
    //  * Bit 4 – FEn: Frame Error
    //  * Bit 3 – DORn: Data OverRun/
    //  * Bit 2 – UPEn: USART Parity Error
    //  * Bit 1 – U2Xn: Double the USART Transmission Speed
    //  * Bit 0 – MPCMn: Multi-processor Communication Mode

    UCSR0B  = (0<<RXCIE0)  |   // Disable ReceiveCompleteInteruptEnable
              (0<<TXCIE0)  |   // Disable TransmitCompleteInteruptEnable
              (0<<UDRIE0)  |   // Disable UsartDataRegisterEmptyInteruptEnable
              (1<<RXEN0)   |   // Enable RX
              (1<<TXEN0)   |   // Enable TX
              (0<<UCSZ02)  ;   // Clear the 9-bit character mode bit

    UCSR0C =  (0<<UMSEL01) | (0<<UMSEL00) |   // UsartModeSelect - Asynchronous (00=Async, 01=Sync, 10=Reserved, 11=Master SPI)
              (0<<UPM01)   | (0<<UPM00)   |   // UsartParatyMode - Disabled  (00=Off, 01=Reserved, 10=Even, 11=Odd)
              (0<<USBS0)   |   // UsartStopBitSelect (0=1 Stop bit, 1 = 2Stop Bits)
              (1<<UCSZ01)  | (1<<UCSZ00);    // UsartCharacterSiZe  - 8-bit (00=5Bit, 01=6Bit, 10=7Bit, 11=8Bit, must be 11 for 9-bit)
}

// Directly read a byte from the UART0 (serial)
inline byte readByteFromUART() {
    while (!( UCSR0A & ( 1 << RXC0 ))){};    // Wait for data to be available
    return UDR0;                                 // Read it
}

// Directly write a byte to the UART0
inline void writeByteToUART(const char value) {
    while(!(UCSR0A & (1<<UDRE0)));                // Wait until the last byte has been sent
    UDR0 = value;                                 // And send another
}

// Directly write a byte to the UART0
inline void waitUART() {
    while(!(UCSR0A & (1<<UDRE0)));                // Wait until UART port is available
}


// Read numeric value from UART (len chars, with leading 0s)
word readNumFromUART(byte len){
  word ret = 0;
  for (byte i=0; i<len; ++i){
    byte v = readByteFromUART();
    if ((v<'0')||(v>'9')) return -1;
    ret*=10;
    ret+=v-'0';
  }
  return ret;
}

// Write numeric value to UART
void writeNumberToUART(long tt)
{
    long t2;
    boolean first=true;
    for(long dd=100000000;dd>0; dd/=10)
    {
       t2=tt;
       tt=tt/dd;
       if (tt!=0 || !first) {
           writeByteToUART('0'+tt);
           first=false;
       }
       tt=t2-tt*dd;
    }
}

// Main arduino setup 
void setup() {
    // Do these right away to prevent the disk being written to
//    digitalWrite(PIN_WRITE_GATE, HIGH);
//    digitalWrite(PIN_WRITE_DATA, HIGH);
//    pinMode(PIN_WRITE_GATE,OUTPUT);
//    pinMode(PIN_WRITE_DATA,OUTPUT);
    pinMode(PIN_CTS,OUTPUT);
//    pinMode(PIN_WRITE_PROTECTED, INPUT_PULLUP);

    pinMode(PIN_DETECT_TRACK_0, INPUT_PULLUP);
    pinMode(PIN_READ_DATA,INPUT_PULLUP);
    digitalWrite(PIN_READ_DATA,HIGH);
    pinMode(PIN_INDEX_DETECTED,INPUT_PULLUP);

    // Prepre the pin inputs and outputs
    pinMode(PIN_DRIVE_ENABLE_MOTOR, OUTPUT);
    pinMode(PIN_HEAD_SELECT, OUTPUT);
    digitalWrite(PIN_DRIVE_ENABLE_MOTOR,HIGH);
    digitalWrite(PIN_HEAD_SELECT,LOW);    
    
    pinMode(PIN_MOTOR_DIR, OUTPUT);
    pinMode(PIN_MOTOR_STEP,OUTPUT);
    pinMode(PIN_ACTIVITY_LED,OUTPUT);
    
    digitalWrite(PIN_ACTIVITY_LED,LOW);
   
    // Disable all interrupts - we dont want them!
    cli();
    TIMSK0=0;
    TIMSK1=0;
    TIMSK2=0;
    PCICR = 0;
    PCIFR = 0;
    PCMSK0 = 0;
    PCMSK1 = 0;
    PCMSK2 = 0;
    // Setup the USART
    prepSerialInterface();
    z1=80;
    z2=112;
//    Stat_Buf=0x0200;
}

// Run a diagnostics test command
void runDiagnostic() {
    // See what test to run
    byte test = readByteFromUART();

    switch (test) {
        case '1':  // Turn off CTS
            PIN_CTS_PORT &= (~PIN_CTS_MASK);   
            writeByteToUART('1');
            readByteFromUART();
            writeByteToUART('1');
            break;

        case '2':  // Turn on CTS
            PIN_CTS_PORT|=PIN_CTS_MASK;   
            writeByteToUART('1');
            readByteFromUART();
            writeByteToUART('1');
            break;
        case '3':  // Index pulse test (with timeout)
            {
               byte state = 0;
               word period = 0;

               TCCR1A = 0 ;
               TCCR1B = bit(CS12); //prescaler 256 (62500 per sec)
               for (uint32_t b=0; b<600000; b++) {
                  if (state==0 && PIN_INDEX_PORT & PIN_INDEX_MASK) { // high level 99% of time
                     state=1; // charged to find LOW impulse
                     }
                 if (state==1 && !(PIN_INDEX_PORT & PIN_INDEX_MASK) ) { //low level 1% of time
                     state=2; //LOW impulse found
                     TCNT1=0; //counter 1 reset
                 }
                 if (state==2 && TCNT1>256 && !(PIN_INDEX_PORT & PIN_INDEX_MASK) ) { //low level 1% of time
                     state=3; //found second LOW impulse (by skipping 4ms)
                     period=TCNT1;
                     break;
                 }
               }
               if (period==0) {
                  writeByteToUART('0');
                  writeByteToUART('\n');
               }
               else {
                  writeNumberToUART(period);
                  writeByteToUART('\n');
               }
               TCCR1B = 0;      // No Clock (turn off)    
            }
            break;   

        case '4':  // Data pulse test (with timeout)
            {
               bool state1 = false;
               bool state2 = false;

              for (unsigned int b=0; b<20; b++) {
                 for (unsigned int a=0; a<60000; a++) {
                      if (PIN_READ_DATA_PORT & PIN_READ_DATA_MASK) state1=true; else state2=true;
                      if (state1&&state2) break;
                  }

                  if (state1&&state2) break;
              }

              if (state1&&state2) {
                  writeByteToUART('1');
              } else {
                  writeByteToUART('0');
              }
            }
            break;                         
              
      default:
        writeByteToUART('0');
        break;
    }
}

// Go to Track 0
bool gotoTrack0() {
    digitalWrite(PIN_MOTOR_DIR,MOTOR_TRACK_DECREASE);   // Set the direction to go backwards
    int counter=0;
    while (digitalRead(PIN_DETECT_TRACK_0) != LOW) {
       stepDirectionHead();   // Keep moving the head until we see the TRACK 0 detection pin
       counter++;
       if (counter>140) return false; //If we got here it means TRACK0 signal doesn't work or STEP didn't get performed
    }
    currentTrack = 0;    // Reset the track number
    return true;
}

// Goto a specific track.
bool gotoTrackX() {
    byte track = readNumFromUART(2);
    if (track>84) return false; // Maximum cyl number 83

    // Exit if it's already been reached
    if (track == currentTrack) return true;

    // And step the head until we reach this track number
    if (currentTrack < track) {
        digitalWrite(PIN_MOTOR_DIR,MOTOR_TRACK_INCREASE);   // Move OUT
        while (currentTrack < track) {
            stepDirectionHead();
            currentTrack++;         
        }
    } else {
        digitalWrite(PIN_MOTOR_DIR,MOTOR_TRACK_DECREASE);   // Move IN
        while (currentTrack > track) {
            stepDirectionHead();
            currentTrack--;
        }
    }
    return true;
}

// Wait, fetch ICR1 and classify single MFM pulse timing
#define readSingleMFM { \
while (PIN_READ_DATA_PORT & PIN_READ_DATA_MASK) {}; \
  asm volatile ( \
  "lds  %0, 0x0086 \n"  /* counter = lowByte(ICR1) */ \
  "mov __tmp_reg__,%0 \n" \
  "sub __tmp_reg__,%2 \n"  /* current-lastICR1 */ \
  "mov %2,%0 \n" /* store lastICR1 */ \
  "ldi %0,1 \n" /* 1 */ \
  "cp __tmp_reg__,%3 \n"  /* <z1? */ \
  "brcs 1f \n" \
  "ldi %0,2 \n" /* 2 */ \
  "cp __tmp_reg__,%4 \n" /* <z2? */ \
  "brcs 1f \n" \
  "ldi %0,3 \n" /* 3 */ \
  "1: \n" \
  "add %1,%1\n" /* shift value 2 bits left */ \
  "add %1,%1\n" \
  "add %1,%0\n" /* and fill with next value */ \
  : "=r"(tm), "=r"(Result), "=r"(lastICR1) : "r"(zz1), "r"(zz2)); \
}
  
//#pragma GCC push_options
//#pragma GCC optimize ("Os")

// Read the track using "timings" to figure each flux patch size
void readTrackData() {
    // Configure timer 1 just as a counter in NORMAL mode
    TCCR1A = 0 ;              // No physical output port pins and normal operation
    TCCR1B = bit(CS20);       // Precale = 1  
    
    // First wait for the serial port to be available
    waitUART();
   
    // Signal we're active
    digitalWrite(PIN_ACTIVITY_LED,HIGH);

    register byte zz1,zz2;
    zz1=z1;
    zz2=z2;

    register byte Result asm ("r19"); // current accumulated result
    register byte lastICR1 asm ("r20");
    register byte fin asm ("r21"); // if we want to break the loop
    register byte tm asm ("r22"); // current value

    TCNT1=0;       // Reset the counter
    word totalBytes=0;

    while (PIN_INDEX_PORT & PIN_INDEX_MASK)  {}; //wait for Index pulse

    writeByteToUART(1); //sof = start of file

    fin=0;
    lastICR1 = ICR1; //init ICR1
    while (fin==0) { //!!! index stop is not really good idea - it's better to log first index, and stop only after 2nd
      readSingleMFM;
      asm volatile ( //index check - if (index) fin=1
      "sbis  0x09, 2 ; 9 \n"
      "ldi %0,1"
      :"=&r"(fin));
      readSingleMFM;
      if (totalBytes<250) fin=0; //skip early index drop
      readSingleMFM;
      ++totalBytes;
      readSingleMFM;
      UDR0=Result; //UART output

      for (byte b=0;b<4;b++)
        readSingleMFM;
      UDR0=Result; //UART output
    }

    //extra 256*4 MFM switches, just in case if index was too early
    //so we could be able to wrap data, and capture pre-index data from the tail
    fin=128;
    while(fin!=0) {
      readSingleMFM;
      readSingleMFM;
      ++totalBytes;
      readSingleMFM;
      --fin;
      readSingleMFM;
      UDR0=Result; //UART output
      for (byte b=0;b<4;b++)
        readSingleMFM;
      UDR0=Result; //UART output
    }

// Minimal UART period in CPU ticks is 16Mhz/2Mbit*8 = 64 ticks
// Minimal MFM switch for 2us is 16Mhz*0.000002 = 32 ticks, for 4us = 64 ticks
// This means we can safely put UART each 3 HD periods (96), or 2 DD periods (128)
// But each 2 HD periods (64) is at the threshold, and might cause "not ready"

    writeByteToUART(0); 
//    writeByteToUART('\n');
//    writeNumberToUART(totalBytes*2); //Summary
//    writeByteToUART('\n');

    // turn off the status LED
    digitalWrite(PIN_ACTIVITY_LED,LOW);

    // Disable the counter
    TCCR1B = 0;      // No Clock (turn off)    
}

byte *Stat_Buf=0x0200;
/* Statistics buffer -- fixed ram address, 0x0200.
// Consists of two butters (odd/even). While first if accumulated, second one is transmitted via UART
// Each buffer length is actually 128 bytes, but transmitted only bytes 8..71 (representing lengths 16..142)
// Each value has clamp of 191, and basket width is "2" clock ticks (i.e. 8Mhz precision, or 0.125us).
*/
// Read track statistics (flux timings)
void scanTrackData() {
    // Configure timer 1 just as a counter in NORMAL mode
    TCCR1A = 0 ;              // No physical output port pins and normal operation
    TCCR1B = bit(CS20);       // Precale = 1  
    
    // First wait for the serial port to be available
    waitUART();
   
    // Signal we're active
    digitalWrite(PIN_ACTIVITY_LED,HIGH);

    for (int i=0;i<256;++i) Stat_Buf[i]=0; //clear statistics buffer

    register byte counter,lastICR1;
    word totalBytes=0;
    byte interleave=0,interleave2=128;
    byte chunk;

    // Prepare high part of Z (register r31) - it should have fixed address 0x02
    asm volatile (
        "ldi r31,0x02 \n"
      );

    TCNT1=0;       // Reset the counter
    while (PIN_INDEX_PORT & PIN_INDEX_MASK)  {}; //wait for index impulse

    writeByteToUART(254); //sof

    lastICR1 = ICR1; //init ICR1
    while (true) {
      chunk=8;
      do {
        register unsigned char bits=4;
        do {
            while (PIN_READ_DATA_PORT & PIN_READ_DATA_MASK) {};
            byte a;
            asm volatile (
            "lds  r30, 0x0086 \n" // counter = lowByte(ICR1)
            "mov __tmp_reg__,r30 \n"
            "sub  r30,%1 \n"
            "mov %1,__tmp_reg__ \n"
            "lsr  r30 \n" // /2
            "add  r30, %2 \n" //L= +interleave (r6)
            "ld  %0, Z \n"
            "cpi  %0, 0xF0 \n" //clamped with 0xf0 
            "brcc  1f \n"
            "subi  %0, 0xFF \n"
            "st  Z, %0 \n"
            "1: \n"
            :"=r"(a),"=r"(lastICR1):"r"(interleave) );
/*            counter = lowByte(ICR1); //Falling edge time mark
            byte a,b;
            a=(counter-lastICR1)/2+interleave;
            b=Stat_Buf[a];
            if (b<250) Stat_Buf[a]=b+1;
            lastICR1 = counter;
*/            
            
        } while (--bits);
        byte b;
        /* 
        byte b=chunk+interleave2;
        UDR0=Stat_Buf[b]; //output previous buffer data
        */
        asm volatile (
        "mov r30, %1 \n" //use r30 to utilize with r31 as Z
        "add r30, %2 \n" // + interleave2
        "ld  %0, Z \n"
        "sts  0x00C6, %0 \n" //UART output
        "st  Z, r1 \n" //clear after output (because it will be used in next iteration)
        :"=r"(b):"r"(chunk),"r"(interleave2) );
        ++totalBytes;
        bits=4;
        do {
            // Wait while RDATA is high
            while (PIN_READ_DATA_PORT & PIN_READ_DATA_MASK) {};
            byte a;
            asm volatile (
            "lds  r30, 0x0086 \n" // counter = lowByte(ICR1)
            "mov __tmp_reg__,r30 \n"
            "sub  r30,%1 \n"
            "lsr  r30 \n" // /2
            "add  r30, %2 \n" //L= +interleave (r6)
            "ld  %0, Z \n"
            "cpi  %0, 0xF0 \n"
            "brcc  1f \n"
            "subi  %0, 0xFF \n"
            "st  Z, %0 \n"
            "1: \n"
            "mov %1,__tmp_reg__ \n"
            :"=r"(a),"=r"(lastICR1):"r"(interleave) );
        } while (--bits);

        if (totalBytes>250)
            if (!(PIN_INDEX_PORT & PIN_INDEX_MASK)) goto exitt;
      } while (++chunk<64+8);

      asm volatile (  //swap values
        "mov __tmp_reg__,%0 \n"
        "mov %0,%1 \n"
        "mov %1,__tmp_reg__ \n"
        : "=r"(interleave), "=r"(interleave2):);
    }
exitt:
    // We need to flush interrupted prev buffer transmission when index occured...
    if (chunk<63+8) { //we don't want to repeat previous byte
        ++chunk;
        do {
            byte b=chunk+interleave2;
            writeByteToUART(Stat_Buf[b]);
        } while (++chunk<64+8);
    }
    chunk=8;
//    writeByteToUART('\n');
    // And output the last one
    do {
        byte b=chunk+interleave;
        writeByteToUART(Stat_Buf[b]);
    } while (++chunk<64+8);
    
    writeByteToUART(255); //eof
//    writeNumberToUART(totalBytes*2);

    // turn off the status LED
    digitalWrite(PIN_ACTIVITY_LED,LOW);

    // Disable the counter
    TCCR1B = 0;      // No Clock (turn off)    
}

bool setThreshold(){
    word a,b;
    a = readNumFromUART(3);
    if ((a>=192)||(a==0)) return false;
    b = readNumFromUART(3);
    if ((a>=192)||(a==0)) return false;
    z1=a;
    z2=b;
    return true;
}

//#pragma GCC pop_option

void dumpTrackData() {
    word a;
    a = readNumFromUART(5); //how many impulses to skip
    if (a==-1) {
      writeByteToUART(0);writeByteToUART(0); //double zero == eof
      return;
    }

    // Configure timer 1 just as a counter in NORMAL mode
    TCCR1A = 0 ;              // No physical output port pins and normal operation
    TCCR1B = bit(CS20);       // Precale = 1 + falling edge(default)
    
    // Signal we're active
    digitalWrite(PIN_ACTIVITY_LED,HIGH);

    while (PIN_INDEX_PORT & PIN_INDEX_MASK)  {}; //wait for index impulse

    register byte counter,lastICR1;
    byte* buf=_Buf;
    word totalBytes=_BUFSIZE;

    // Skip first N impulses after index (and initialize first distance)
    while (true) {
        while (PIN_READ_DATA_PORT & PIN_READ_DATA_MASK) {}; //wait for low
        lastICR1 = ICR1; //Init the counter
        while (!(PIN_READ_DATA_PORT & PIN_READ_DATA_MASK)) {}; //skip lower
        if (a--==0) break;
        while (PIN_READ_DATA_PORT & PIN_READ_DATA_MASK) {}; //wait for low
        lastICR1 = ICR1; //Init the counter
        while (!(PIN_READ_DATA_PORT & PIN_READ_DATA_MASK)) {}; //skip lower
    }
    do {
        while (PIN_READ_DATA_PORT & PIN_READ_DATA_MASK) {}; //wait for low
        counter = ICR1; //Falling edge time mark
        *(buf++)= lowByte(counter-lastICR1); //store time difference ignoring H
        lastICR1 = counter;
    } while (--totalBytes>0);
    buf=_Buf;
    totalBytes=_BUFSIZE;
    byte b;
    while (totalBytes-->0) {  //dump values
        b=*(buf++);
        if (b<8) b=8; //ignore zeroes
        writeByteToUART(b);
    }
    
    writeByteToUART(0); // zero == eof
    writeByteToUART('\n');
  
    // turn off the status LED
    digitalWrite(PIN_ACTIVITY_LED,LOW);

    // Disable the counter
    TCCR1B = 0;      // No Clock (turn off)    
}

// The main command loop
void loop() {
    PIN_CTS_PORT &= (~PIN_CTS_MASK);            // Allow data incoming
   
    // Read the command from the PC
    byte command = readByteFromUART();

    switch (command) {
  
        // Command: "?" Means information about the firmware
        case '?':writeByteToUART('A');
                 writeByteToUART('F');
                 writeByteToUART('D');
                 writeByteToUART('1');
                 writeByteToUART('.');
                 writeByteToUART('0');
                 writeByteToUART('b');
                 writeByteToUART('\n'); 
                 break;
  
        // Command "." means go back to track 0
        case '.': if (!driveEnabled) writeByteToUART('0'); else {
                   if (gotoTrack0())    // reset 
                      writeByteToUART('.');
                   else writeByteToUART(',');
                 }  
                 break;
  
        // Command "#" means goto track.  Should be formatted as #00 or #32 etc
        case '#': // Goto Track
                  if (!driveEnabled) {
                      readNumFromUART(2); //skip
                      writeByteToUART('0'); 
                  } else
                  if (gotoTrackX()) {
                      smalldelay(20); // wait for drive
                      writeByteToUART('#');
                  } else writeByteToUART('0');
                  break;
  
        // Command "[" select LOWER disk side
        case '[': digitalWrite(PIN_HEAD_SELECT,LOW);
                  writeByteToUART('[');
                  break;
  
        // Command "]" select UPPER disk side
        case ']': digitalWrite(PIN_HEAD_SELECT,HIGH);
                  writeByteToUART(']');
                  break;

        // Command "Dxxxxx" Dump track flux stream starting with xxxx flux
        case 'D': if (!driveEnabled) {
                      readNumFromUART(5); //skip
                      writeByteToUART('0');
                  } else {
                      dumpTrackData();
                  }
                  break;
  
        // Command "R" Read MFM track with threshold
        case 'R': if (!driveEnabled) writeByteToUART('0'); else {
                     readTrackData();
                  }
                  break;

        // Command "S" Scan track stats
        case 'S': if (!driveEnabled) writeByteToUART('0'); else {
                     scanTrackData();
                  }
                  break;

        // Command "T" means Threshold values. Should be formatted as Tnnnmmm
        case 'T': if (setThreshold()) 
                      writeByteToUART('T');
                  else writeByteToUART('0');
                  break;
  
        // Turn off the drive motor
        case '-': digitalWrite(PIN_DRIVE_ENABLE_MOTOR,HIGH);
                  driveEnabled = 0;
                  writeByteToUART('-');
                  break;
  
       // Turn on the drive motor and setup in READ MODE
       case '+':  if (!driveEnabled) {
                     digitalWrite(PIN_DRIVE_ENABLE_MOTOR,LOW);
                     driveEnabled = 1;
                     smalldelay(250); // wait for drive
                  }
                  writeByteToUART('+');
                  break;
  
        case '&': runDiagnostic();
                  break;
  
  
      // We don't recognise the command!
      default:
                 writeByteToUART('!'); // error
                 break;
     }
}