#include "Arduino.h"
#include <SPI.h>    /* .arduino15/packages/adafruit/hardware/samd/1.7.16/libraries/SPI/SPI.h */
#include <stdio.h>


/******
 * This file has the pin definitions for the Qtpy
 * /home/neil/.arduino15/packages/adafruit/hardware/samd/1.7.16/variants/qtpy_m0/variant.h
 *
 * This process can probably be speeded up by transfering the page data using 
 * base64 or binary
 ******/

#define MYSPI             SPI
#define MYSPI_SS          PIN_A0
#define MYSPI_SI          13
#define MYSPI_SO          12
#define MYSPI_CLK         14

/* The instruction codes for the Flash memory */
#define SPI_READ_DATA     0x03
#define SPI_JEDEC_ID      0x9F
#define SPI_ERASE         0x60
#define SPI_ENABLE_WRITE  0x06
#define SPI_PAGE_PROGRAM  0x02
#define SPI_READ_STATUS   0x05

static unsigned long address = 0;             // temporary storage for the Flash address
static byte * write_page = NULL;              // buffer for the write page data
static short write_page_used = 0;             // how much of the buffer is used

void spi_enable (void) {
  pinMode (MYSPI_SS, OUTPUT);
  pinMode (MYSPI_CLK, OUTPUT);
  pinMode (MYSPI_SI, OUTPUT);
  pinMode (MYSPI_SO, INPUT);
  digitalWrite (MYSPI_SS, LOW);
  MYSPI.beginTransaction (SPISettings(1000000, MSBFIRST, SPI_MODE3));
}

void spi_disable (void) {
  MYSPI.endTransaction();
  digitalWrite (MYSPI_SS, HIGH);  
  pinMode (MYSPI_SS, INPUT);                  // make all the signals inputs, 
  pinMode (MYSPI_CLK, INPUT);                 // so they wont affect the FPGA using the Flash memory
  pinMode (MYSPI_SI, INPUT);
  pinMode (MYSPI_SO, INPUT);
}

// the setup routine runs once when you press reset:
void setup() {
  // initialize serial communication
  Serial.begin(115200);
  while (!Serial) delay(10);
  // init. the SPI
  MYSPI.begin();
  /* This seems to be needed for the Flash memory */
  spi_enable();
  delay(5);
  spi_disable();
}

void spi_read (int address, void *buffer, int size) {
  byte *data = (byte *)buffer;
  spi_enable();
  MYSPI.transfer(SPI_READ_DATA);
  MYSPI.transfer((address >> 16) & 0xFF);
  MYSPI.transfer((address >> 8) & 0xFF);
  MYSPI.transfer(address & 0xFF);
  for (int x = 0; x < size; x++) {
    data[x] = MYSPI.transfer(0) & 0xFF;
  }
  spi_disable();
}

unsigned long spi_jedic_id (void) {
  unsigned long result = 0;
  spi_enable();
  MYSPI.transfer(SPI_JEDEC_ID);
  for (int x = 0; x < 3; x++) {
    result = (result << 8) | (MYSPI.transfer(0) & 0xFF);
  }
  spi_disable();
  return result;
}

unsigned short spi_wait_busy(void) {
  unsigned short status;
  do {
    delay (100);            // 1/10 of a second
    digitalWrite (MYSPI_SS, LOW);
    MYSPI.transfer(SPI_READ_STATUS);
    status = MYSPI.transfer(0);
    status <<= 8;
    status |= MYSPI.transfer(0);
    digitalWrite (MYSPI_SS, HIGH);
  } while (status & (1 << 0));
  return status;
}

unsigned short spi_erase_flash (void) {
  long status = 0;
  spi_enable();
  MYSPI.transfer(SPI_ENABLE_WRITE);
  digitalWrite (MYSPI_SS, HIGH);
  digitalWrite (MYSPI_SS, LOW);
  MYSPI.transfer(SPI_ERASE);
  digitalWrite (MYSPI_SS, HIGH);
  status = spi_wait_busy();
  spi_disable();
  return status;
}

// The different states for the FSM
enum state {
  ST_WAIT,
  ST_READ,
  ST_WRITE,
  ST_WRITE_PAGE,
  ST_WRITE_CRC};

static int state_wait (int state, String line) {
  if (line == "AT") {
    Serial.println("OK ATTENTION");                       // no state change here
  }
  else if (line == "READ") {
    Serial.println("OK");
    return (ST_READ);                                     // start the read process
  }
  else if (line == "WRITE") {
    Serial.println("OK");
    return (ST_WRITE);                                    // start the write process
  }
  else if (line == "ERASE") {
    unsigned short status = spi_erase_flash();            // erase the flash now!
    Serial.print("OK ");                                  // doesn't change the state
    Serial.println(status,HEX);
  }
  return state;
}

static int state_read (int state, String line) {
  if (line.startsWith("ADDRESS")) {
    int index = line.indexOf(":");
    String addr = line.substring(index + 1);
    address = strtol(addr.c_str(), NULL, 16);                               // extract the address to read from
    /* read the page */
    byte page[256];
    spi_read (address, page, sizeof(page));
    for (int x = 0; x < 16; x++) {                                          // send the page 16 bytes at a time
      String text = "";
      char buffer[16];                                                      // buffer for the ascii
      for (int y = 0; y < 16; y++) {
        snprintf (buffer, sizeof(buffer), "%02X", page[(x * 16) + y]);      // easy way to convert byte to ascii
        //Serial.print("buffer:\"");Serial.print(buffer);Serial.println("\"");
        if (text.length() > 0) text += ",";                                 // add to the line
        text += String(buffer);
      }
      Serial.println(text);                                                 // send to the source
    }
  }
  return ST_WAIT;
}

