/********************************************************************\

  Name:         mu3e_hv.c
  Created by:   Stefan Ritt


  Contents:     Application specific (user) part of
                Midas Slow Control Bus protocol 
                for Mu3e Negative and Positive HV box

\********************************************************************/

#include <stdio.h>
#include <stdlib.h>             // for atof()
#include <string.h>
#include <math.h>
#include "mscbemb.h"

extern bit FREEZE_MODE;
extern bit DEBUG_MODE;

char code node_name[] = "HV";

/* declare number of sub-addresses to framework */
unsigned char idata _n_sub_addr = 1;

bit flush_flag;

sbit SCK         = P0^0;  // Serial Clock (output)
sbit MISO        = P0^1;  // Master In / Slave Out (input)
sbit MOSI        = P0^2;  // Master Out / Slave In (output)

sbit NCS_DAC     = P0^3;  // /CS DAC
sbit NHV_ENABLE  = P0^7;
sbit NCS_ADC_HV  = P1^3;  // /CS HV ADC
sbit EN_HV       = P1^4;  // CW enable
sbit NCS_ADC_CUR = P1^5;  // /CS current ADC

sbit HV_ON       = P1^6;

sbit EN_OUTA     = P2^3;  // HV switches A-D, inverse order!
sbit EN_OUTB     = P2^2;
sbit EN_OUTC     = P2^1;
sbit EN_OUTD     = P2^0;

sbit POS_NEG     = P2^6;  // Board identification bit

#define DIVIDER     ((1E6+15000)/(15000))
#define DIVIDER_OFS 2.5
#define GND_OFS     5 // 5 V output generates 0V relative to GND

#define R_SHUNT_N 10000
#define R_SHUNT_P  1000

// delay for opto-coupler in microseconds
#define OPT_DELAY 1

/*---- Define variable parameters returned to CMD_GET_INFO command ----*/

/* data buffer (mirrored in EEPROM) */

struct {
   float hv;
   float hv_meas;
   unsigned char enabled;
   unsigned char on_0;
   unsigned char on_1;
   unsigned char on_2;
   unsigned char on_3;
   float i_0;
   float i_1;
   float i_2;
   float i_3;
   float hv_max;
   float hv_set;
   float i_ofs;
} xdata user_data;


MSCB_INFO_VAR code vars[] = {
   4, UNIT_VOLT,            0, 0,    MSCBF_FLOAT,                "HV",      &user_data.hv,
   4, UNIT_VOLT,            0, 0,    MSCBF_FLOAT,                "HVMeas",  &user_data.hv_meas,
   1, UNIT_BOOLEAN,         0, 0,    0,                          "Enabled", &user_data.enabled,
   1, UNIT_BOOLEAN,         0, 0,    0,                          "On0",     &user_data.on_0,
   1, UNIT_BOOLEAN,         0, 0,    0,                          "On1",     &user_data.on_1,
   1, UNIT_BOOLEAN,         0, 0,    0,                          "On2",     &user_data.on_2,
   1, UNIT_BOOLEAN,         0, 0,    0,                          "On3",     &user_data.on_3,
   4, UNIT_AMPERE, PRFX_MICRO, 0,    MSCBF_FLOAT,                "I0",      &user_data.i_0,
   4, UNIT_AMPERE, PRFX_MICRO, 0,    MSCBF_FLOAT,                "I1",      &user_data.i_1,
   4, UNIT_AMPERE, PRFX_MICRO, 0,    MSCBF_FLOAT,                "I2",      &user_data.i_2,
   4, UNIT_AMPERE, PRFX_MICRO, 0,    MSCBF_FLOAT,                "I3",      &user_data.i_3,
   4, UNIT_VOLT,            0, 0,    MSCBF_FLOAT | MSCBF_HIDDEN, "HVMax",   &user_data.hv_max,
   4, UNIT_VOLT,            0, 0,    MSCBF_FLOAT | MSCBF_HIDDEN, "HVSet",   &user_data.hv_set,
   4, UNIT_AMPERE, PRFX_MICRO, 0,    MSCBF_FLOAT | MSCBF_HIDDEN, "IOfs",    &user_data.i_ofs,
   0
};

MSCB_INFO_VAR *variables = vars;

bit turn_on;

/********************************************************************\

  Application specific init and inout/output routines

\********************************************************************/

void user_write(unsigned char index) reentrant;
void write_gain(void);
void set_hv(float value);

/*---- User init function ------------------------------------------*/

extern SYS_INFO idata sys_info;

