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 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 offset_out;
double fluxscale=30*4.7/11.0;  // A=11, R=30K, C=47uF   a=0.0107056 
double flux_data[200];
double flux_data_b[200];
int flux_datab=0;
int flux_delay_time=100;
boolean flux_meas=false;
double temp=20;


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 current_back_b;
double current_hall_b;
double u_hall_b;
double u_flux_b;
boolean init_flux=false;
int flux_datap=150;

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;
 current_hall=current_hall_b;
 u_hall=u_hall_b;
 u_flux=u_flux_b;
 flux_data[flux_datap]=flux_data_b[flux_datap];
spin_unlock_unsafe(spinlock_count); 
//printf("%f\n",current_hall_b);
}



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

setchannel(I_IN); // I measured back
a=r_ad();
current_back_b=2.0*(a-2.56)*3.0/(2.237);
current_back_b=current_set;
//printf("%f %f \n",a,current_back_b);

setchannel(I_POT); // I potmeter
a=r_ad();
//printf("%f \n",a);
k=int(a*100);
potmeter_I=2*double(k)/100.0;
if (manual)
    {
      current_set=potmeter_I;
    }
else
    {
      current_set=current_set_auto;
    }
current_b=current_back_b;
if (current_back_b>4.8) current_b=current_actual;
//current_b=current_set;

setchannel(HALL_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 \n",potmeter_hall);
if (manual)
    {
      current_hall_out=potmeter_hall;
    }
else
    {
      current_hall_out=current_set_auto_hall;
    }
if (current_hall_out>8.0) current_hall_out=8.0;

//hall_in=1;
if (hall_in==1) setchannel(IH1);  // current_hall measured back
else setchannel(IH2);
a=r_ad();

current_hall_b=2*a;  //in mA
//if ((current_hall_out-current_hall_b)>0.5) current_hall_b=0;
//printf("I_hull %f %f\n",hall_in,current_hall_b);

if (hall_in==1) setchannel(UH1); // U_hall 
else setchannel(UH2);
a=r_ad();
u_hall_b=a/(50.0/20.0+1)*1000;   // INA 51* in mV

setchannel(OFFSET_POT); // offset potmeter 
a=r_ad();
//printf("%f\n",a);
k=int(a*100);
potmeter_offset=double(k)/100.0;
if (manual)
    {
      offset_out=2*potmeter_offset;
    }
else
    {
      offset_out=offset_auto;
    }


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

filter[FLUXOUT]=0.1;
setchannel(FLUXOUT);
a=r_ad();
setgain(0);
//printf("%f\n",a);
if ((manual) && (abs(a)<0.03)) if (!flux_meas) init_flux=true;
//if (abs(a)<0.03) if (!flux_meas) init_flux=true;
if (init_flux) 
  {
    init_flux=false;
    flux_meas=true;
    u_flux_b=0;
    flux_datap=0;
    flux_start_time=measure_time;
    if (testb) printf("start %f %f \n",offset,a);
  }
if (flux_meas)
 {
   if ((int) (measure_time-flux_start_time)>(flux_datap*flux_delay_time*1000))
   { 
     //printf("%f\n",flux_data_b[flux_datap]);
    flux_datap++;
    if (flux_datap>150) flux_datap=150;
   }
   if (device==1)
    {
     flux_data_b[flux_datap]=-fluxscale*a+0.30;
     flux_data_b[flux_datap]=flux_data_b[flux_datap]*8.3/11.24;
    }
   else 
     {
     flux_data_b[flux_datap]=-fluxscale*a+0.45;
     flux_data_b[flux_datap]=flux_data_b[flux_datap]*8.1/8.96;
    }
   u_flux_b=flux_data_b[flux_datap];
   if (flux_datap==150)
     {
       u_flux_b=flux_data_b[149];
       flux_meas=false;
       spin_lock_unsafe_blocking(spinlock_count);
        flux_datab=1;
       spin_unlock_unsafe(spinlock_count); 
       if (testb) {printf("ready %f\n",u_flux_b/fluxscale);
       }
     }
    
 }
data_to_1();
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;
if (device==1)  
  if (current_actual>5.0) current_actual=5.0;
else
  if (current_actual>8.0) current_actual=8.0;

  //printf("%f\n",current_actual_hall);
  r_da(0,20.0/80.0*(current_actual)+0.4);
  if (current_actual_hall<=7.0) r_da(2,0.5*current_actual_hall);
  else r_da(2,0.5*7.0);
  r_da(1,-offset_out-0.075);
  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()<12) 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));
   
   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)
     {
        flux_datab=0;
        Serial.println("FLUX_DATA");
        for (i=0;i<150;i++)
          {
            //Serial.println(i);
            Serial.println(flux_data[i], 3);
          } 
     }        
  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);  
      hall_in_b=1;
    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)/10.0;
    break;
    case SFLUX_DATA:
      flux_delay_time=parameter_b;
      //init_flux=true;
    break;
    case SCLEAR_C:
      init_flux=true;
    break;
    case SFLUX12:
        if (parameter_b==1) hall_in_b=1;
        else hall_in_b=0;
    break;
    case SOFFSETZERO:
      offset_zero_0=parameter_b;
    break;
    
  }
}