byte USB_data[1024];
int USB_data_pointer=0;
int USB_data_length=0;
long USB_time_pointer;

unsigned long long  curhtime,measurestarthtime,holdstarthtime,senddatatime;
double measuretime,dtime,holdtime;




int senddataon=0; //turn on datasend
int holdon=1;
int measureon=0;
int running_p=0;
int makeonestep=0;
int USB_ready=0;
int power_enabled=0;

int poweron=0;
double temp=0;
double rate=0;
double signedrate=0;
double tempto=0;
double holduntiltime=0;
double zero2=0;

double hpower;
double tempm_1,tempm_2;
double tempmax=600;


int timep=0;

double q[8];

double lagarray[ISIZE];
double diffarray[DSIZE];
double posdata[DSIZE];
int programpointer=0;
int programfill=0;
int programfillpointer=0;
int saverequest=0;
int diffpointer=0;
int lagpointer=0;
int da2=0;
int life=0;

int param;
double paramdata;
unsigned long long t1;
unsigned long long t2;
long long time_max=0;
unsigned long long ledt;
unsigned long long t_onestep;
unsigned long long t_current;

int next=0;
int running_v=0;

int error_x=0;
int error_y=0;


char bt[32];

int connectedp=0;
int runingp=0;

struct data_t{
          double ad[8];
    double mtime;
    double cont;  // it was temp
    double rate;
    double paramdata;
    int param;
      };
      
struct data_t data;

unsigned long long currenttime;
unsigned long long runningtime;
unsigned long long connectedtime;

void init_data()
{
    q[0]=1.12;
    q[1]=2.12;
    q[2]=3.12;
    q[3]=4.12;
    q[4]=5.12;
    q[5]=6.12;
    da[0]=10.2;
    da[1]=20.2;
    d_in=127;
    d_out=66;
    measuretime=1000;
    param=244;
    paramdata=700.18;
    senddataon=1;
}





double get_double(int start)
{
  int i;
  double d;
  char *bytePointer = (char *)&d;
  for (i=0;i<sizeof(double);++i)  
     {
        bytePointer[i]=USB_data[i+start];
     }
  return d;
  
}





int send_data(int d)
{
  Serial.write(byte(d));
   return 0;
}


int send_double(double d)
{
  int i;
  int j=0;
  char f;
  char *bytePointer = (char *)&d;
  for (i=0;i<sizeof(double);++i)  
     {
      f=bytePointer[i];
      j+=send_data(f);
     }
  if (j==0) return 0;
  return -1;
}

void test_send(void)

{
  
}

int send_data_USB(void)
{
  int i;
  int j=0;
  //if (!USB_ready) return 0;
  //test_send();
  //Serial.println("Data send");
  Serial.write('S');
  Serial.write('T');
  for (i=0;i<8; ++i)
  {
    send_double(data.ad[i]);
  }
  send_double(data.mtime);
  send_double(data.cont);
  send_double(data.rate);
  send_double(data.paramdata);
  send_double(data.param);
  send_double(tempzero);
  send_double(tempslope);
  send_double(tempparab);
  send_double(pa);
  send_double(pb);
  send_double(pc);
  send_double(inttime);
  send_double(12);
  send_double(q0); //22*8+2 char
  return 1;
}


char aa[128];

void send_data_P(void)
{
  int i;
  //test_send();
  Serial.println("MS Data");
  for (i=0;i<8; ++i)
  {
    ftoa(aa,data.ad[i],4);
    Serial.println("MS "+String(aa));
  }
  ftoa(aa,data.mtime,3);
  Serial.println("MS "+String(aa));
  ftoa(aa,data.cont,3);
  Serial.println("MS "+String(aa));
  ftoa(aa,data.rate,3);
  Serial.println("MS "+String(aa));
  ftoa(aa,data.paramdata,3);
  Serial.println("MS "+String(aa));
  ftoa(aa,data.param,3);
}

void get_ads(void)
{
int j;
double q1[8];

for (j=0;j<4;j++)
    {
#ifdef TEST
       filter[j]=0.01;
#endif
      setgain(amp[j]);
      setchannel(j); 
      sleep_ms(10);
      q1[j]=r_ad();
    }
spin_lock_unsafe_blocking(spinlock_count);
for (j=0;j<4;j++) q[j]=q1[j];
spin_unlock_unsafe(spinlock_count); 
filter[0]=rfiltert;
filter[1]=rfiltert;
filter[2]=rfilter;
filter[3]=0.0002;
}


double a1=25.2820347;
double a2=-0.0802500;
double a3=0.0013108;

