/*
	v3.0

	New board.
	- Less input noise
	- High range, high precision output voltage by using 2 DACS
	- Automatic gain and offset selection
	- Faster loop (<100microseconds after waveform acquisition is completed)
*/

#include<ADuC7020.h>

#include<stdlib.h>

#include<stdio.h>

#include<string.h>

void My_IRQ_Handler(void);

///////////////////////////////////////
// VARIABLE AND OPTION DEFINITIONS
///////////////////////////////////////

short i = 0; // Dummy loop variables
// I2C stuff
unsigned char Byte_addr = 0;
int first = 1;
int i2c_cnt = 0;

// BigDat stores the latest valid waveform
// This is used so that if the acquisition is corrupted due to communication
// with the board only data but not BigDat is corrupted
#define BIGDAT_SZ 256
unsigned short BigDat[BIGDAT_SZ];
char text[512];

// DAC outputs
unsigned short Vout[4]; // DAC values, used via get_DACs and set_DACs
unsigned short Vlearn; // Coarse output of the board when in learn mode

double Vin, Vin_gnd; // Measurements of the signal and ground in the last waveform

unsigned short start, stop; // Points at which to start and stop measuring the signal
unsigned short start_gnd, stop_gnd; // Points at which to start and stop measuring the background
unsigned short Vset; // Value measured in learn mode to which we aim to stabilize in lock mode
unsigned short wf_len; // Number of points to measure
unsigned short step_max; // Maximum fine output step allowed in one iteration of the loop
unsigned short N; // Number of waveforms to average before stabilizing

unsigned char transf; // i2c
unsigned short enab_gnd; // Subtract the background from the signal in enab_gnd=1. 
unsigned short busy; // i2c
unsigned short remote_trigg; // Set to 0 so that the microcontroller uses the trigger input, 
// set it to 1 so that it continuously triggers
unsigned short auto_set_pga; // Whether the microcontroller should automatically select the 
// best offset and gain when in learn mode
float coarse_fine_ratio; // Ratio of sensitivities of the fine to coarse voltage 
// outputs of the board
unsigned short mode; // read_only; 0 when in learn; 1 when in lock
unsigned short out_of_lock; // 1 when the board is out of lock

unsigned int trigg_cnt; // Total number of triggers. Never reset, may overflow.
unsigned long long int v_cnt; // Total number of Vout updates. Never reset, may overflow.
unsigned long long int wf_cnt; // Total number of non-corrupted waveforms measured so far. May overflow.

float Gain; // Gain of the feedback loop


// pga_state is defined via = g*16+8 where g is such that the gain G
// in the input measurements is G=2^g and G is one of {1, 2, 4, 8, 16, 32, 64, 128}
// i.e. 0<g<7 or pga_state = {0x08, 0x18, 0x28, 0x38, 0x48, 0x58, 0x68, 0x78}
unsigned char pga_state = 0x08; // gain 1 channel 8 default

unsigned char * pbuff; // Pi communication
unsigned char * plist[256];

///////////////////////////////////////
// HELPER FUNCTIONS
///////////////////////////////////////

// Dummy delay function
void delay(int length) {
  while (length >= 0)
    length--;
}

// Conversion of the read value into it corresponding 12bits integer
int ADCtoDAT(unsigned long ADC) {
  return (ADC & 0xFFF0000) >> 16;
}
// Conversion of 12bits integer into the corresponding ADC or DAC value
unsigned long DATtoADC(int DAT) {
  unsigned long ADC;
  ADC = DAT;
  return ADC << 16;
}
unsigned long DATtoDAC(unsigned short DAT) {
  unsigned int ADC;
  ADC = DAT;
  return ADC << 16;
}

// Read GPIO pin P0.n. Return is either 0 (low) or (high)
int Read_Digital(int n) {
  return ((GP0DAT & 0x000000FF) >> n) & 0x1;
}
// Write to GPIO pin P2.n. State is either 0 (low) or (high)
void Write_Digital(int n, int state) {
  if (state == 1)
    GP2DAT = (0x00000001 << (n + 16)) | GP2DAT;
  else
    GP2DAT = ~((0x00000001 << (n + 16)) | (~GP2DAT));
}

// TO DO
void ADCpoweron(int time) {
  ADCCON = 0x620; // power-on the ADC
  while (time >= 0) // wait for ADC to be fully powered on
    time--;
}

