char aa[128];

boolean newcommand=false;
int command;
double parameter;

int thermo=0;

double current;
double current_out;
double current_hall;
double current_hall_out;
double potmeter_I;
double potmeter_hall;
double u_hall;
double I_hall_out;
double u_flux;
double force;
double current_set;
double current_set_auto=0;
double current_set_hall;
double current_set_auto_hall=0;
double current_actual=0;
double current_actual_hall;
double potmeter_offset;
double offset;
double da_offset;
double offset_auto;
double offset_out;
double fluxscale_0=11*30*4.7;  // A=11, R=30K, C=47uF
double fluxscale=1;
double flux_data[150];
double flux_data_b[150];
int flux_datab=0;
int flux_delay_time=33;
boolean flux_meas=false;
double temp=0;


String current_s; 
String current_set_s;
String current_hall_s;
String u_hall_s;
String u_flux_s;
String force_s;
String old_current_s; 
String old_current_set_s; 
String old_current_hall_s;
String old_u_hall_s;
String old_u_flux_s;
String old_force_s;


double q[8];

unsigned long long t1;
unsigned long long t2;
unsigned long long ledt;
unsigned long long t_onestep;
unsigned long long t_current;
unsigned long long flux_start_time;
unsigned long long measure_time;
double mtime;
long mode;


unsigned long long  curhtime;
double dtime;
int hall_in;


int hall_in_b;
double current_out_b;
double current_b;
double I_hall_out_b;
double current_hall_b;
double u_hall_b;
double u_flux_b;

void initdevice(void)
 {
   initboard();
   spi_start();
   //init_data();
   r_da(0,0.0);
   r_da(1,0.0);
   r_da(2,0.0);
   setchannel(0);
   setgain(0);
 }

void data_to_1(void)
{
  spin_lock_unsafe_blocking(spinlock_count);
 hall_in=hall_in_b;
 current_out=current_out_b;
 current=current_b;
 I_hall_out=I_hall_out_b;
 current_hall=current_hall_b;
 u_hall=u_hall_b;
 u_flux=u_flux_b;
spin_unlock_unsafe(spinlock_count); 
}

boolean init_flux=false;
int flux_datap=150;

void dataget(int arg)
{
  int k;
  int j,i;
  double a;
  double p;
 
 measure_time=micros();
 hall_in_b=gpio_get(HALL_IN);
 //printf("%d\n",hall_in_b);
 //hall_in_b=0;
 setgain(0);
 setchannel(U_I_OUT);  // I programed out for the Current generator
 a=r_ad();
 current_out_b=a;

setchannel(U_I_IN); // I measured back
a=r_ad();
current_b=a;

setchannel(U_I_POT); // I potmeter
a=r_ad();
//printf("%f \n",a);
k=int(a*100);
potmeter_I=double(k)/100.0;
if (manual)
    {
      current_set=potmeter_I;
    }
else
    {
      current_set=current_set_auto;
    }
current_b=current_actual;
setchannel(U_HULL_POT);
a=r_ad();
k=int(a*100);
potmeter_hall=double(k)/100.0;
potmeter_hall=potmeter_hall*2; // in mA
//printf("potmeter_hull %f ",potmeter_hall);
if (manual)
    {
      current_hall_out=potmeter_hall;
    }
else
    {
      current_hall_out=current_set_auto_hall;
    }
if (hall_in==1) setchannel(U_H1_OUT);  //I_hall programed out
else setchannel(U_H2_OUT);
a=r_ad();
I_hall_out_b=a*2;   //in mA

if (hall_in==1) setchannel(U_IH1);  // I_hall measured back
else setchannel(U_IH1);
a=r_ad();
current_hall_b=2*a;  //in mA

if (hall_in==1) setchannel(U_UH1); // U_hall 
else setchannel(U_UH2);
a=r_ad();
u_hall_b=-a/(50.0/4.7+1)*1000;   // INA 51* in mV

setchannel(U_OFFSET_POT); // offset potmeter 
a=r_ad();
k=int(a*100);
potmeter_offset=double(k)/100.0;
if (manual)
    {
      offset_out=potmeter_offset;
    }
else
    {
      offset_out=offset_auto;
    }

setchannel(U_DA_OFFSET);  // offset programed out
da_offset=r_ad();

setchannel(U_OFFSET); //offset measured back
offset=r_ad();

//printf("%f ",offset);
filter[U_FLUXOUT]=0.1;
setchannel(U_FLUXOUT);
a=r_ad();
setgain(0);
if ((manual) && (abs(a)<0.003)) if (!flux_meas) init_flux=true;
if (init_flux) 
  {
    init_flux=false;
    flux_meas=true;
    u_flux_b=0;
    flux_data_b==0;
    flux_datap=0;
    printf("start %f %f \n",offset,a);
  }
data_to_1();
if (flux_meas)
//if ((flux_meas) && (flux_data_b==0))
 {
   flux_datab=0;
   for(i=0;i<150;i++)
    {
      flux_data_b[i]=-fluxscale_0*r_ad();
      sleep_ms(flux_delay_time);
    }
   flux_meas=false;
   u_flux_b=flux_data_b[99];
   data_to_1();
   spin_lock_unsafe_blocking(spinlock_count);
     for(i=0;i<150;i++)
    {
       flux_data[i]=flux_data_b[i];
    }
   flux_datab=1;
   spin_unlock_unsafe(spinlock_count); 
   printf("kesz\n");
 }
if ((manual && (started==0)) && (current_set<0.4))
    {
      started=1;
      zeroscreen=1; 
    }
}

void timesget(void)
{
   unsigned long long t =millis();
   dtime=((double) (t-curhtime))*1.1E-3;
   curhtime=t;
  }