double temp_mv(double t)
{
  return  a1*t+a2*t*t+a3*t*t*t;
}

double d_temp_mv(double t)
{
  return  a1+2*a2*t+3*a3*t*t;
}

double mv_temp(double t)
{
  double mv=t/a1;
  double mv_0;
  int i;
  for (i=0;i<10;i++)
    {
      mv_0=mv;
      mv=mv_0+(t-temp_mv(mv_0))/d_temp_mv(mv_0);
    }
  return mv;
}


void dataget(int arg)
{
  int j;
  double t;
  double dumi;
  double dT;
  
  spin_lock_unsafe_blocking(spinlock_count);
  for (j=0;j<4;j++)
    {  
        data.ad[j]=q[j];
        //printf("%f\n",-q[j]);
    }
  spin_unlock_unsafe(spinlock_count); 
  timep=(timep+1) % 200*inttime;
  lagpointer=(int) (ISIZE*((double) timep)/((double) 200*inttime));
  if (device==1)
   {
     tempm_1=7.42*(-data.ad[0])-3.20; 
     tempm_2=7.42*(-data.ad[1])-2.72; 
     dT=1.0*(-data.ad[2])-0.18;
   }
  else
   {
     tempm_1=7.42*(-data.ad[0])-1.70; 
     tempm_2=7.42*(-data.ad[1])-1.23;
     dT=1.0*(-data.ad[2])-0.01; 
   }
  tempm_1=t1_slope*tempm_1-t1_zero;
  tempm_2=t2_slope*tempm_2-t2_zero;
  dT=dt_slope*(dT)-dt_zero; 


  dT=temp_mv(dT);
  dT=q0*dT;
  tempm_1=temp_mv(tempm_1)+25.0;
  tempm_2=temp_mv(tempm_2)+25.0;

if (testb)
  printf("    T1 %f T2 %f dT %f \n", tempm_1,tempm_2,dT);

  tempm_1=tempslope*tempm_1+tempzero;
  tempm_2=tempslope*tempm_2+tempzero;

  if (tempm_2>70) gpio_put(LED1, 0);
  else gpio_put(LED1, 1);;
  
      
  lagarray[lagpointer]=temp-tempm_2;
   hpower=0;
   for (j=0;j<ISIZE;j++) hpower+=lagarray[j];
   hpower=pa*lagarray[lagpointer]+pb*hpower/((double) ISIZE);
   j=(lagpointer+1) % ISIZE;
   dumi=pc*(lagarray[lagpointer]-lagarray[j])/((double) inttime);
   if (dumi<0) dumi=0;
   hpower+=dumi;
   hpower=hpower/50.0;
   if (hpower<0) hpower=0;
   if (hpower>1.0) hpower=1; 
   if (poweron==0) hpower=0;
   if (lagarray[lagpointer]<0) hpower=0;
   if (tempm_1>tempmax) hpower=0;
   if (tempm_2>tempmax) hpower=0;
   data.ad[7]=hpower;
   data.mtime=measuretime;
   data.cont=temp;
   data.rate=signedrate;
   data.param=curprogram.param+
        256*(measureon+4*saverequest+8*running_v+32*poweron);
   data.paramdata=curprogram.paramdata;
   if ((holdon)&& (curprogram.param!=SRATE))
         data.paramdata=holduntiltime-holdtime; 
   data.ad[0]=dT;
   data.ad[1]=tempm_1;
   data.ad[2]=tempm_2;
   data.ad[3]=temp;
   data.ad[4]=temp-tempm_2;
   data.ad[5]=20*(q[3]); //power back;
}


void sethold(double t)
{
  curprogram.param=SHOLD;
  curprogram.paramdata=t;
  holdon=1;
  holdstarthtime=curhtime;
  holduntiltime=t;
}



void timesget(void)
{
   unsigned long long t =millis();
   dtime=((double) (t-curhtime))*1000.0;
   curhtime=t;
   //Serial.println(measureon);
   if (!measureon) measurestarthtime=t;
   if (!holdon) holdstarthtime=t;
   measuretime=((double) (t-measurestarthtime))*1000.0;
   holdtime=((double) (t-holdstarthtime))*1000.0;
  }


void limits(int arg)
{
  if (curprogram.param==SLAMPTEST) 
   { if (tempm_1<temp) next=1;
   }
  if  (curprogram.param==STO) 
    { 
      if ((signedrate>0)&&(temp>=tempto)) next=1;
      if ((signedrate<0)&&(temp<=tempto)) next=1;
    }
  if (holdon)
    { if (holdtime>=holduntiltime) next=1;
    }
  return;
  }


