#define AD_C0 2
#define AD_C1 3
#define AD_C2 4
#define AD_G0 5
#define AD_G1 6
#define AD_G2 7
#define SPIA2 8
#define START 9
#define DD10 10
#define SPIENABLE 11
#define SPIA0 12
#define SPIA1 13
   


#define A0 14
#define A1 15
#define A2 17
#define A3 22
#define A4 26
#define A5 27
#define A6 28
#define A7 21
#define A8 20

#define MISO 16
#define SCLK  18
#define MOSI 19
int rate_us=5000;

#define HA0 28
#define HA1 21
#define ZERO_Q 20
#define HALL_IN 10

#define I_OUT 0
#define I_IN 1
#define I_POT 2
#define H1_OUT 3 
#define H2_OUT 4
#define OFFSET 5
#define FLUXOUT 6
#define REF5V 7
#define IH1 8
#define UH1 9
#define I_HALL 10
#define HALL_POT 11
#define IH2 12
#define UH2 13 
#define AGND 14
#define OFFSET_POT 15

#define TX1 0
#define RX1 1

 


double da_zero[4]={0.0,0.0,0.0,0.0};
double da_gain[4]={1.0,1.0,1.0,1.0};
double ad_zero[8]={0,0,0,0,0,0,0,0};
double ad_gain[8]={1.0,0.5,0.2,1.0,1.0,1.0,1.0,1.0};
double filter[16]={0.010,0.010,0.010,0.010,0.010,0.010,0.010,0.010,0.010,0.010,0.010,0.010,0.010,0.010,0.010,0.010};
double scale[8]={1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
double zero[8]={0,0,0,0,0,0,0,0};
int  amp[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
double resistor[4]={0.0,0.0,0.0,0.0};
byte addabuf[4];
unsigned int activ_gain=0;
unsigned int activ_chanel=0;
double ad[16];
double da[4];
int d_out;
int d_in;
int fifo_full;
int sendperiod=1;
int dostepp=0;
char id[16];
char get_USB[1024];
int get_fifo_pointer=0;
double temp1_0;
double temp2_0;
double current_0;






int read_name(void)
{
  char s[16];
  String ss;
  ss="Suscept";
if (device==1)
  ss=ss+"1";
else
  ss=ss+"2";
  Serial.println("Starting "+ss);
   return 0;
}



void spi_start(void)
{  
  gpio_init(SPIENABLE);
  gpio_set_dir(SPIENABLE, GPIO_OUT);
  gpio_put(SPIENABLE, 1);

  gpio_init(SPIA0);
  gpio_set_dir(SPIA0, GPIO_OUT);
  gpio_put(SPIA0, 1);

  gpio_init(SPIA1);
  gpio_set_dir(SPIA1, GPIO_OUT);
  gpio_put(SPIA1, 1);

  gpio_init(SPIA2);
  gpio_set_dir(SPIA2, GPIO_OUT);
  gpio_put(SPIA2, 1);

  gpio_init(AD_C0);
  gpio_set_dir(AD_C0, GPIO_OUT);
  gpio_put(AD_C0, 0);

  gpio_init(AD_C1);
  gpio_set_dir(AD_C1, GPIO_OUT);
  gpio_put(AD_C1, 0);

  gpio_init(AD_C2);
  gpio_set_dir(AD_C2, GPIO_OUT);
  gpio_put(AD_C2, 0);

  gpio_init(AD_G0);
  gpio_set_dir(AD_G0, GPIO_OUT);
  gpio_put(AD_G0, 0);

  gpio_init(AD_G1);
  gpio_set_dir(AD_G1, GPIO_OUT);
  gpio_put(AD_G1, 0);

  gpio_init(AD_G2);
  gpio_set_dir(AD_G2, GPIO_OUT);
  gpio_put(AD_G2, 0);

  gpio_init(START);
  gpio_set_dir(START, GPIO_OUT);
  gpio_put(START, 1);

  gpio_init(HA0);
  gpio_set_dir(HA0, GPIO_OUT);
  gpio_put(HA0, 0);

  gpio_init(HA1);
  gpio_set_dir(HA1, GPIO_OUT);
  gpio_put(HA1, 0);

  gpio_init(HALL_IN);
  gpio_set_dir(HALL_IN, GPIO_IN);
  gpio_pull_up(HALL_IN);

  gpio_init(ZERO_Q);
  gpio_set_dir(ZERO_Q, GPIO_OUT);
  gpio_put(ZERO_Q,1);
  
  spi_init(spi0,1000000);
  gpio_set_function(MISO, GPIO_FUNC_SPI);
  gpio_set_function(SCLK, GPIO_FUNC_SPI);
  gpio_set_function(MOSI, GPIO_FUNC_SPI);
  spi_set_format(spi0, 8, SPI_CPOL_0, SPI_CPHA_1, SPI_MSB_FIRST);
}



void select_spi(int dev)
{
  
  switch (dev) {
   case 0:
     gpio_put(SPIA0, 0);
     gpio_put(SPIA1, 0);
     gpio_put(SPIA2, 0);
    break;
    case 1:
      gpio_put(SPIA0, 1);
      gpio_put(SPIA1, 0);
      gpio_put(SPIA2, 0);
     break;
    case 2:
      gpio_put(SPIA0, 0);
      gpio_put(SPIA1, 1);
      gpio_put(SPIA2, 0);
     break;
    case 3:
      gpio_put(SPIA0, 1);
      gpio_put(SPIA1, 1);
      gpio_put(SPIA2, 0);
     break;
    case 4:
      gpio_put(SPIA0, 0);
      gpio_put(SPIA1, 0);
      gpio_put(SPIA2, 1);
     break;
    case 5:
      gpio_put(SPIA0, 1);
      gpio_put(SPIA1, 0);
      gpio_put(SPIA2, 1);
     break;
    case 6:
      gpio_put(SPIA0, 0);
      gpio_put(SPIA1, 1);
      gpio_put(SPIA2, 1);
     break;
    case 7:
      gpio_put(SPIA0, 1);
      gpio_put(SPIA1, 1);
      gpio_put(SPIA2, 1);
     break;
  }
}


//DA utils



void da_hex(int num, unsigned long data)
{
  byte dd;
  unsigned long data0=data;
  spi_set_format(spi0, 8, SPI_CPOL_0, SPI_CPHA_1, SPI_MSB_FIRST);
  select_spi(num+1);
  //printf("itt %d %d\n",num, data);
  if (data0>0x3ffff) data0=0x3ffff;
  data0=data0<<2;
  addabuf[0]=(data0 >> 16) & 0xff;
  addabuf[1]=(data0 >> 8) & 0xff;
  addabuf[2]=data0 & 0xff;
  gpio_put(SPIENABLE, 1);
  gpio_put(START, 0);
  spi_write_blocking(spi0,addabuf,3);
  gpio_put(SPIENABLE, 0);
  gpio_put(START, 1);
  spi_set_format(spi0, 8, SPI_CPOL_0, SPI_CPHA_1, SPI_MSB_FIRST);
}


void r_da(int num, double data)
{
  unsigned long d; 
  double dd;
  
  dd=da_gain[num]*data+da_zero[num];
  //dd=data;
  dd=dd/10.0;
  //Serial.println(da_gain[num]);
  if (dd>1) dd=1;
  if (dd<-1) dd=-1;
  da[num]=dd;
  dd-=0.002;
  d=(unsigned long) (0x20000*dd+0x20000);
  if (d>0x3ffff) d=0x3ffff;
  da_hex(num,d);
}


// AD utils 

void setgain(unsigned int gain)
{
 if (gain >7) return;
 if (gain & 1) gpio_put(AD_G0, 1);
 else gpio_put(AD_G0, 0);
 if ((gain & 2 )!=0) gpio_put(AD_G1, 1);
 else gpio_put(AD_G1, 0);
 if ((gain & 4 )!=0) gpio_put(AD_G2, 1);
 else gpio_put(AD_G2, 0);
 activ_gain=gain; 
 sleep_us(1);
}

void setchannel_base(int chanel)
{
 
 if (chanel >7) return;
 activ_chanel=chanel;
 if (chanel & 1) gpio_put(AD_C0, 1);
 else gpio_put(AD_C0, 0);
 if ((chanel & 2 )!=0) gpio_put(AD_C1, 1);
 else gpio_put(AD_C1, 0);
 if ((chanel & 4 )!=0) gpio_put(AD_C2, 1);
 else gpio_put(AD_C2, 0);
 sleep_us(20);
}

void setchannel(int chanel)
{
 if (chanel >15) return;
 //printf("channel %d\n",chanel);
 activ_chanel=chanel;
 int c=chanel;
 if (chanel>7) c=3;
 if (chanel>11) c=4;
 //if (c==4) printf("it % d %d\n",c,chanel );
 //if (c==3) printf("it % d %d\n",c,chanel );
 if (c & 1) gpio_put(AD_C0, 1);
 else gpio_put(AD_C0, 0);
 if ((c & 2 )!=0) gpio_put(AD_C1, 1);
 else gpio_put(AD_C1, 0);
 if ((c & 4 )!=0) gpio_put(AD_C2, 1);
 else gpio_put(AD_C2, 0);
 switch (chanel)
 {
    case 8:
      gpio_put(HA0, 0);
      gpio_put(HA1, 0);
     break;
     case 9:
      gpio_put(HA0, 1);
      gpio_put(HA1, 0);
     break;
     case 10:
      gpio_put(HA0, 0);
      gpio_put(HA1, 1);
     break;
     case 11:
      gpio_put(HA0, 1);
      gpio_put(HA1, 1);
     break;
     case 12:
      gpio_put(HA0, 0);
      gpio_put(HA1, 0);
     break;
     case 13:
      gpio_put(HA0, 1);
      gpio_put(HA1, 0);
     break;
     case 14:
      gpio_put(HA0, 0);
      gpio_put(HA1, 1);
     break;
     case 15:
      gpio_put(HA0, 1);
      gpio_put(HA1, 1);
     break;
 }
 //setgain(amp[chanel]);
 //setgain(0);
 if (chanel>7) sleep_us(20);
 else sleep_us(10);
}



void adstart(void)
{
   select_spi(0);
   gpio_put(SPIENABLE, 1);
   sleep_us(1); 
   gpio_put(START, 0);
   sleep_us(1);
   gpio_put(START, 1);
   sleep_us(10);
 }



long adget(void)
{
  long d,d1,d2,d3;
   spi_set_format(spi0, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
   gpio_put(SPIENABLE, 1);
   gpio_put(START, 0);
   spi_read_blocking(spi0,0,addabuf,3);
   gpio_put(START, 1);
   gpio_put(SPIENABLE, 0);
   d1=addabuf[0];
   d2=addabuf[1];
   d3=addabuf[2];
   d=((d1<<16)+(d2<<8)+d3)>>6;
   if (d>0x20000) d-=0x40000;
   spi_set_format(spi0, 8, SPI_CPOL_0, SPI_CPHA_1, SPI_MSB_FIRST);
   //Serial.println(d);
   return -d;
}

double r_adget(void)
{
 double d=((double) adget())/((double) 0x20000);
 if (device==1)  
  return 10*(-d/0.7904-0.012);
else
  return 10*(-d/0.7904-0.012);
}

double r_ad_0(void)
{
  adstart();
  sleep_us(2);
  return r_adget();
}

double r_ad_a(void)
{
  double d=0;
  int i=0;
  for (i=0;i<5;i++) d+=r_ad_0();
  return d/5.0;
}


double r_ad(void)
{
 double a;
 double d=r_ad_a();
 a=ad_gain[activ_gain]*d+ad_zero[activ_gain];
 //printf("%f %f\n", ad_zero[activ_gain], ad_gain[activ_gain]);
 ad[activ_chanel]=filter[activ_chanel]*a+(1.0-filter[activ_chanel])*ad[activ_chanel];
 return ad[activ_chanel];
 //return d;
}

double r_ad_calibr(void)
{
  double d=0;
  double a;
  int i=0;
  for (i=0;i<500;i++) 
    {
      a=r_ad_0();
      //printf("%f\n",a);
      d+=a;
      sleep_us(10000);
    }
  return d/500.0;
}

void softReset(void)
{
  rp2040.reboot();
}


void dostep(void)
{
  dostepp=1;
}

void initboard(void)
{
   delay(500);
   rate_us=5000;
}





void(* resetFunc) (void) = 0;//declare reset function at address 0