/*
   I2S bi-directional input and output buffered loopback example
   Released to the Public Domain by Cooper Dalrymple
*/
#include <Wire.h>
#include <I2S.h>

I2S i2s(INPUT_PULLUP);

#define SIZE 256
int16_t buffer[SIZE];

int16_t volume = 0;
size_t count;

void codec_transmit() {
  i2s.write((const uint8_t *)&buffer, count * sizeof(int16_t));
}

void codec_receive(){
  count = i2s.read((uint8_t *)&buffer, SIZE * sizeof(int16_t)) * sizeof(uint32_t) / sizeof(int16_t);
  size_t index = 0;
  volume = 0;
  while (index < count) {
    volume += buffer[index];
    buffer[index] = 0;
    index++;
  }
}

void cw(unsigned char first, unsigned char second){
  Wire1.beginTransmission(0x18);
  Wire1.write(first);
  Wire1.write(second);
  int result = Wire1.endTransmission();
  Serial.print(0x18, HEX);
  Serial.print(" ");
  Serial.print(first, HEX);
  Serial.print(" ");
  Serial.print(second, HEX);
  Serial.print(" : ");
  if(result == 0) {
    Serial.println("OK");
  } else {
    Serial.print("ERROR: ");
    Serial.println(result);
  }
  delay(5);
}

void cr(unsigned char first, size_t len){
   Wire1.beginTransmission(0x18);  
   Wire1.write(first);  // set register for read
   Wire1.endTransmission(false); // false to not release the line

   Wire1.requestFrom(0x18, len, true); // request bytes from register XY
   
   Serial.print(first, HEX);
   Serial.print(" ");

   byte buff[len];    
   Wire1.readBytes(buff, len);
   for (int i = 0; i < len; i++) {
     Serial.print(buff[i], HEX);
     Serial.print(" ");
     Serial.println(buff[i], BIN);
   }
}

