#include "usbscope.h"

#define SCOPE50_HWRAMSIZE 3072 /* 0xC00 */
#define SCOPE50_SWRAMSIZE 3000

#define SCOPE50_POINTER_READ    0
#define SCOPE50_POINTER_WRITE   1

#undef DEBUG

struct channel_info {
  unsigned char upper;      /* upper nibble of reg 0x0 */
  float raw_offset;
  float o2dac_scale;
  int trig_offset;
  /*int ris_min;
  int ris_max;*/
} cinfo[MAXFD];

unsigned pretrigger;
unsigned startwdp;
int use_ris;
float corr_gain = 1.06;     /* independent of channel */
int baseclk;                /* 10, 12(.5), 25, or 50 - in MHz */
unsigned char baseclk_ix;   /* 3, 2, 1, 0 acc. to baseclk */

unsigned static char clamp(int val) {
  if(val < 0) return 0;
  else if(val > 255) return 255;
  else return val;
}

/* Doplnit RIS */
int scope50_acquisition_start(int fd) {
  if(use_ris) {
    unsigned char seq[] = {
      0x20,  cinfo[fd].upper | 0xC,
      0x16,  0,
      0x17,  0,
      0x27,  0,
      0x28,  0,
      0x20,  cinfo[fd].upper | 0x0,
      0x20,  cinfo[fd].upper | 0x1,   /* Run*/
      0x20,  cinfo[fd].upper | 0x3};  /* Arm */
    return writeregmulti(fd, seq, countof(seq));
  } else {
    unsigned char seq[] = {
      0x20,  cinfo[fd].upper | 0x8,
      0x16,  (unsigned char)(pretrigger & 0xFF),
      0x17,  (unsigned char)(pretrigger >> 8),
      0x27,  (unsigned char)(startwdp & 0xFF),
      0x28,  (unsigned char)(startwdp >> 8),
      0x20,  cinfo[fd].upper | 0xC,
      0x20,  cinfo[fd].upper | 0xF,
      0x20,  (cinfo[fd].upper | 0xF) & 0xDF,
      0x20,  cinfo[fd].upper | 0xF};
    return writeregmulti(fd, seq, countof(seq));
  }
}

int scope50_set_ris_mode(int fd, int state) {
  use_ris = state;
  return changereg(fd, 0x10, ~0x06, state?0x02:0);
}

int scope50_set_trig_master(int fd, int state) {
  if(state) cinfo[fd].upper |= 0x40;
  else cinfo[fd].upper &= ~0x40;
  return writereg(fd, 0x0, cinfo[fd].upper);
}

int scope50_get_ris_data(int fd, unsigned char *ret) {
  return readreg(fd, 0x11, ret);
}

int scope50_set_pretrig_depth(int samples) {
  //if(pct >= 0) pretrigger = (unsigned)(pct * SCOPE50_SWRAMSIZE);
  if(samples >= 0 && samples < SCOPE50_HWRAMSIZE-10) pretrigger = samples;
  else return 0;
  startwdp = SCOPE50_HWRAMSIZE - 10 - pretrigger;
  return 1;
}

int scope50_set_trigger_delay(int fd, unsigned val) {
  return writeregdouble(fd, 0x12, val);
}

int scope50_set_cal_source(int fd, int state) {
  return changereg(fd, 0xA, (unsigned char)~0x80, state?0x80:0);
}

/* Base ADC clock = 10, 12.5, 25, or 50 MHz
 * 10 MHz (only!) can be divided by the decimation ratios:
 *   1, 2, 4, 10, 20, 40, ..., 400000
 * => min sample interval = 20 ns (50 MHz)
 *    max sample interval = 40 ms (10 MHz : 4E5)
 */
int scope50_set_base_adc_clk(int fd, int clk) {
  baseclk = clk;
  switch(clk) {
    case 10: baseclk_ix = 3; break;
    case 12: baseclk_ix = 2; break;
    case 25: baseclk_ix = 1; break;
    default: baseclk_ix = 0;
  }
  //printf("clk %i -> %i\n", clk, baseclk_ix);
  return changereg(fd, 0xC, ~0x3, baseclk_ix);
}

int scope50_set_decimation_ratio(int fd, int dr) {
  unsigned char code = 0;
  while(dr >= 10) {
    dr /= 10;
    code += 4;
  }
  if(dr == 2) code += 1;
  else if(dr != 1) code += 2;
  //printf("dec %i -> %i\n", dr, code);
  return changereg(fd, 0xC, 0x3, code << 2);
}