void user_init(unsigned char init)
{
   if (init) {
      user_data.hv = 0;
      user_data.hv_max = 150;
      user_data.hv_set = 0;
      user_data.on_0 = 1;
      user_data.on_1 = 1;
      user_data.on_2 = 1;
      user_data.on_3 = 1;
   }

   SFRPAGE = LEGACY_PAGE;
   P0MDOUT = 0x5D; // SCLK, MOSI, CS_DAC, DI, DE_RE push-pull
   P1MDOUT = 0xFB; // TEMP_OUT, TEMP_OUT_EN, CS_HV_ADC, EN_HV, CS_ADC,
                   // HV ON, STATUS_OK push-pull
   P2MDOUT = 0x0F; // EN_OUTX push-pull
   
   turn_on   = 1;
   set_hv(0);
   user_data.hv_set = 0;
   
   EN_OUTA   = 0; // turn off all outputs
   EN_OUTB   = 0;
   EN_OUTC   = 0;
   EN_OUTD   = 0;
   
   EN_HV     = 0; // disable CW

   if (POS_NEG)
      strcpy(sys_info.node_name, "HV+");
   else
      strcpy(sys_info.node_name, "HV-");
}


/*---- User write function -----------------------------------------*/

/* buffers in mscbmain.c */
extern unsigned char xdata in_buf[300], out_buf[300];

#pragma NOAREGS

void user_write(unsigned char index) reentrant
{
   if (index == 0) {
      if (user_data.hv < 0)
         user_data.hv = 0;
      if (user_data.hv > user_data.hv_max)
         user_data.hv = user_data.hv_max;
   }

   if (index == 3)
      EN_OUTA = user_data.on_0;
   if (index == 4)
      EN_OUTB = user_data.on_1;
   if (index == 5)
      EN_OUTC = user_data.on_2;
   if (index == 6)
      EN_OUTD = user_data.on_3;

   if (index == 11)
      if (user_data.hv_max > 150)
         user_data.hv_max = 150;
}

/*---- User read function ------------------------------------------*/

unsigned char user_read(unsigned char index)
{
   if (index == 0);
   return 0;
}

/*---- User function called vid CMD_USER command -------------------*/

unsigned char user_func(unsigned char *data_in, unsigned char *data_out)
{
   /* echo input data */
   data_out[0] = data_in[0];
   data_out[1] = data_in[1];

   return 2;
}

/*---- DAC functions -----------------------------------------------*/

void write_dac(unsigned short value)
{
unsigned char i, m, b;
   
   NCS_DAC = 0; // chip select
   delay_us(OPT_DELAY);
   watchdog_refresh(1);
   
   // command 3: write and update
   for (i=0,m=8 ; i<8 ; i++) {
      SCK = 0;
      MOSI = (3 & m) > 0;
      delay_us(OPT_DELAY);
      SCK = 1;
      delay_us(OPT_DELAY);
      m >>= 1;
      watchdog_refresh(1);
   }

   // MSB
   b = value >> 8;
   for (i=0,m=0x80 ; i<8 ; i++) {
      SCK = 0;
      MOSI = (b & m) > 0;
      delay_us(OPT_DELAY);
      SCK = 1;
      delay_us(OPT_DELAY);
      m >>= 1;
      watchdog_refresh(1);
   }

   // LSB
   b = value & 0xFF;
   for (i=0,m=0x80 ; i<8 ; i++) {
      SCK = 0;
      MOSI = (b & m) > 0;
      delay_us(OPT_DELAY);
      SCK = 1;
      delay_us(OPT_DELAY);
      m >>= 1;
      watchdog_refresh(1);
   }

   NCS_DAC = 1; // remove CS
   delay_us(OPT_DELAY);
}

/*---- HV ADC functions --------------------------------------------*/

unsigned char read_adc_hv(float *v)
{
unsigned char i;
long d;
float f;
static unsigned int last = 0;

   // only measure once every 200 ms
   if (time() < last+20)
      return 0;
   last = time();
   
   SCK = 0;
   MOSI = 1;
   NCS_ADC_HV = 0; // chip select
   delay_us(OPT_DELAY);
   
   if (MISO == 1) { // check /EOC
      NCS_ADC_HV = 1; // remove chip select
      delay_us(OPT_DELAY);
      return 0;
   }
   
   for (i=0,d=0 ; i<32 ; i++) {
      delay_us(OPT_DELAY);
      SCK = 1;
      d = (d << 1) | MISO;
      delay_us(OPT_DELAY);
      SCK = 0;
      MOSI = 0;
   }
   
   SCK = 1;
   MOSI = 1;   
   NCS_ADC_HV = 1; // remove CS
   
   if ((d >> 28) == 0x03) {        // overrange
      f = 1.25;                    // +FS
   } else if ((d >> 28) == 0x00) { // underrange
      f = -1.25;                   // -FS
   } else if ((d >> 29) == 0x01) { // over VREF/2
      d = d & 0x0FFFFFFF;          // discard sign bit
      f = d;
      f = f / (1l<<28) * 1.25;
   } else {                        // under VREF/2
      d = (d | 0xF0000000);
      f = d;
      f = f / (1l<<28) * 1.25;
   }
   
   if (POS_NEG) {
      f = 1.25 + f;
      f *= DIVIDER; // scale form reference to HV
   } else {
      f = 1.25 - f;
      f *= DIVIDER;
      f -= 2.5;
   }

   *v = f;
   return 1;
}

/*---- Current ADC functions ---------------------------------------*/