// Sync the values of the DACs with those stored in the array Vout
// i.e. after changing the value in Vout[i] remember to call this
void set_DACs(void) {
  DAC0DAT = DATtoDAC(Vout[0]);
  DAC1DAT = DATtoDAC(Vout[1]);
  DAC2DAT = DATtoDAC(Vout[2]);
  DAC3DAT = DATtoDAC(Vout[3]);
}
void get_DACs(void) {
  Vout[0] = DAC0DAT >> 16;
  Vout[1] = DAC1DAT >> 16;
  Vout[2] = DAC2DAT >> 16;
  Vout[3] = DAC3DAT >> 16;
}

// Sync the state of the pga with that stored in the pga_state
// i.e. after changing the value of pga_state remember to call this
void set_pga() {
  SPITX = 0x3A; // transmit command or any dummy data
  while ((SPISTA & 0x02) != 0x02); // wait for data received status bit
  SPITX = pga_state; // transmit command or any dummy data
  while ((SPISTA & 0x02) != 0x02); // wait for data received status bit
}
void get_pga() {
  SPITX = 0x7A; //0x3A;				  // transmit command or any dummy data
  while ((SPISTA & 0x02) != 0x02); // wait for data received status bit
  SPITX = 0; //pga_state;				  // transmit command or any dummy data
  while ((SPISTA & 0x02) != 0x02); // wait for data received status bit
  delay(500);
  SPITX = 0; //0x3A;				  // transmit command or any dummy data
  while ((SPISTA & 0x02) != 0x02); // wait for data received status bit
  SPITX = 0; //pga_state;				  // transmit command or any dummy data
  while ((SPISTA & 0x02) != 0x02); // wait for data received status bit

}

///////////////////////////////////////
// MAIN FUNCTION: LOCKING
///////////////////////////////////////