void dasout(int arg)
{
  double c;
  current_actual=current_set;
  if (manual && (started==0)) current_actual=0;
  current_actual_hall=current_hall_out;
  #ifdef SUSCEPT1    
  if (current_actual>5.0) current_actual=5.0;
#else  //SUSCEPT2
  if (current_actual>8.0) current_actual=8.0;
#endif
  r_da(0,20.0/80.0*(current_actual)+0.4);
  if (current_actual_hall>10.0) current_actual_hall=10.0;
  r_da(2,0.5*current_actual_hall);
  r_da(1,offset_out);
  return;  
} 

void out(void)
{
  if (!started) return;
  outpointer++;
  if (outpointer<100) return;
  ftoa(aa,current,2);
  current_s="I="+String(aa);
  while (current_s.length()<8) current_s+=' ';
  current_s+="A";
  ftoa(aa,current_set,2);
  current_set_s="I="+String(aa)+"A";
  while (current_set_s.length()<8) current_set_s+=' ';
  current_set_s+="A";
  
  ftoa(aa,current_hall,2);
  current_hall_s="IH="+String(aa);
  while (current_hall_s.length()<8) current_hall_s+=' ';
  current_hall_s+="mA,";
  
  ftoa(aa,u_hall,0);
  u_hall_s="UH="+String(aa);
  while (u_hall_s.length()<7) u_hall_s+=' ';
  u_hall_s+="mV";
  
  ftoa(aa,u_flux,3);
  u_flux_s="F="+String(aa);
  while (u_flux_s.length()<9) u_flux_s+=' ';
  u_flux_s+="mVs";
  outpointer=0;
    lcd.setCursor(5, 0); 
      if (old_current_s!=current_s) 
        { lcd.print("          ");
         lcd.setCursor(5, 0);
         lcd.print(current_s);
         old_current_s=current_s;
        }
  lcd.setCursor(0, 1);
  if (old_current_hall_s!=current_hall_s) 
    { lcd.print("          ");
      lcd.setCursor(0, 1);
      lcd.print(current_hall_s);
      old_current_hall_s=current_hall_s;
    }
  lcd.setCursor(11, 1);
  if (old_u_hall_s!=u_hall_s) 
    { lcd.print("          ");
      lcd.setCursor(11, 1);
      lcd.print(u_hall_s);
      old_u_hall_s=u_hall_s;
    }
  lcd.setCursor(5, 2);
  if (old_u_flux_s!=u_flux_s) 
    { lcd.print("          ");
      lcd.setCursor(5, 2);
      lcd.print(u_flux_s);
      old_u_flux_s=u_flux_s;
    }
  lcd.setCursor(0, 3);
  lcd.print("M=");
  if (old_force_s!=force_s) 
    { 
      lcd.setCursor(0, 3);
      lcd.print("M=             ");
      lcd.setCursor(2, 3);
      lcd.print(force_s+'g');
      old_force_s=force_s;
    }
   lcd.setCursor(14, 3);
  if (manual) 
      lcd.print("Manual");
  else
      lcd.print("Remote");
}


void get_mass(void)
{
  String mstr;
  if (serial1_data_get())
    {
      mstr=serial1_in;
      force_s=mstr.substring(4,12);
      mstr=mstr.substring(0,2);
      if (mstr!="S ") force_s="* "+force_s;
      else force_s="  "+force_s;
    }
}

void clean_screen(void)
{
  if (zeroscreen==0) return;
  zeroscreen=0;
  lcd.setCursor(0, 0);          
  lcd.print("                   ");
  lcd.setCursor(0, 1);
  lcd.print("          ");
}

void onestep(void)
{
 if (!dostepp) return;
 dostepp=0; 
 t1 =micros();
 timesget();
 get_mass();
 clean_screen();
 out();
 
}

void time_onestep(void)
{ double t=double(micros()-t_onestep);
  t_current=micros();
  if ((t_current-t_onestep)>4000)
    {
       t_onestep=t_current;
       dostepp=1;
    }
}


void senddata(void)
{ 
  int i;
   Serial.println("Data");
   ftoa(aa,current,3);
   Serial.println(String(aa));
   ftoa(aa,current_hall,3);
   Serial.println(String(aa));
   ftoa(aa,u_hall,3);
   Serial.println(String(aa));
   //ftoa(aa,force,3);
   Serial.println(force_s);
   ftoa(aa,temp,3);
   Serial.println(String(aa));
   ftoa(aa,u_flux,3);
   Serial.println(String(aa));
   //ftoa(aa,mtime,1);
   //Serial.println(String(aa));
   if (flux_datab)
     {
        Serial.println("FLUX_DATA");
        for (i=0;i<150;i++)
          {
            //Serial.println(i);
            Serial.println(1000*flux_data[i]);
          } 
     }        
  flux_datab=0;
  Serial.println("End");
}



void do_newcommand(void)
{
 int command_b;
 double parameter_b;
 spin_lock_unsafe_blocking(spinlock_count);
    if (!newcommand) 
     {
       spin_unlock_unsafe(spinlock_count); 
       return;
     }
    else
      {
        command_b=command;
        parameter_b=parameter;
        newcommand=false;
        spin_unlock_unsafe(spinlock_count); 
      }
  
  switch (command_b)
  {
    case SMANUAL:
      manual=int(parameter_b);  
    break;
    case SCURRENT:
      current_set_auto=parameter_b;
    break;
    case SCURRENT_HALL:
      current_set_auto_hall=parameter_b;
    break;
    case SOFFSET:
      offset_auto=parameter_b+offset_zero_0;
    break;
    case SFLUX_DATA:
      //flux_delay_time=parameter_b;
      flux_meas=true;
    break;
    case SOFFSETZERO:
      offset_zero_0=parameter_b;
    break;
    
  }
}