void setup() {
  i2s.setSysClk(48000);

  Serial.begin(115200);

  delay(2000);

  Wire1.setSDA(2);
  Wire1.setSCL(3);
  Wire1.begin();

  delay(100);

  pinMode(19, OUTPUT); // MCLK enable
  digitalWrite(19, HIGH); // enable MCLK

  pinMode(20, OUTPUT); // CODEC reset
  digitalWrite(20, HIGH);


  // GENERAL
  cw(0x00, 0x00); // select page 0
  cw(0x01, 0x01); // soft reset
  cw(0x1b, 0x30); // select I2S with 32 bit word length
  cw(0x1d, 0b00000000); // disable loopback

  // POWER and CM
  cw(0x00, 0x01); // select page 1
  cw(0x01, 0x08); // disable weak (crude) AVdd connection to DVdd
  cw(0x02, 0x01); // enable internal AVdd LDO and enable analog blocks
  cw(0x09, 0b00001100); // power up LOL, LOR, power down MAL, MAR, HPL, HPR
  cw(0x0a, 0b00000000); // set full chip CM to 0.9V
  cw(0x47, 0b00110010); // analog input quick charge time 6.4ms

  // ROUTING
  cw(0x00, 0x01); // select page 1
  cw(0x34, 0b11000000); // route IN1L to LEFT_P with 20k input impedance
  cw(0x36, 0b11000000); // route CM to LEFT_M with 20k input impedance
  cw(0x37, 0b11000000); // route IN1R to RIGHT_P with 20k input impedance
  cw(0x39, 0b11000000); // route CM to RIGHT_M with 20k input impedance
  cw(0x3a, 0b00111100); // connect IN2, IN3 weakly to CM

  // GAIN
  cw(0x00, 0x01); // select page 1
  cw(0x3b, 0b00000000); // unmute left MICPGA, set gain to > 0db
  cw(0x3c, 0b00000000); // unmute right MICPGA, set gain to > 0db

  // VOLUME
  cw(0x00, 0x01); // select page 1
  cw(0x16, 0b01110101); // MUTE IN1L to HPL
  cw(0x17, 0b01110101); // MUTE IN1R to HPR

  // ADC
  cw(0x00, 0x00); // select page 0
  cw(0x12, 0x81); // NADC 1
  cw(0x13, 0x82); // MADC 2
  cw(0x14, 0x80); // OSR ADC 128
  cw(0x3d, 0x01); // ADC PRB_R1

  cw(0x00, 0x01); // select page 1
  cw(0x3d, 0b01100100); // ADC PTM_R3

  // DAC
  cw(0x00, 0x00); // select page 0
  cw(0x0b, 0x81); // NDAC 1
  cw(0x0c, 0x82); // MDAC 2
  cw(0x0d, 0x00); // OSR DAC 128
  cw(0x0e, 0x80); // OSR DAC 128
  cw(0x1b, 0b00000000); // word length 16bits
  cw(0x3c, 0x08); // PRB_P8

  cw(0x00, 0x01); // select page 1
  cw(0x7b, 0x01); // set REF charging time to 40ms
  //cw(0x14, 0x25); // set HP soft stepping for anti pop
  //cw(0x0a, 0x0B); // set input CM to 0.9V and LO to 1.65V
  
  // ROUTING
  cw(0x00, 0x01); // select page 1
  cw(0x0e, 0b00001000); // left DAC reconstruction filter routed to LOL
  cw(0x0f, 0b00001000); // right DAC reconstruction filter routed to LOR
  cw(0x03, 0b00000100); // DAC PTM_P3/4
  cw(0x04, 0b00000100); // DAC PTM_P3/4

  // LO GAIN
  cw(0x00, 0x01);
  cw(0x12, 0b00000010); // LOL gain 0dB
  cw(0x13, 0b00000010); // LOR gain 0dB

  // POWER UP
  // ADC
  cw(0x00, 0x00); // select page 0
  cw(0x51, 0b11000000); // power up ADC
  cw(0x52, 0b00000000); // unmute ADC
  cw(0x3f, 0b11010100); // power up and route left digital audio to left dac channel and right to right
  cw(0x40, 0x00); // unmute DAC digital volume

  // DAC VOLUME 0b00000000 = 0dB, 10000001 = -63.5dB, 0b00110000 = +24dB
  cw(0x00, 0x00); // select page 0
  cw(0x41, 0b00000000); // LEFT 
  cw(0x42, 0b00000000); // RIGHT
  
  // ADC VOLUME 0b1101000 = -12dB, 0b00000000 = 0dB, 0b0101000 = +20dB
  cw(0x00, 0x00); // select page 0
  cw(0x53, 0b00000000); // LEFT
  cw(0x54, 0b00000000); // RIGHT

  // STATUS FLAGS
  Serial.println("CODEC STATUS");
  cw(0x00, 0x00); // select page 0
  Serial.println("ADC Flags");
  cr(0x24, 1);
  Serial.println("DAC Flags");
  cr(0x25, 1);
  Serial.println("P0_42 - Sticky Flags");
  cr(0x2A, 1);

  //i2s.onTransmit(codec_transmit);
  //i2s.onReceive(codec_receive);

  i2s.setDOUT(15);
  i2s.setDIN(14);
  i2s.setBCLK(16); // Note: LRCLK = BCLK + 1
  i2s.setMCLK(18);
  i2s.setMCLKmult(256); // 12.288.000Hz

  i2s.swapClocks();
  i2s.setBitsPerSample(16);

  //i2s.setBuffers(6, SIZE * sizeof(int16_t) / sizeof(uint32_t));

  if(!i2s.begin(48000)){
    Serial.println("I2S error!");
    while(100);
  }

}
int32_t last = 0;
int frame = 0;
void loop() {
  int16_t l, r;
  i2s.read16(&l, &r);

  // float sine_pos = (2.0f * M_PI * 1000.0f * (float)frame) / (float)48000;
  // l = (int16_t)(sin(sine_pos) * 8192.0f);
  // r = l;

  i2s.write16(l, r);

  // volume += l + r;
  // if(millis() - last > 1000){
  //   Serial.println(volume);
  //   volume = 0;
  //   last = millis();

  //   // STATUS FLAGS
  //   Serial.println("CODEC STATUS");
  //   cw(0x00, 0x00); // select page 0
  //   Serial.println("ADC Flags");
  //   cr(0x24, 1);
  //   Serial.println("DAC Flags");
  //   cr(0x25, 1);
  //   Serial.println("P0_42 - Sticky Flags");
  //   cr(0x2A, 1);
  //   Serial.println("P0_43 - Interrupt Flags");
  //   cr(0x2B, 1);
  //   Serial.println("P0_44 - Sticky Flags");
  //   cr(0x2C, 1);

  // }
  frame++;
  if(frame == 48000) frame = 0;
}