void nextp(int arg)
{
  if (!next) return;
  next=0;
  programpointer++;
  curprogram=program_s[programpointer];
  switch(curprogram.param){
    case SFINISH:
      running_v=0;
      sethold(1e30);
      break;
    case SHOLD:
      sethold(curprogram.paramdata);
      break;
    case SRATE:
      rate=curprogram.paramdata;
      next=1;
      break;
    case STO:
      poweron=1;
      power_enabled=1;
      tempto=curprogram.paramdata;
      holdon=0;
      signedrate=rate;
      if (temp>tempto) signedrate=-rate;
      break;
    case SSAVE:
      sethold(1e30);
      saverequest=1;
      break;  
    case SMEASUREON:
      measureon=1;
      next=1;
      break;
    case SMEASUREOFF:
      measureon=0;
      next=1;
      break;
    case SLAMPTEST:
      break;
    case SBACK:
      temp=curprogram.paramdata;
      sethold(1e30);
      break;
    case SID:
      temp=0;
      sethold(1e30);
      break;
   
    case SPOWER:
      poweron=(int)curprogram.paramdata;
      next=1;
      break;
     }
  return;
  }



void dasout(int arg)
{
  r_da(0,5.0*hpower);
  //printf("%f\n",hpower);
  if (poweron==1)
    gpio_put(LED3, 0);
  else 
    gpio_put(LED3, 1);
  return;  
  } 


void onestep(void)
{
 struct program_t pp;
 //if (!USB_ready) return;
 if (!dostepp) return;
 dostepp=0; 
 t1 =micros();
 timesget();
 if ((!holdon) && (curprogram.param!=SLAMPTEST))
       temp+=signedrate*dtime;
 dataget(1);
 dasout(1);
 if (power_enabled) gpio_put(EN, 1);
 else gpio_put(EN, 0);
 limits(1);
 nextp(1);
 t2=micros()-t1;
 if (t2>time_max) 
    {
      time_max=t2;
    }
  return;
}

void time_onestep(void)
{
  t_current=micros();
  if (running_v)
    {
      if ((t_current-t_onestep)>5000)
       {
         t_onestep=t_current;
         dostepp=1;
       }
    }
  else 
    {
      if ((t_current-t_onestep)>20000)
       {
         t_onestep=t_current;
         dostepp=1;
      }
    }
}



String fill_serial_buff_data(String s,double d)
{
  ftoa(bt,d,8);
  return s+String(bt);
}