/* Allowed values: 25, 50, 100, 250, ..., 1E7, 1.25E7, 2.5E7, 5E7 */
int scope50_set_sample_frequency(int fd, int freq, int *real) {
  int clk, dec, pow10;
  if(freq < 25) freq = 25;
  if(freq > 5E7) freq = 5E7;
  if(freq > 1E7) {
    dec = 1;
    freq /= 1E6;
    if(freq < 11) clk = 10;
    else if(freq < 20) clk = 12;
    else if(freq < 40) clk = 25;
    else clk = 50;
  } else {
    clk = 10;
    dec = 1E7 / freq;
  }
  for(pow10 = 1; dec >= 10; ) {
    pow10 *= 10;
    dec /= 10;
  }
  if(dec > 2) dec = 4;
  dec *= pow10;
  if(real) *real = clk * 1E6 / dec;
  if(!scope50_set_base_adc_clk(fd, clk)) return 0;
  if(!scope50_set_decimation_ratio(fd, dec)) return 0;
  return 1;
}

/* code parameter is optional */
int scope50_set_offset(int fd, float pct, unsigned char *code) {
  unsigned char tmp;
  tmp = clamp(cinfo[fd].raw_offset + 0x80*(pct/100)*cinfo[fd].o2dac_scale);
  if(code) *code = tmp;
  if(!writereg(fd, 0x3, 1)) return 0;
  if(!writereg(fd, 0x4, tmp)) return 0;
  return delaycycle(fd, 6);
}

int scope50_set_trig_thresh(int fd, float pct) {
  unsigned char tmp;
  tmp = clamp(cinfo[fd].trig_offset + 73*(pct/100));
  if(!writereg(fd, 0x3, 2)) return 0;
  return writereg(fd, 0x4, tmp);
}

int scope50_set_run(int fd, int state) {
  return changereg(fd, 0x0, ~0x1, state?0x1:0);
}

int scope50_set_arm(int fd, int state) {
  return changereg(fd, 0x0, ~0x2, state?0x2:0);
}

int scope50_set_adc_clk(int fd, int state) {
  return changereg(fd, 0x0, ~0x4, state?0x4:0);
}

int scope50_set_ctr_clk(int fd, int state) {
  return changereg(fd, 0x0, ~0x8, state?0x8:0);
}

int scope50_get_acquisition_state(int fd, int *state) {
  unsigned char res;
  if(!readreg(fd, 0x0, &res)) return 0;
  *state = res & 0x1;
  return 1;
}

int scope50_acquisition_end(int fd) {
  return writereg(fd, 0x0, cinfo[fd].upper & 0xF0);
}

int scope50_set_trig_type(int fd, int type) {
  return changereg(fd, 0x1, ~0x3, type);
}

int scope50_set_norm_trig(int fd, int state) {
  if(state) {
    if(!changereg(fd, 0x0, ~0x30, 0x30)) return 0;
    cinfo[fd].upper |= 0x30;
    scope50_set_pretrig_depth(-1); /* Use current value */
  } else {
    if(!changereg(fd, 0x0, ~0x30, 0x20)) return 0;
    cinfo[fd].upper &= ~0x30;
    cinfo[fd].upper |= 0x20;
    if(!scope50_set_trigger_delay(fd, 0)) return 0;
    scope50_set_pretrig_depth(0); /* No pre-trig */
  }
  return 1;
}

int scope50_setup_front_end(int fd, int range, int coupl, int ris) {
  unsigned char reg;
  if(!readreg(fd, 0x2, &reg)) return 0;
  if(ris) reg |= 0x20;
  else {
    reg &= 0x80;
    switch(coupl) {
      case SCOPE50_COUPLING_AC:
        reg |= 0x10;
        break;
      case SCOPE50_COUPLING_DC:
        reg |= 0x18;
        break;
      case SCOPE50_COUPLING_GND:
        reg &= 0xE0;
        range = SCOPE50_RANGE_300V;
    }
    reg |= 1 << range;
  }
  if(!writereg(fd, 0x2, reg & 0xF8)) return 0; /* Without amps */
  if(!delaycycle(fd, 5)) return 0;
  if(!writereg(fd, 0x2, reg)) return 0;
  if(!delaycycle(fd, 5)) return 0;
  return 1;
}

int scope50_set_ptr(int fd, int ptr, unsigned val) {
  return writeregdouble(fd, ptr?0x7:0x5, val);
}

int scope50_get_ptr(int fd, int ptr, unsigned *ret) {
  int i;
  unsigned char a, b, c;
  for(i = 0; i < 100; i++) {
    if(!readreg(fd, ptr?0x8:0x6, &a)) return 0;
    if(!readreg(fd, ptr?0x7:0x5, &b)) return 0;
    if(!readreg(fd, ptr?0x8:0x6, &c)) return 0;
    if(a == c) break;   /* The pointer is changing */
  }
  if(i == 100) *ret = 0;
  else *ret = ((((int)c)<<8)+b) & 0x7FFF;
  return 1;
}

