#include "EEG1.h"
#include "EEG1_Definitions.h"
#include <SoftwareSerial.h>
#include <SPI.h>
#include <FunctionalInterrupt.h>

// start of simulation declarations
#define ISR_TRIGGER_GPIO 39
#define PULSE_OUTPUT_GPIO 32

// Scale factor for data
#define GAIN 1
#define scaleFactor 1000000*((4.5/(2^23)-1)/GAIN)
#define testScale 0.001*4.5/2.4

// Clock Signal
#define tCLK 0.0005     // 2 MHz Clock signal, 500 ns

int sampleFreq = 250; // to 500. A value of 128 will give 512 samples
int factor = 1000/sampleFreq/2;

// generate output pulses to emulate DRDY from ADS
// this should be placed in multiple places to emulate an unsynchronised DRDY
#define GENERATE_DRDY  digitalWrite(PULSE_OUTPUT_GPIO, (millis() / factor) % 2);
//#define GENERATE_DRDY  digitalWrite(PULSE_OUTPUT_GPIO, FALLING)
// end of simulation declarations

EEG1 EEG;

// Functions 
const int  WAKEUP  = 0x02; // Wake-up from standby mode
const byte STANDBY = 0b00000100; // Enter Standby mode
const byte RESET   = 0b00000110; // Reset the device
const byte START   = 0b00001000; // Start and restart (synchronize) conversions
const byte STOP    = 0b00001010; // Stop conversion
const byte RDATAC  = 0b00010000; // Enable Read Data Continuous mode (default mode at power-up)
const byte SDATAC  = 0b00010001; // Stop Read Data Continuous mode
const byte RDATA   = 0b00010010; // Read data by command; supports multiple read back
const byte TEST1   = 0b00000000;
const byte RREG    = 0x20;// (also = 00100000) is the first opcode that the address must be added to for RREG communication
const byte WREG    = 0x40;// 01000000 in binary (Datasheet, pg. 35)

double data;
const byte led = 2;
byte registerData[24];         // array is used to mirror register data
byte channelDataRaw[24];
long channelData[4];
long ads[4];
int sampleCounter = 0;
//byte RREG(byte,int);            // read one ADS register

bool channelDataAvailable;

 /* //SPI pin numbers:
 SCK   18  // Serial Clock.
 MISO  19  // Master In Slave Out.  (DOUT)
 MOSI  23  // Master Out Slave In.  (DIN)
 SS    5  // Slave Select*/

 //ADS1299-X Control Pin Declarations
 const int SCLK = 30;  // Serial Clock.
 const int STARTPIN = 12;
 const int ADS_RST = 13;
 const int PWDN = 3;
 const int ADS_DRDY = 4;         // ADS data ready GPIO 36

void IRAM_ATTR ADS_DRDY_Service()
 {
//  if (bitRead(ESP32_DRDY, 0) == 0)
  if (bitRead(ADS_DRDY, 0) == 0)
  {
//    board.channelDataAvailable = true;
      channelDataAvailable = true;
  }
}

int devicebyte = 0;

void setup() {
  Serial.begin(115200);
  Serial.flush();
  delay(1000);

    while(!Serial);

//  Start Pin -- Pull Start Pin High in order to access ADS1299-X data - DRDY toggles at 250Hz
  pinMode(STARTPIN,OUTPUT);
  digitalWrite(STARTPIN,HIGH); 

  //SPI Communication Setup with ADS1299-X  
  // Chip Select Pin 
  pinMode(CS_ADS, OUTPUT);
  digitalWrite(CS_ADS, HIGH);

  //DRDY Pin - Drops LOW if Data is ready 
  pinMode(ADS_DRDY, INPUT);
  digitalWrite(ADS_DRDY, HIGH);
  
  // Set up SPI
  SPI.begin();
  SPI.setFrequency(2000000);    // Set 4 MHz for ADS
  SPI.setDataMode(SPI_MODE1);   // Datasheet pg. 12: CPOL = 0 and CPHA = 1 (SPI MODE 1)

  //Powering Up Sequence
  DeviceStartUp();
  Serial.println("Device power up complete...");
  delayMicroseconds(2);

  // Get Device ID 
  devicebyte = getDeviceID();
  Serial.println();

  // Print default settings for registers
  EEG.printADSregisters(CS_ADS);

  Serial.println();

  //Writing Channel Configuration
  writeRegisterConfig();
  delayMicroseconds(2);

   Serial.println();

  EEG.START();
  delay(1);
  EEG.RDATAC();
  delay(1);

  testSignals();
  Serial.println("Register Print After TestSignals:");
  EEG.printADSregisters(CS_ADS);

}