void interpretlast(void)
{
 int i=0;
 int j;
 double mv1,mv2;
 if (programfill)
    { programfillpointer++;
      if (programfillpointer>PROGRAMSIZE) 
        {
          programfillpointer=PROGRAMSIZE;
          lastp.param=SFINISH;
        }
      program_s[programfillpointer]=lastp;
      if (lastp.param==SFINISH) 
        {
           programpointer=0;
           programfill=0;
        }
      }
  else
    switch(lastp.param){
       case SFILL:
          programfill=1;
          programfillpointer=0;
          for (i=1;i<PROGRAMSIZE;i++) program_s[i].param=0;
          break;
       case SSTART:
          programpointer=0;
          zero2=0;
          running_v=1;
          next=1;
          break;
      case SSTOP:
          sethold(1e30);
          running_v=0;
          break;
       case SKILL:
          sethold(1e30);
          measureon=0;
          running_v=0;
          break;
       case SHOLD:
         sethold(lastp.paramdata);
         break;
       case SBACK:
          temp=lastp.paramdata;
          sethold(1e30);
          break;
       case SID:
          if (device==1)
            Serial.println("MS DTA1");
          else 
            Serial.println("MS DTA2");
          break;
        case SRESET:
          if (device==1)
            Serial.println("MS DTA1");
          else 
            Serial.println("MS DTA2");
          break;
        case SDEVICE:
          if (lastp.paramdata==2)
            device=(int) lastp.paramdata;
          else device=1;
          save_params();
         break;
        case SHRESET:
          softReset();
          break;
       case SNEXT:
          if (running_v)
           next=1; 
           break;
       case SSENDDATA:
            send_data_USB();
            ledt=millis();
            break;
       case SSEND:
            send_data_P();
            break;
       case SSENDDATAON:
           senddataon=1;
           senddatatime=curhtime;
           temp=tempm_1;
           if (temp<15) temp=15; 
           break;
       case SSENDDATAOFF:
           senddataon=0;
           break;
       case SFILTER:
           rfilter=lastp.paramdata;
           break;  
       case SFILTERT:
           rfiltert=lastp.paramdata;
           break;    
       case STIMEINT:
           sendperiod=(int) (lastp.paramdata/20.0);
           if (sendperiod<1) sendperiod=1;
           break;
       case STEMPZERO:
          tempzero=lastp.paramdata;
          if (tempzero<-50) tempzero=-50;
          break;
       case STEMPSLOPE:
          tempslope=lastp.paramdata;
          if (tempslope<0.8) tempslope=0.8;
          break;
       case STEMPPARAB:
          tempparab=lastp.paramdata;
          break;
       case SPA:
          pa=lastp.paramdata;
          break;
       case SPB:
          pb=lastp.paramdata;
          break;
       case SPC:
          pc=lastp.paramdata;
          break;
       case SQ0:
          q0=lastp.paramdata;
          break;
       case SSAVED:
          saverequest=0;;
          break;
       case SPOWER:
          poweron=(int) lastp.paramdata;
          break;   gpio_init(LED1);
   gpio_set_dir(LED1, GPIO_OUT);
   gpio_put(LED1, 0);
       case SINTTIME:
          inttime=(int) (lastp.paramdata);
          if (inttime<ISIZE) inttime=ISIZE;
          break;
        case ST1ZERO:
          t1_zero=lastp.paramdata;
        break;
        case ST2ZERO:
          t2_zero=lastp.paramdata;
        break;
        case SDTZERO:
          dt_zero=lastp.paramdata;
        break;
        case STEST:
           if (lastp.paramdata==1) testb=true;
           else testb=false;
        break;
        case ST1SLOPE:
#ifdef TEST1
          t1_slope=lastp.paramdata;
#else
          mv1=temp_mv(lastp.paramdata);
          mv2=temp_mv(231.9);  //Sn
          t1_slope=mv1/mv2*t1_slope;
#endif
        break;
        case ST2SLOPE:
#ifdef TEST1
          t2_slope=lastp.paramdata;
#else
          mv1=temp_mv(lastp.paramdata);
          mv2=temp_mv(231.9);  //Sn
          t2_slope=mv1/mv2*t2_slope;
#endif          
        break;
        case SDTSLOPE:
         if (lastp.paramdata<0.1) lastp.paramdata=1.0;
          dt_slope=dt_slope*lastp.paramdata;
#ifdef TEST1
         dt_slope=lastp.paramdata;
#endif 
        break;
        case SGETPARAMS:
         get_params();
        break;
        case SONESTEP:
          senddataon=1;
          makeonestep=1;
          dostepp=1;
          onestep();
          makeonestep=0;
          senddataon=0;
          //Serial.println((int) time_max);
          break;
        case SSAVEPARAM:
          save_params();
          break;
        case SDAGAIN_0:
          da_gain[0]=lastp.paramdata;
          break;
        case SDAZERO_0:
          da_zero[0]=lastp.paramdata;
          break;
        case SDA_0:
          r_da(0,lastp.paramdata);
          break;
        case SDAGAIN_1:
          da_gain[1]=lastp.paramdata;
          break;
        case SDAZERO_1:
          da_zero[1]=lastp.paramdata;
          break;
        case SDA_1:
          r_da(1,lastp.paramdata);
          break;
        case SDAGAIN_2:
          da_gain[2]=lastp.paramdata;
          break;
        case SDAZERO_2:
          da_zero[2]=lastp.paramdata;
          break;
        case SDA_2:
          r_da(2,lastp.paramdata);
          break;
        case SDAGAIN_3:
          da_gain[3]=lastp.paramdata;
          break;
        case SDAZERO_3:
          da_zero[3]=lastp.paramdata;
          break;
        case SDA_3:
          r_da(3,lastp.paramdata);
          break;
        case SADGAIN_0:
          ad_gain[0]=lastp.paramdata;
          break;
        case SADZERO_0:
          ad_zero[0]=lastp.paramdata;
          break;
        case SADGAIN_1:
          ad_gain[1]=lastp.paramdata;
          break;
        case SADZERO_1:
          ad_zero[1]=lastp.paramdata;
          break;
        case SADGAIN_2:
          ad_gain[2]=lastp.paramdata;
          break;
        case SADZERO_2:
          ad_zero[2]=lastp.paramdata;
          break;
        case SADGAIN_3:
          ad_gain[3]=lastp.paramdata;
          break;
        case SADZERO_3:
          ad_zero[3]=lastp.paramdata;
          break;
         case SLED1:
           //Serial.println("LED1");
           //REG_PIOB_CODR =(1<<16);
           ledt=millis();
          break;
          case STP0:
            temp=tempm_1;
            if (temp<15) temp=15; 
          break;
         case SHELP:
           ;
          break;
         default:
          //Serial.println("MS Illegal instruction "+String(lastp.param));
          break;
    }
    return;
}