int scope50_get_halt_ptr(int fd, unsigned *ret) {
  return readregdouble(fd, 0x14, ret);
}

int scope50_get_trig_ptr(int fd, unsigned *ret) {
  int r = readregdouble(fd, 0x18, ret);
  *ret &= 0x7FFF;
  return r;
}

int scope50_get_triggered_status(int fd, int *ret) {
  unsigned char res;
  if(!readreg(fd, 0x9, &res)) return 0;
  *ret = (res & 2) ? 1 : 0;
  return 1;
}

int scope50_get_samples_since_trigger(int fd, unsigned *ret) {
  unsigned a, b;
  int res;
  *ret = 0;
  if(!scope50_get_triggered_status(fd, &res)) return 0;
  if(res) return 1;
  if(!scope50_get_ptr(fd, SCOPE50_POINTER_WRITE, &a)) return 0;
  if(!scope50_get_trig_ptr(fd, &b)) return 0;
  if(a < startwdp && a > 10) return 1;
  *ret = (int)a - b;
  if(*ret < 0) *ret += SCOPE50_HWRAMSIZE;
  if(*ret) (*ret)--;
  return 1;
}

int scope50_get_buffer_blocks(int fd, float *data, int nblocks) {
  unsigned wdp;
  int i;
  unsigned char fld[512];
  if(!writereg(fd, 0xF, 0)) return 0;   /* Read 512 B */
  if(!scope50_get_halt_ptr(fd, &wdp)) return 0;
  if(!scope50_set_ptr(fd, SCOPE50_POINTER_READ, (wdp+1)%SCOPE50_HWRAMSIZE)) return 0;
  while(nblocks--) {
    if(!readregmulti(fd, 0xB, 512, fld)) return 0;
    for(i = 0; i < 512; i++) {
      *data = ((int)fld[i] - 0x80) * corr_gain;
      data++;
    }
  }
  return 1;
}

int scope50_get_buffer_blocks_raw(int fd, unsigned char *data, int nblocks) {
  unsigned wdp;
  int i;
  if(!writereg(fd, 0xF, 0)) return 0;   /* Read 512 B */
  if(!scope50_get_halt_ptr(fd, &wdp)) return 0;
  if(!scope50_set_ptr(fd, SCOPE50_POINTER_READ, (wdp+1)%SCOPE50_HWRAMSIZE)) return 0;
  for(i = 0; i < nblocks; i++)
    if(!readregmulti(fd, 0xB, 512, data + i*512)) return 0;
  return 1;
}

/* buffer blocks multi, incr, RIS */

int scope50_calibrate_offset(int fd) {
  unsigned char o1, o2;
  float a1, a2;
  unsigned char buf[100];
  int i;
  unsigned char seq[] = {
    0x20,  cinfo[fd].upper | 0x8,
    0x2F,  50,
    0x25,  0,
    0x26,  0,
    0x27,  0,
    0x28,  0,
    0x20,  cinfo[fd].upper | 0xC,
    0x20,  cinfo[fd].upper | 0xD,
    0x20,  cinfo[fd].upper | 0xD,
    0x20,  cinfo[fd].upper | 0x0};
  if(!scope50_setup_front_end(fd, 0, SCOPE50_COUPLING_GND, 0)) return 0;
  if(!scope50_set_norm_trig(fd, 0)) return 0;
  if(!scope50_set_base_adc_clk(fd, 50)) return 0;
  cinfo[fd].raw_offset = 0x80;  /* Assumption */
  cinfo[fd].o2dac_scale = 0.5;

  if(!scope50_set_offset(fd, 50, &o1)) return 0;
  if(!delaycycle(fd, 50)) return 0;
  if(!writeregmulti(fd, seq, countof(seq))) return 0;
  if(!readregmulti(fd, 0xB, 100, buf)) return 0;
  for(i = 0, a1 = 0; i < 100; i++)
    a1 += buf[i] - 0x80;
  a1 /= 100.0;

#ifdef DEBUG
  printf("o1 = %i\na1 = %f\n", o1, a1);
#endif

  if(!scope50_set_offset(fd, -50, &o2)) return 0;
  if(!delaycycle(fd, 50)) return 0;
  if(!writeregmulti(fd, seq, countof(seq))) return 0;
  if(!readregmulti(fd, 0xB, 100, buf)) return 0;
  for(i = 0, a2 = 0; i < 100; i++)
    a2 += buf[i] - 0x80;
  a2 /= 100.0;

#ifdef DEBUG
  printf("o2 = %i\na2 = %f\n", o2, a2);
#endif

  /*Original algorithm:
  cinfo[fd].o2dac_scale = (o1-o2) / (a1-a2);
  cinfo[fd].raw_offset = o2 - a2*cinfo[fd].o2dac_scale;
  cinfo[fd].o2dac_scale /= 1.09;*/
  /* New algorithm: */
  cinfo[fd].o2dac_scale = (o1-o2)/(a1-a2);
  cinfo[fd].raw_offset = (int)(1.1/2.-(o1*a2-o2*a1)/(a1-a2));

#ifdef DEBUG
  printf("%f\n%f\n", cinfo[fd].o2dac_scale, cinfo[fd].raw_offset);
#endif

  return 1;
}

