#include "usbwave.h"

struct channel_info {
  int cal_offset;
  int zero_offset_code;
} cinfo[MAXFD];

int wave12_set_pll(int fd, int m, int n, int u, int dly) {
  unsigned char reg, temp;
  unsigned int pllbits;
  double fout, ratio;
  int obmux = 2, bit;
  if(!readreg(fd, 0xA, &reg)) return 0;
  reg &= 0xE0;
  if(!writereg(fd, 0xA, reg | 0x10)) return 0;   /* PLL Mode Hi */
  if(!writereg(fd, 0xA, reg | 0x12)) return 0;   /* PLL Mode & PLL Shift Hi */
  fout = 12.5E6 * m/(n*u);
  ratio = (n*u)/(double)m;
  if(ratio == (int)ratio)
    obmux = 1, u = ratio;
  n--; m--; u--;
  pllbits = n | (m << 5) | (u << 11) | (obmux<<17) | (1<<20) | (dly<<22) | (1<<26);
  for(bit = 0; bit < 27; bit++) {
    temp = reg | 0x12 | (pllbits&1?0x4:0x0);
    if(!writereg(fd, 0xA, temp)) return 0;       /* Data in */
    if(!writereg(fd, 0xA, temp | 0x1)) return 0; /* Clock up */
    if(!writereg(fd, 0xA, temp)) return 0;       /* Clock down */
    pllbits >>= 1;
  }
  if(!writereg(fd, 0xA, reg | 0x10)) return 0;   /* Disable shift */
  if(!writereg(fd, 0xA, reg | 0x1A)) return 0;   /* Update */
  if(!writereg(fd, 0xA, reg | 0x10)) return 0;   /* Update done */
  return 1;
}

int wave12_write_gain_pot(int fd, unsigned code, unsigned char fe) {
  if(!writeregdouble(fd, 0x2, code)) return 0;
  if(!writereg(fd, 0x4, 0xB0)) return 0;
  if(!writereg(fd, 0x8, fe | 0x40)) return 0;
  return delaycycle(fd, 6);
}

int wave12_write_offset_pot(int fd, unsigned code, unsigned char fe) {
  if(!writeregdouble(fd, 0x5, code)) return 0;
  if(!writereg(fd, 0x7, 0xB0)) return 0;
  if(!writereg(fd, 0x8, fe | 0x40)) return 0;
  return delaycycle(fd, 6);
}

int wave12_set_offset(int fd, float off, float volt) {
  unsigned char fe;
  if(!readreg(fd, 0x8, &fe)) return 0;
  if(off < -10) off = -10;
  if(off > 10) off = 10;
  return wave12_write_offset_pot(fd, (unsigned)
      (off*0.9*511/30 + cinfo[fd].zero_offset_code + volt*cinfo[fd].cal_offset), fe);
}

int wave12_set_amplitude(int fd, float val, float off) {
  unsigned char fe;
  if(!readreg(fd, 0x8, &fe)) return 0;
  if(val < 0.0) val = 0.0;
  if(val > 10.0) val = 10.0;
  if(!wave12_write_gain_pot(fd, (unsigned)(1023 - 102.3*0.882555*val), fe)) return 0;
  return wave12_set_offset(fd, off, val);
}

int wave12_cal_count(int fd) {
  unsigned char reg;
  int i, c;
  for(i = c = 0; i < 10; i++) {
    if(!readreg(fd, 0x8, &reg)) return 0;
    if(!delaycycle(fd, 1)) return 0;
    if(reg & 0x10) c++;
  }
  return c;
}

int wave12_calibrate(int fd) {
  unsigned char reg;
  unsigned code;
  int i, c;
  /* min gain, integrator off, zero cross comp on */
  if(!wave12_write_offset_pot(fd, 0x1F0, 0x0)) return 0;
  if(!wave12_write_gain_pot(fd, 0x3FF, 0x0)) return 0;
  for(code = 0x1F0; code < 0x20E; code++) {
    if(!wave12_write_offset_pot(fd, code, 0x0)) return 0;
    if(!delaycycle(fd, 1)) return 0;
    if(wave12_cal_count(fd) == 10) {
      cinfo[fd].zero_offset_code = code-1;
      cinfo[fd].cal_offset = code-1;
      break;
    }
  }
  /* gain 9V, integrator on, zero cross comp on */
  if(!wave12_write_offset_pot(fd, 0x210, 0x4)) return 0;
  if(!wave12_write_gain_pot(fd, 0x180, 0x4)) return 0;
  if(!delaycycle(fd, 50)) return 0;
  code = 0x210;
  for(i = 0; i < 50; i++) {
    c = wave12_cal_count(fd);
    if(c == 0 || c == 10) break;
  }
  if(!readreg(fd, 0x8, &reg)) return 0;
  if(c == 0) {
    for(i = 1; i < 700; i++) {
      code++;
      if(!wave12_write_offset_pot(fd, code, reg)) return 0;
      if(!delaycycle(fd, 25)) return 0;
      if(wave12_cal_count(fd) == 10) {
        cinfo[fd].cal_offset = code-1;
        break;
      }
    }
  } else if(c == 10) {
    for(i = 1; i < 500; i++) {
      code--;
      if(!wave12_write_offset_pot(fd, code, reg)) return 0;
      if(!delaycycle(fd, 25)) return 0;
      if(wave12_cal_count(fd) == 0) {
        cinfo[fd].cal_offset = code-1;
        break;
      }
    }
  }
  cinfo[fd].cal_offset = (cinfo[fd].cal_offset - cinfo[fd].zero_offset_code)/9;
  /* integrator off, zero cross comp off */
  return writereg(fd, 0x8, 0x8);
}