int interpret(String input)
{
  int space_pos=input.indexOf(' ');
  String data_inst;
  String dd;
  int j=0;
  int done=0;
  if (space_pos<0) space_pos=input.length();
  data_inst=input.substring(0,space_pos);
  dd=input.substring(space_pos+1,input.length());
  //Serial.print(data_inst); Serial.println("g");
  while(!done)
    {
     if (data_inst.equals(int_names[j].Sname))
        {
          done=1;
          lastp.param=int_names[j].param;
        }

       j++;
       if (j>INSTNUM)
        { 
          Serial.print("MS Illegal intruction ");      
          done=1;
          return -1;
        }
    }
  lastp.paramdata=dd.toDouble();
  //Serial.print("MS ");Serial.println(lastp.param);
  //Serial.print("MS ");Serial.println(lastp.paramdata);
  return 1;  
}


int do_serials( String input)
{
  int i=0;
  if  (input.equals("HELP"))
       {for (i=0;i<INSTNUM;i++)
          {
              Serial.print("MS "+int_names[i].Sname);
              Serial.print(" :");
              Serial.println(int_names[i].Shelp);
           }
          return 0;
       }
  return interpret(input);
}



int get_bytes_from_USB(void)
{
  int gg;
  String s="";
  String ss;
  String input;
  int i;
  if (Serial.available()==0) return -1;
  gg=Serial.readBytes(USB_data,2);
  s=s+char(USB_data[0])+char(USB_data[1]);
  //Serial.println(s);
  if (s=="ST") 
    {
     gg=Serial.readBytes(USB_data,18);
     //Serial.println(gg);
     //for (i=0;i<8;i++) Serial.println(USB_data[i]);
     if (gg==18) return 0;
     else 
       {
        Serial.println("MS Wrong data num."); 
        return -3;
       }
    }
 if (s=="MS") 
  {
     s=Serial.readStringUntil(char(13));
     s=s.substring(1,s.length());
     Serial.println("MS "+s);
     if (do_serials(s)>0) interpretlast();
     //Serial.print(lastp.param); Serial.print(' '); Serial.println(lastp.paramdata);
     return -1;
  }
 ss=Serial.readStringUntil(char(13));
 s=s+ss;
 if (do_serials(s)>0) 
    { 
      interpretlast();
      return -1;
    }
 else 
   { Serial.println("MS Wrong data ."+s+"!");
     return -2;
   }
}








void main_step(void)

{
  int i;
  unsigned long long t=millis();
  if (t-ledt>50000) 
    {
      //Serial.println("off");
      //REG_PIOB_SODR =(1<<16);  // LED off;
      poweron=0;
      //REG_PIOB_SODR =(1<<15);
    }
  int p=get_bytes_from_USB();
  if (p==0)
     {
      lastp.param=get_double(0);
      lastp.paramdata=get_double(8);
      if ((lastp.param!=SRESET) && (lastp.param!=SSENDDATA) && (lastp.param!=SID)) 
       {
        Serial.print("MS Get data from USB "); Serial.print(lastp.param); Serial.print(" "); Serial.println(lastp.paramdata);
       }
       if (lastp.param==SUSB_READY)
         {
          USB_ready=1;
         }
      interpretlast();
     }
}






void initdevice(void)
 {
   initboard();
   init_data();
   gpio_init(EN);
   gpio_set_dir(EN, GPIO_OUT);
   gpio_put(EN, 0);
   gpio_init(LED1);
   gpio_set_dir(LED1, GPIO_OUT);
   gpio_put(LED1, 0);
   gpio_init(LED2);
   gpio_set_dir(LED2, GPIO_OUT);
   gpio_put(LED2, 0);
   gpio_init(LED3);
   gpio_set_dir(LED3, GPIO_OUT);
   gpio_put(LED3, 1);
   runningtime=millis();;
   connectedtime=millis();
   temp=15;
   program_s[0].param=SHOLD;
   program_s[0].paramdata=1e30;
   q[1]=0.0;
   q[2]=0.0;
  signedrate=0;
   //send_data(0x101);
   //send_data('s');
   //send_data(0x100);
 }