int scope50_calibrate_trig(int fd) {
  unsigned char code, res;
  if(!scope50_set_offset(fd, 0, NULL)) return 0;
  if(!scope50_setup_front_end(fd, 0, SCOPE50_COUPLING_GND, 0)) return 0;
  cinfo[fd].trig_offset = 0x80;
  for(code = 100; code <= 156; code++) {
    if(!writereg(fd, 0x3, 2)) return 0;
    if(!writereg(fd, 0x4, code)) return 0;
    if(!delaycycle(fd, 1)) return 0;
    if(!readreg(fd, 0x9, &res)) return 0;
    if(!(res & 0x4)) {
      cinfo[fd].trig_offset = code;
      break;
    }
  }
  return 1;
}

/*int scope50_calibrate_ris(int fd) {
  int data[SCOPE50_HWRAMSIZE];
  int min, max;
  int i;
  unsigned char res;
  unsigned char seq[] = {
    0x20,  cinfo[fd].upper | 0xC,
    0x27,  0,
    0x28,  0,
    0x20,  cinfo[fd].upper | 0x0,
    0x20,  cinfo[fd].upper | 0x3,
    0x30,  3,
    0x80};
  if(!scope50_set_ris_mode(fd, 1)) return 0;
  if(!scope50_set_trig_master(fd, 1)) return 0;
  for(i = 0; i < 100; i++) {
    if(!writeregmulti(fd, seq, countof(seq))) return 0;
    if(!readreg(fd, 0x11, &res)) return 0;
    data[i] = (int)res - 0x80;
  }
  max = -128;
  min = 127;
  for(i = 0; i < 100; i++) {
    if(data[i] < 127 && data[i] > max) max = data[i];
    if(data[i] > -127 && data[i] < min) min = data[i];
  }
  cinfo[fd].ris_min = 0.75*min;
  cinfo[fd].ris_max = 0.75*max;
  if(!writereg(fd, 0x10, 0)) return 0;
  if(!scope50_set_trig_master(fd, 0)) return 0;
  return 1;
}*/

int scope50_init_scope(int fd, int master, float *raw, float *scale, int *trig) {
  cinfo[fd].upper = master?0x80:0;
  if(!writereg(fd, 0x0, cinfo[fd].upper)) return 0;
  if(!scope50_set_base_adc_clk(fd, 50)) return 0;
  if(!scope50_set_decimation_ratio(fd, 1)) return 0;
  /*cinfo[fd].ris_max = 127;
  cinfo[fd].ris_min = -128;
  if(!scope50_calibrate_ris(fd)) return 0;*/
  if(raw == NULL || scale == NULL || trig == NULL ||
      (*raw == 0 && *scale == 0 && *trig == 0)) {
    if(!scope50_calibrate_offset(fd)) return 0;
    if(!scope50_calibrate_trig(fd)) return 0;
  } else {
    cinfo[fd].raw_offset = *raw;
    cinfo[fd].o2dac_scale = *scale;
    cinfo[fd].trig_offset = *trig;
  }
  if(raw != NULL || scale != NULL || trig != NULL) {
    *raw = cinfo[fd].raw_offset;
    *scale = cinfo[fd].o2dac_scale;
    *trig = cinfo[fd].trig_offset;
  }
  if(!writereg(fd, 0x1, 0x20)) return 0;
  if(!writereg(fd, 0x1, 0x24)) return 0;
  if(!writereg(fd, 0x1, 0x04)) return 0;
  if(!writereg(fd, 0x1, 0x24)) return 0;
  if(!scope50_set_norm_trig(fd, 0)) return 0;
  if(!scope50_set_trig_type(fd, SCOPE50_TRIG_TYPE_EDGE_UP)) return 0;
  if(!usbtm_set_led_mode(fd, USBTM_LED_MODE_NORMAL)) return 0;
  if(!scope50_setup_front_end(fd, SCOPE50_RANGE_3V, SCOPE50_COUPLING_DC, 0)) return 0;
  if(!scope50_set_offset(fd, 0, NULL)) return 0;
  if(!scope50_set_trig_thresh(fd, 0)) return 0;
  use_ris = 0;
  return 1;
}