// --------------------------- LOOP ---------------------------------------------------
void loop() {

  updateChannelData();

}

// ------------------------- Device Start up Procedure -------------------------------------

void DeviceStartUp(){
  
  //PWDN and RESET PIN set HIGH
 
  pinMode(PWDN, OUTPUT);
  digitalWrite(PWDN, HIGH);
  
  pinMode(ADS_RST, OUTPUT);
  digitalWrite(ADS_RST, HIGH);
  
  //Delay for 1 second, for power up sequence 
  delay(1000);

  //RESET PULSE
  digitalWrite(ADS_RST,LOW);
  delayMicroseconds(1); //2 tclk cycles
  digitalWrite(ADS_RST,HIGH);

  //Wait to Settle 16-18 tclk
  delayMicroseconds(18*tCLK);

  //Send SDATAC
  EEG.SDATAC();
  delayMicroseconds(4*tCLK); //delay 4 tCLK cycles
  
  }

  // ----------------------- Retrieving Device ID function ---------------------------------------------------
int getDeviceID(){
  
  //Delay 
  delay(100);
 
  // Begin SPI 
  digitalWrite(CS_ADS,LOW); //Pull CS low to communicate
  SPI.transfer(RREG); // Read Register 00100000
  delayMicroseconds(2);
  
  SPI.transfer(ID_REG); //0x00 ID register 00000000
  delayMicroseconds(2);
  byte data = SPI.transfer(ADS_ID); // Binary value for device ID, 4CH = 0b???11100
  delayMicroseconds(2);
  
  digitalWrite(CS_ADS, HIGH); // Pull CS HIGH to disable communication between Teensy and ADS1299-X
  return data;
  }

  // ------------------------- Write Registers Configuration -------------------------------------

void writeRegisterConfig() {
 // ADS1299-4 Datasheet pg 62:
 
  SPI.beginTransaction(SPISettings(2000000,MSBFIRST,SPI_MODE1));
  digitalWrite(CS_ADS,LOW);           //Pull CS LOW to start communication
 
  SPI.transfer(SDATAC);
  delayMicroseconds(4*tCLK); //delay 4 tCLK cycles

  //Using internal reference i.e BIASREF siganl (AVDD + AVSS)/2
  WREGConf(CONFIG3, 0xE0); 
  delayMicroseconds(2);
  
 // Set Device for f_mod/4096 (250 SPS)
  WREGConf(CONFIG1, 0x96);    WREGConf(CONFIG2, 0xC0);
  delayMicroseconds(2);
  
  // Set Channels to Input Short
  WREGConf(CH1SET, 0x01);  WREGConf(CH2SET, 0x01);
  WREGConf(CH3SET, 0x01);  WREGConf(CH4SET, 0x01);
  delayMicroseconds(2);

  WREGConf(MISC1, 0x00);
  delayMicroseconds(2);
  
  EEG.RREGS(ID_REG, CONFIG4, CS_ADS);
  delayMicroseconds(2);
  digitalWrite(CS_ADS,HIGH); //Pull CS HIGH to stop communication
  }

// --------------------------- Write channel configuration procedure--------------------------
 
