#include <avr/sleep.h>
#include <Wire.h>
#include "progmemData.h"
#include "MB85RC16.h"

#define FRAMsize 2048

void setup() {
  Wire.begin();
  Serial.begin(115200);
}

void loop() {
  // w8 for start
  while (digitalRead(SCL) == HIGH);
  uint32_t startmillis = millis();
  while (digitalRead(SCL) == LOW);
  if (millis()-startmillis > 3000) { 
    delay(100);
//  dumpFRAM()
//  restoreBackupFRAM();
    trialPatch();         // 2160min. trial time
    bandwidthPatch(0, 4); // channel 0, 250Mhz
    bandwidthPatch(1, 4); // channel 1, 250Mhz
//  timeBase2nsPatch();   // 2ns time base
    timeBase1nsPatch();   // 1ns time base
    restoreCrc();         // fix CRC32
  } 

  // sleepmode
  SMCR = (1<<SM1) | (1<<SE);
  sleep_cpu();
}

void dumpFRAM() {
  uint16_t i = 0;

  do {
    readBytes(i, 8);
    while (Wire.available()) {
      Serial.write(Wire.read());
      i++;
    }
  } while (i<FRAMsize);
}

void restoreBackupFRAM() {
  uint16_t i=0, j;

  do {
    setAddress(i);
    for (j=0; j<8; j++) { 
      Wire.write(pgm_read_byte_near(backupFRAM + i));
      i++;
    }
    Wire.endTransmission();  
  } while (i<FRAMsize); 
}

void bandwidthPatch(uint8_t channel, uint8_t bandwidth) {
  // bandwith 0=20Mhz, 1=OFF, 2=100Mhz, 3=200Mhz, 4=250Mhz

  if (channel > 1 || bandwidth > 4) return;
  if (channel == 0) setAddress(0x90);
  else setAddress(0xCC);
  Wire.write(bandwidth);
  Wire.endTransmission();  
}

void timeBase2nsPatch() {
  setAddress(0x4A);
  Wire.write(0x02);
  Wire.write(0x00);
  Wire.endTransmission();  
  setAddress(0x56);
  Wire.write(0x01);
  Wire.write(0x00);
  Wire.endTransmission();  
  setAddress(0x5A);
  Wire.write(0xD0);
  Wire.write(0x07);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.endTransmission();  
  setAddress(0x62);
  Wire.write(0xE8);
  Wire.write(0x03);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.endTransmission();  
} 

void timeBase1nsPatch() {
  setAddress(0x4A);
  Wire.write(0x01);
  Wire.write(0x00);
  Wire.endTransmission();  
  setAddress(0x56);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.endTransmission();  
  setAddress(0x5A);
  Wire.write(0xE8);
  Wire.write(0x03);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.endTransmission();  
  setAddress(0x62);
  Wire.write(0xF4);
  Wire.write(0x01);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.endTransmission();  
} 

void trialPatch() {
  setAddress(0x7F6);
  Wire.write(0x49);
  Wire.write(0x0A);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.endTransmission();  
} 

uint32_t crcUpdate(uint32_t crc, uint8_t data) {
  uint8_t tbl_idx;

  tbl_idx = crc ^ (data >> (0 * 4));
  crc = pgm_read_dword_near(crc_table + (tbl_idx & 0x0f)) ^ (crc >> 4);
  tbl_idx = crc ^ (data >> (1 * 4));
  crc = pgm_read_dword_near(crc_table + (tbl_idx & 0x0f)) ^ (crc >> 4);
  return crc;
}

void restoreCrc() {
  const uint16_t crcEnd = FRAMsize-10; 
  uint32_t crc = 0xFFFFFFFF;
  uint16_t i = 0;

  do {
    readBytes(i, 8);
    while (Wire.available() && i<crcEnd) {
      crc = crcUpdate(crc, Wire.read());
      i++;
    }
  } 
  while (i<crcEnd);
  crc = ~crc;

  // clear Buffer
  while (Wire.available()) Wire.read();  

  // write crc32
  setAddress(0x7FC);
  Wire.write((uint8_t *)&crc, sizeof(crc));
  Wire.endTransmission();    
}