static int state_write (int state, String line) {
  if (line.startsWith("ADDRESS")) {                                         // check for the address header
    int index = line.indexOf(":");
    String addr = line.substring(index + 1);
    unsigned long address = strtol(addr.c_str(), NULL, 16);                 // extract the address value
    if (write_page == NULL) {
      write_page = (byte *)malloc(256);                                     // allocate the page buffer
    }
    write_page_used = 0;
    Serial.println("OK");
    return ST_WRITE_PAGE;
  }
  return ST_WAIT;
}

/* this needs to be called multiple times (16) reading in 16 bytes at a time */
static int state_write_page (int state, String line) {
  byte data = 0;

  if (line.length() < 31) {                                 // minimum length of a valid line
    Serial.println("INVALID LENGTH"); 
    return ST_WAIT;
  }

  char *ch = (char *)line.c_str();                          // get the start of the line
  while (*ch != 0) {                                        // stop at the end of the line
    data <<= 4;
    if (*ch >= '0' && *ch <= '9')                           // this is a digit
      data |= (*ch++ - '0') & 0xF;
    else                                                    // else it must be an uppercase letter
      data |= (*ch++ - 'A' + 10) & 0xF;
    if ((*ch == ',') || (*ch == 0)) {                       // this is the end of a byte
      write_page[write_page_used++] = data;
      data = 0;
      if (*ch == ',') ch++;
    }
  }

  Serial.println("OK");
  if (write_page_used == 256) return ST_WRITE_CRC;          // after getting a whole page, start the check for the CRC
  else return ST_WRITE_PAGE;
}


/* 
Found this here https://stackoverflow.com/questions/69369408/calculating-crc16-in-python-for-modbus 
converted from python
*/
static int modbusCrc (byte *page) {
  unsigned short crc = 0xFFFF;
  for (int x = 0; x < 256; x++) {
    crc ^= page[x];
    for (int i = 0; i < 8; i++) {
      if (crc & 1) {
        crc >>= 1;
        crc ^= 0xA001;
      }
      else {
        crc >>= 1;
      }
    }
  }
  return crc;
}

static int state_write_crc (int state, String line) {
  /* find the CRC value */
  if (line.startsWith("CRC")) {                                 // There is a CRC header
    int index = line.indexOf(":");
    String text = line.substring(index + 1);
    unsigned short crc = strtol(text.c_str(), NULL, 16);        // Get the CRC from the source
    unsigned short calculated = modbusCrc (write_page);         // Calculate the CRC of the received data
    if (crc == calculated) {                                    // Both the crc are the same
      spi_enable();
      MYSPI.transfer(SPI_ENABLE_WRITE);                         // enable the write
      digitalWrite (MYSPI_SS, HIGH);
      digitalWrite (MYSPI_SS, LOW);
      MYSPI.transfer(SPI_PAGE_PROGRAM);                         // send the page
      MYSPI.transfer((address >> 16) & 0xFF);
      MYSPI.transfer((address >> 8) & 0xFF);
      MYSPI.transfer(address & 0xFF);
      for (int x = 0; x < 256; x++) {
        MYSPI.transfer(write_page[x]);
      }
      digitalWrite (MYSPI_SS, HIGH);
      spi_wait_busy();                                          // wait for the write to finish
      spi_disable();
      Serial.println("OK");
      free(write_page);                                         // free the resources
      write_page = NULL;
      write_page_used = 0;
    }
    else {
      Serial.println("BAD CRC");
    }
  }
  else {
    Serial.println("INVALID HEADER");
  }
  return ST_WAIT;
}

/** the array of function pointers for the state machine **/
int (*state_functions[]) (int, String) = {
  state_wait,
  state_read,
  state_write,
  state_write_page,
  state_write_crc
};

// the loop routine runs over and over again forever:
void loop() {
  unsigned long now = millis();
  static int state = ST_WAIT;
  static unsigned long last = millis();   // print timer


  if (state == ST_WAIT) {                                   // in idle mode always reset the timeout
    last = now;
  }
  else {
    if (now - last >= 30000) {                              // 30 second timeout
      last = now;
      state = ST_WAIT;                                      // been idle for too long reset the FSM
      //Serial.println("reset");
    }
  }

  /** check if there is serial input **/
  if (Serial.available() > 0) {                             // There is serial data to read
    static String line = "";
    static String incomming = "";
    last = now;                                             // got something! reset the timeout
    byte recv = Serial.read();                              // get the serial data
    if ((recv == '\r') || (recv == '\n')) {                 // end of a line
      if (incomming.length() > 0) {                         // there is text in the line
        line = incomming;                                   // cache the line
        incomming = "";                                     // clear the line buffer
        int maximum = sizeof(state_functions) / sizeof(int(*)(int));
        if (state < maximum)  {                             // the state is within bounds
          state = state_functions[state](state,line);       // call the state function
          //Serial.print("state:");Serial.println(state);
        }
        else {
          Serial.print("INVALID STATE ");
          Serial.println(state);
        }
      }
    }
    else {
      if ((recv >= 'a') && (recv <= 'z')) {
        recv &= ~(1 << 5);                                  // convert to uppercase
      }
      incomming += (char)recv;
    }
  }
}