unsigned char ltc2488_cmd[] = {
   0xB0,
   0xB8,
   0xB1,
   0xB9
};


unsigned char read_adc_current(float *v)
{
static unsigned char channel = 0;
unsigned char i, cmd;
long d;
float f;
static unsigned int last = 0;

   // only measure once every 200 ms
   if (time() < last+20)
      return 0;
   last = time();
   
   SCK = 0;
   MOSI = 1;
   NCS_ADC_CUR = 0; // chip select
   delay_us(OPT_DELAY);
   watchdog_refresh(1);
   
   if (MISO == 1) { // check /EOC
      NCS_ADC_CUR = 1; // remove chip select
      delay_us(OPT_DELAY);
      return 0;
   }
   
   cmd = ltc2488_cmd[(channel+1) % 4];
   MOSI = (cmd & 0x80) > 0 ? 1 : 0;
   cmd <<= 1;
   
   for (i=0,d=0 ; i<24 ; i++) {
      delay_us(OPT_DELAY);
      SCK = 1;
      d = (d << 1) | MISO;
      delay_us(OPT_DELAY);
      SCK = 0;
      MOSI = (cmd & 0x80) > 0 ? 1 : 0;
      cmd <<= 1;
   }
   
   SCK = 1;
   MOSI = 1;   
   NCS_ADC_CUR = 1; // remove CS
   
   if (POS_NEG) {

      if ((d >> 20) == 0x03) {        // overrange (electically not possible)
         f = 0;
      } else if ((d >> 20) == 0x00) { // underrange
         f = 1.25;
      } else if ((d >> 21) == 0x01) { // over COM  (electically not possible)
         f = 0;
      } else {                        // under COM
         d = (d & 0x000FFFFF) | ((d & 1<<21) >> 1); // isolate the result and add the sign bit
         d = d >> 4;                 // remove last 4 bits (always zero)
         d = ((1l<<16) - d); // correct for the "higher current->lower voltage" relation
         f = ((float)d / (1l<<16)) * 1.25; // convert raw value into voltage
      }

      // convert voltage to current
      f = f / R_SHUNT_P * 1E6;

   } else {

      if ((d >> 20) == 0x03) {
         f = 1.65;
      } else if ((d >> 20) == 0x00) {
         f = -1.65;
      } else if ((d >> 21) == 0x01) {
         d = d & 0x000FFFFF;
         f = d;
         f = f / (1l<<20) * 1.25;
      } else {
         d = (d | 0xFFF00000);
         f = d;
         f = f / (1l<<20) * 1.25;
      }

      // correct for reference at voltage divider
      f += 1.25;

      // convert voltage to current
      f = f / R_SHUNT_N * 1E6;
   }
   
   // correct for voltage divider error
   f -= user_data.i_ofs;
   
   // round to two digits
   f = floor(f * 100 + 0.5) / 100;
   
   // channels are in reverse order
   v[3-channel] = f;

   // increment channel
   channel = (channel + 1) % 4;
   
   return 1;
}

/*---- User loop function ------------------------------------------*/

void set_hv(float voltage)
{
   float fraction;
   if (POS_NEG)
      fraction = voltage/DIVIDER/2.5; // scale voltage from 0...150 to 0...1
   else
      fraction = 1-(voltage-DIVIDER_OFS+GND_OFS)/DIVIDER/2.5;
   if (fraction < 0)
      fraction = 0;
   if (fraction > 1)
      fraction = 1;
   write_dac(fraction * 65535); // value is fraction of vref
}

void user_loop(void)
{
float value, diff;
   
   /* 3 s after boot turn on outputs (needed for CW to settle) */
   if (turn_on && time() > 300) {
      EN_OUTA = 1; // turn on all outputs
      EN_OUTB = 1;
      EN_OUTC = 1;
      EN_OUTD = 1;
      turn_on = 0;
   }
   
   if (read_adc_hv(&value)) {

      // correct for current on channel A
      if (POS_NEG)
         value -= user_data.i_0/1E6*R_SHUNT_P;
      else
         value -= user_data.i_0/1E6*R_SHUNT_N;

      // round to three digits
      value = floor(value * 1000 + 0.5) / 1000;

      if (value < 0)
         value = 0;

      user_data.hv_meas = value;

      if (user_data.hv == 0) {
      
         EN_HV = 0; // disable CW
         user_data.hv_set = 0;
         set_hv(0);
      
      } else {

         if (user_data.hv < 0)
            user_data.hv = 0;
         if (user_data.hv > 150)
            user_data.hv = 150;

         EN_HV = 1; // enable CW

         // measure difference and correct for half of it
         diff = user_data.hv_meas - user_data.hv;
         user_data.hv_set -= diff/2;

         if (user_data.hv_set < 0)
            user_data.hv_set = 0;
         if (user_data.hv_set > 155)
            user_data.hv_set = 155;

         set_hv(user_data.hv_set);
      }
   }
   
   read_adc_current(&user_data.i_0);

   // read enabled bit
   user_data.enabled = !NHV_ENABLE;
}