void lock_StabPulse_i2c(void) {
  // DEFINITIONS AND INITIALIZATION //////////////////////
  unsigned int cnt_N; // Number of non-corrupted waveforms measured
  unsigned long int sum, sum_gnd; // Sum of the points in the signal and background
  unsigned short Vout0, Vout1, Vout2; // Used to set the DACs
  // Vout0: input offset; Vout1: coarse output; Vout2: fine output

  int step = 100; // Step to move by in the loop, dummy initialization
  short armed; // This is used to detect the trigger :
  // when the trigger input is low armed is set to 1
  // when a measurement start it is set to 0
  short valid_data; // 
  register int k; // Dummy loop variable
  unsigned short Data[256]; // Waveform measurement

  // 
  POWKEY1 = 0x01;
  POWCON = 0x00; // 41.78MHz
  POWKEY2 = 0xF4;

  // ADC&DAC setting
  ADCpoweron(20000); // power on ADC
  REFCON = 0x01; // internal 2.5V reference
  DAC0CON = 0x12; // AGND-ARef range 0x12 2.5V
  DAC1CON = 0x12; // AGND-ARef range 0x12 2.5V
  DAC2CON = 0x12; // AGND-ARef range 0x12 2.5V
  DAC3CON = 0x12; // AGND-ARef range 0x12 2.5V
  ADCCP = 0x03; // conversion on ADC0
  ADCCON = 0x3E4; // continuous conversion

  // IO setting
  GP2CON = 0x00000000; // IO initialization
  GP2DAT = 0xFF000000; // set P2.n as digital output
  GP0CON = 0x00000000; // IO initialization
  GP0DAT = 0x00000000; // set P0.n as digital input

  // Initialize locking parameter defaults
  // This will likely be selected using the Pi
  N = 10;
  Vin = 0;
  Vin_gnd = 0;
  start = 25;
  stop = 80;
  start_gnd = 110;
  stop_gnd = 135;
  wf_len = 200;
  Vset = 0;
  Vlearn = 3200;
  step = 50;
  step_max = 300;
  Gain = 10;

  transf = 0;
  trigg_cnt = 0;
  wf_cnt = 0;
  v_cnt = 0;
  cnt_N = 0;
  enab_gnd = 1;
  remote_trigg = 0;
  auto_set_pga = 1;
  mode = 0; //read only
  out_of_lock = 0;
  coarse_fine_ratio = 20.0; //As per circuit design; can be tuned by Pi
  valid_data = 0;

  // SPI configuration
  GP1CON = 0x22220022; // configure SPI such the P1.0 and P1.1 are set for I2C0
  // P1.2 and P1.3 are set for GPIO (connected on 50Ohms driver TTL)
  // P1.4-7 on SPI
  SPIDIV = 0xCC; // set SPI clock 40960000/(2x(1+SPIDIV))
  // 0xCC = 100kHz
  SPICON = 0x1043; // enable SPI master in continuous transfer mode
  // slave select will stay low during the all transmission

  IRQ = My_IRQ_Handler;
  IRQEN = 0x200; // I2C0 Slave Interupt

  I2C0CFG = 0x04001;
  // Slave ID
  I2C0ID0 = (0x50) << 1; //(0x50 + (((GP0DAT&0x000000FF)>>5)&0x1)+(((GP0DAT&0x000000FF)>>7)&0x1)*2)<<1;
  I2C0STX = 0x00;
  I2C0STX = 0x00;

  // assignation of the different pointers for the I2C exchange of data
  for (k = 0; k < 16; k++) {
    plist[k] = (unsigned char * )(BigDat + k * 16);
    plist[k + 50] = (unsigned char * )(text + k * 32);
  }

  for (i = 0; i < BIGDAT_SZ; i++)
    BigDat[i] = 0;

  sprintf(text, "pulse stabilization v2.0 => %s\ncompiled: %s\nbecause we can!", __func__, __DATE__);

  for (k = 0; k < 4; k++) {
    plist[100 + k] = (unsigned char * )(Vout + k);
  }

  // 104	to execute the function get_DACs
  // 105	to execute the function set_DACs

  plist[110] = (unsigned char * ) & Vlearn;
  plist[111] = (unsigned char * ) & Vin;
  plist[112] = (unsigned char * ) & Vin_gnd;
  plist[113] = (unsigned char * ) & wf_cnt;
  plist[114] = (unsigned char * ) & v_cnt;
  plist[115] = (unsigned char * ) & remote_trigg;

  plist[120] = (unsigned char * ) & I2C0ID0;
  plist[121] = (unsigned char * ) & ADCCP;
  plist[122] = (unsigned char * ) & transf;
  plist[123] = (unsigned char * ) & wf_len;
  plist[124] = (unsigned char * ) & trigg_cnt;

  plist[125] = (unsigned char * ) & Vset;
  plist[126] = (unsigned char * ) & N;
  plist[127] = (unsigned char * ) & start;
  plist[128] = (unsigned char * ) & stop;
  //plist[129] = (unsigned char*)&start2;	  //not in use
  //plist[130] = (unsigned char*)&stop2;  //not in use
  plist[129] = (unsigned char * ) & auto_set_pga;
  plist[130] = (unsigned char * ) & coarse_fine_ratio;
  plist[131] = (unsigned char * ) & start_gnd;
  plist[132] = (unsigned char * ) & stop_gnd;
  plist[133] = (unsigned char * ) & enab_gnd;
  plist[134] = (unsigned char * ) & Gain;
  plist[135] = (unsigned char * ) & step_max;
  plist[136] = (unsigned char * ) & pga_state;
  // 137; 138 for running set_pga and get_pga commands
  plist[139] = (unsigned char * ) & mode;
  plist[140] = (unsigned char * ) & coarse_fine_ratio;
  plist[141] = (unsigned char * ) & out_of_lock;

  // Dummy DAC initialization
  DAC0DAT = DATtoADC(10);
  DAC1DAT = DATtoADC(20);
  DAC2DAT = DATtoADC(2000);
  DAC3DAT = DATtoADC(40);
  Vout[3] = 111;
  set_DACs();

  // LOOP FOR LEARNING AND LOCKING //////////////////////
  while (1) {
    // trigg in is on p0.3 => we check that it is low first (rising edge detection)
    if ((((GP0DAT & 0x000000FF) >> 3) & 0x1) == 0) {
      armed = 1;
    }

    // now p0.3 is high => this is our rising edge
    if (((((GP0DAT & 0x000000FF) >> 3) & 0x1) == 1 && armed == 1) || remote_trigg) {
      armed = 0;
      trigg_cnt++;

      // Measure signal
      for (k = 0; k < wf_len; k++) {
        while (!ADCSTA) {} // wait for the end of ADC conversion
        Data[k] = (unsigned short)(ADCDAT >> 16); // read voltage from ADC0
      }

      if (busy == 0) { // supposed to guaranty that no i2c transfer has been performed during the wf acquisition
        //*** copy for the i2c ***
        memcpy(BigDat, Data, 256 * sizeof(short)); // Takes around 20 microseconds

        wf_cnt++;
        cnt_N++;

        //sum of the data
        for (k = start; k < stop; k++) {
          sum += Data[k];
        }
        for (k = start_gnd; k < stop_gnd; k++) {
          sum_gnd += Data[k];
        }

        // If we have already measured the N waveforms we need, find 
        if (cnt_N >= N) { // Can take up to 80 microseconds
          Vin = ((double) sum) / (cnt_N * (stop - start)); // calculate signal average value
          Vin_gnd = ((double) sum_gnd) / (cnt_N * (stop_gnd - start_gnd)); // calculate background average value
          valid_data = 1;
          v_cnt++;
          sum = 0; // re-initialization of the measurement
          sum_gnd = 0;
          cnt_N = 0;
        }
      } else busy = 0;

      //*** feedback *** (mode is on pin p0.6)

      // LOCK MODE
      if ((((GP0DAT & 0x000000FF) >> 6) & 0x1) == 1) {
        mode = 1; // Locking
        if (valid_data == 1) {
          valid_data = 0; // Restart measurements
          cnt_N = 0;

          // Calculate necessary step and update fine output
          step = (Vset - Vin + Vin_gnd * enab_gnd) * Gain;
          if (step > step_max)
            step = step_max;
          else if (step < -step_max)
            step = -step_max;
          Vout2 = Vout2 + step;

          // Adjust coarse output if necessary
          if (Vout2 < 2000 || Vout2 > 4000) {
            Vout1 = Vout1 + (Vout2 - 3000) / coarse_fine_ratio;
            Vout2 = 3000;
          }

          // Check if we're out of range
          if (Vout1 > 4095) {
            out_of_lock = 1;
            Write_Digital(0, 1);
            Vout1 = 4095;
          } else {
            out_of_lock = 0;
            Write_Digital(0, 0);
          }

          // Set output
          DAC1DAT = DATtoADC(Vout1); // output voltage, coarse
          DAC2DAT = DATtoADC(Vout2); // output voltage, fine
        }
      }

      // LEARN MODE
      else {
        mode = 0; // Learning

        // Save the current input level as the set point of the next locking 
        if (valid_data == 1) {
          valid_data = 0;
          Vset = Vin - Vin_gnd * enab_gnd;
          cnt_N = 0;
        }
        Vout1 = Vlearn; // Coarse output = Vlearn, fine output is free.

        // Automatically find input gain and offset
        if (auto_set_pga == 1) {
          get_DACs();
          Vout0 = Vout[0];
          if (Vin_gnd < 30 && Vout0 < 4095) { //may clip, shift up
            Vout0++;
            DAC0DAT = DATtoADC(Vout0);
          } else if (Vin_gnd > 100 && Vout0 > 0) { //shift down
            Vout0--;
            DAC0DAT = DATtoADC(Vout0);
          } else if (Vin > 4000) { //too big, may clip
            if ((pga_state - 8) / 16 > 0) pga_state -= 16;
            set_pga();
            //else: too big even with no gain?
          } else if (Vin < 2000) { //too small
            if ((pga_state - 8) / 16 < 7) pga_state += 16;
            set_pga();
          }
        }

        // Set output
        DAC1DAT = DATtoADC(Vout1);
        // Do not set Vout2 so that we can scan it

      }
    }
  }
}