void WREGConf(byte _address, byte _regValue){
   
  byte opcodeW = _address + 0x40;     // Write to one register with address (WREG = 0x40),  WREG expects 010rrrrr where rrrrr = _address
  SPI.transfer(opcodeW);              // Send WREG command and address
  SPI.transfer(0x00);                 // Send dummy byte to ADS. Send number of registers to read -1
  SPI.transfer(_regValue);            // Write register value

  registerData[_address] = _regValue; // Update mirror array
} 

// -------------------------------- Setup Test Signals -----------------------------------------

void testSignals(){
  digitalWrite(CS_ADS,LOW);           //Pull CS LOW to start communication
  delayMicroseconds(4*tCLK); //delay 4 tCLK cycles
  SPI.transfer(SDATAC);

  // Set Device for f_mod/4096 (250 SPS)
//  WREGConf(CONFIG2, 0xD3);              // For Internal square signals: Set CONFIG2.BIT1 and CONFIG2.BIT0 to '1' (TI Forum reply)
  WREGConf(CONFIG2, 0xD0);

  // CONFIG3
  WREGConf(CONFIG3, 0xEC);      // Internal reference buffer & BIASREF signal generated internally

  // CONFIG1: Multiple readback mode
  WREGConf(CONFIG1, 0x96);
 
//  // Set Channels to Test Signals: Changed 0D to 05
  WREGConf(CH1SET, 0x05);  WREGConf(CH2SET, 0x05);
  WREGConf(CH3SET, 0x05);  WREGConf(CH4SET, 0x05);

  digitalWrite(CS_ADS, HIGH); // Pull CS HIGH to disable communication between Teensy and ADS1299-X
  delayMicroseconds(4*tCLK); //delay 4 tCLK cycles

  EEG.RDATAC();
  delayMicroseconds(4*tCLK); //delay 4 tCLK cycles 
 }

  // ------------------------------- Update Channel Data -------------------------------------

void updateChannelData(){
  byte inByte;
  int byteCounter = 0; 
  
  int numChan = 4;  //assume 4 channel.  If needed, it automatically changes to 16 automatically in a later block.
  long stat_1[1];

  long channelDat;

 if(digitalRead(ADS_DRDY) == LOW){
  digitalWrite(CS_ADS, LOW);        //  open SPI
  delayMicroseconds(2);
  
  // READ CHANNEL DATA FROM FIRST ADS
  for(int i=0; i<3; i++){     //  read 3 byte status register from ADS 1 (1100+LOFF_STATP+LOFF_STATN+GPIO[7:4])
    inByte = SPI.transfer(0x00);
    stat_1[i] = (stat_1[i] << 8) | inByte;
  } 
  
  for(int i = 0; i < 4; i++){
    for(int j=0; j < 3; j++){   //  read 24 bits of channel data from 1st ADS in 8 3 byte chunks
      inByte = SPI.transfer(0x00);
//      channelDataRaw[byteCounter] = inByte;             // Raw data array
//      byteCounter++;      
      channelData[i] = (channelData[i]<<8) | inByte;    // Int data array
    }
  }  
  
  digitalWrite(CS_ADS, HIGH);       //  close SPI
  delayMicroseconds(2);
 
  //reformat the numbers
//  for(int i = 0; i < 4; i++){     // convert 3 byte 2's compliment to 4 byte 2's compliment 
//    if(bitRead(channelData[i],23) == 1){  
//      channelData[i] |= 0xFF000000;
//      ads[i] = channelData[i];
//    }else{
//      channelData[i] &= 0x00FFFFFF;
//      ads[i] = channelData[i];
//    }    
//	printChannelData();
//  }

for(int i = 0; i < 24; i++){
    if(bitRead(stat_1[i],23) == 1){  
    stat_1[i] |= 0xFF000000;
      } else{
    stat_1[i] &= 0x00FFFFFF;
      } 
   Serial.println(stat_1[i], HEX);
    }
 
  }
}

 // ------------------------------- Print Channel Data -------------------------------------
  
void printChannelData(){
//  Serial.println(" ");
 for(int i = 0; i < 4; i++){  

  Serial.println(ads[i],HEX);
 }  
}