int wave12_set_wave_type(int fd, int type) {
  switch(type) {
    case WAVE12_WAVE_TYPE_SINE:
      if(!writereg(fd, 0xB, 0x8)) return 0;
      return changereg(fd, 0x8, ~0x22, 0x20);
    case WAVE12_WAVE_TYPE_SQUARE:
      if(!writereg(fd, 0xB, 0x28)) return 0;
      return changereg(fd, 0x8, ~0x22, 0x22);
    case WAVE12_WAVE_TYPE_TRIANGLE:
      if(!writereg(fd, 0xB, 0xA)) return 0;
      return changereg(fd, 0x8, ~0x22, 0x20);
  }
  return 0;
}

int wave12_set_frequency(int fd, float val) {
  unsigned long uval;
  if(val < 0.2) val = 0.2;
  if(val > 1.2E7) val = 1.2E7;
  uval = (unsigned long)(val*(1<<26)/12.5E6) & 0xFFFFFFF;
  if(!writereg(fd, 0x0f, uval & 0xFF)) return 0;
  if(!writereg(fd, 0x10, ((uval >> 8) & 0x3F) | 0x40)) return 0;
  if(!writereg(fd, 0x11, (uval >> 14) & 0xFF)) return 0;
  if(!writereg(fd, 0x12, ((uval >> 22) & 0x3F) | 0x40)) return 0;
  if(!writereg(fd, 0x13, uval & 0xFF)) return 0;
  if(!writereg(fd, 0x14, ((uval >> 8) & 0x3F) | 0x80)) return 0;
  if(!writereg(fd, 0x15, (uval >> 14) & 0xFF)) return 0;
  if(!writereg(fd, 0x16, ((uval >> 22) & 0x3F) | 0x80)) return 0;
  return changereg(fd, 0x8, 0xFF, 0x20);
}

int wave12_set_phase(int fd, float val) {
  unsigned uval;
  if(val <= 0 || val >= 360) val = 0;
  uval = (unsigned)(val*4096.0/360.0) & 0xFFF;
  if(!writereg(fd, 0x17, uval & 0xFF)) return 0;
  if(!writereg(fd, 0x18, (unsigned char)((uval >> 8) & 0xF) - 0x40)) return 0;
  if(!writereg(fd, 0x19, uval & 0xFF)) return 0;
  if(!writereg(fd, 0x1A, (unsigned char)((uval >> 8) & 0xF) - 0x20)) return 0;
  return changereg(fd, 0x8, 0, 0x20);
}

int wave12_set_run(int fd, int state) {
  return changereg(fd, 0x0, 0xFE, state?0x01:0);
}

int wave12_set_arm(int fd, int state) {
  return changereg(fd, 0x0, 0xFD, state?0x02:0);
}

int wave12_set_trigger(int fd, int state) {
  return changereg(fd, 0x0, 0xDF, state?0x20:0);
}

int wave12_set_enable(int fd, int state) {
  return changereg(fd, 0x8, 0xFE, state?0x1:0);
}

int wave12_init_wave(int fd, int master, int *cal, int *zero) {
  cinfo[fd].cal_offset = 0;
  cinfo[fd].zero_offset_code = 0x1FF;
  if(!writereg(fd, 0x0, 0x4)) return 0;
  if(!writereg(fd, 0x1, 0x0)) return 0;
  if(!wave12_set_enable(fd, 0)) return 0;
  if(!wave12_set_amplitude(fd, 0, 0)) return 0;
  if(!writereg(fd, 0xB, 0x8)) return 0;
  if(!writereg(fd, 0xC, 0x22)) return 0;
  if(!wave12_set_frequency(fd, 1000)) return 0;
  if(!wave12_set_phase(fd, 0)) return 0;
  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(!usbtm_set_clock_master(fd, master)) return 0;
  if(!usbtm_set_trig_master(fd, master)) return 0;
  if(!wave12_set_trigger(fd, 0)) return 0;
  if(!wave12_set_arm(fd, 1)) return 0;
  if(!wave12_set_run(fd, 1)) return 0;
  if(cal == NULL || zero == NULL || (*cal == 0 && *zero == 0)) {
    if(!wave12_calibrate(fd)) return 0;
  } else {
    cinfo[fd].cal_offset = *cal;
    cinfo[fd].zero_offset_code = *zero;
  }
  if(cal != NULL && zero != NULL) {
    *cal = cinfo[fd].cal_offset;
    *zero = cinfo[fd].zero_offset_code;
  }
  if(!usbtm_set_led_mode(fd, USBTM_LED_MODE_NORMAL)) return 0;
  return 1;
}