int main(void) {
  lock_StabPulse_i2c();
  return 0;
}

///////////////////////////////////////
// IRQ Service Routine
///////////////////////////////////////

void My_IRQ_Handler() {
  int status = I2C0SSTA;
  busy = 1;

  // Slave Recieve
  if ((status & 0x08) == 0x08) {
    if (first == 1) {
      first = 0;
      Byte_addr = I2C0SRX;
      I2C0FSTA |= 1 << 8;

      i2c_cnt = 0;
      if (Byte_addr == 122)
        transf = 1;
      if (Byte_addr == 104)
        get_DACs();
      if (Byte_addr == 105)
        set_DACs();
      if (Byte_addr == 137)
        set_pga();
      if (Byte_addr == 138)
        get_pga();

      //Write_Digital(2, 0);
      pbuff = plist[Byte_addr];
      I2C0STX = pbuff[0];

    } else {
      pbuff[i2c_cnt] = I2C0SRX;
      i2c_cnt++;
    }
  }

  // Slave Transmit
  else if ((status & 0x04) == 0x04) // Slave Transmit IRQ
  {
    i2c_cnt++;
    I2C0STX = pbuff[i2c_cnt];
    I2C0ADR = 0xA1;
    //if(Byte_addr>=110 && Byte_addr<=113 && i2c_cnt==1)
    //set_DACs();
  } else if ((status & 0x0400) == 0x0400) //
  {
    first = 1;
    //Write_Digital(2,1);
  }

  busy = 1;
}