//#define SEMILAB

#define ADDA_D1 21
#define ADDA_D2 22

// chanell 6
//0 Ref_5V
//1 AGND
//2 LOAD_IN
//3 DISP_In

// chanell 7
//0 TARA_PIEZO DA_0
//1 PIEZO DA_1
//2 TARA_LOAd DA_2
//3 TARA_DISP DA_3






void calibration_message_out(String s,double a)
{
  Serial.print(s); printf("%f\n",a);
}



double a0,a1;

void calibration_process(void)
{
  switch (calibration_phase) {
    case 0:
      calibration_phase++;
      calibration_manual=0;
      calibration_message_out("Calibration ",0);
    break;
    case 1:
      calibration_message_out("Phase AD_1 ",1);
      ad_gain[0]=1;
      ad_zero[0]=0;

      setchannel(AGND);
      setgain(0);

      sleep_ms(5000);
      a0=r_ad_calibr();
      printf("a0 %f\n",a0);
       
      setchannel(REF5V);
      setgain(0); 

      sleep_ms(5000);
      a1=r_ad_calibr();
      printf("a1 %f\n",a1);

      ad_zero[0]=a0;
      ad_gain[0]=5.0/(a1-a0);


      setchannel(AGND);
      setgain(0);

      sleep_ms(5000);
      a0=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
    
      setchannel(REF5V);
      setgain(0);

      sleep_ms(5000);
      a1=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      calibration_message_out("AD_0 m_0V=",a0);
      calibration_message_out("AD_0 m_5V=",a1);
      calibration_phase++;
      //calibration_phase++;
      if (calibration_manual==1) calibration_phase=10;
      printf("\n");
    break;
    case 2:
      calibration_message_out("Phase DA_current ",2);
      setchannel(I_OUT);
      setgain(0);
      sleep_ms(5000);
      da_gain[0]=1;
      da_zero[0]=0;
      r_da(0,0);
      sleep_ms(5000);
      a0=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      r_da(0,5.0);
      sleep_ms(5000);
      a1=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      calibration_message_out("DA0 at 0V=",a0);
      calibration_message_out("DA0 at 5V=",a1);
      da_gain[0]=5.0/(a1-a0);
      da_zero[0]=-da_gain[3]*a0;
      r_da(0,0);
      sleep_ms(5000);
      a0=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      r_da(0,5.0);
      sleep_ms(5000);
      a1=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      calibration_message_out("DA0_cal 0V=",a0);
      calibration_message_out("DA0_cal 5V=",a1);
      calibration_phase++;
      calibration_phase=5;
      if (calibration_manual==1) calibration_phase =10;
      printf("\n");
    break;
    case 3:
       calibration_message_out("Phase DA_offset ",3);
      setchannel(OFFSET);
      setgain(0);
      sleep_ms(5000);
      da_gain[1]=1;
      da_zero[1]=0;
      r_da(1,0);
      sleep_ms(5000);
      a0=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      r_da(1,5.0);
      sleep_ms(5000);
      a1=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      da_gain[1]=5.0/(a1-a0);
      da_zero[1]=-da_gain[2]*a0;
      calibration_message_out("DA1 at 0V=",a0);
      calibration_message_out("DA1 at 5V=",a1);

      r_da(1,0);
      sleep_ms(5000);
      a0=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      r_da(1,5.0);
      sleep_ms(5000);
      a1=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      calibration_message_out("DA1 cal 0v=",a0);
      calibration_message_out("DA1 cal 5V=",a1);
      calibration_phase++;
      if (calibration_manual==1) calibration_phase=10;
      printf("\n");
    break;

case 4:
       calibration_message_out("Phase DA_hall ",4);
      setchannel(I_HALL);
      setgain(0);
      sleep_ms(5000);
      da_gain[2]=1;
      da_zero[2]=0;
      r_da(2,0);
      sleep_ms(5000);
      a0=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      r_da(2,5.0);
      sleep_ms(5000);
      a1=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      da_gain[2]=5.0/(a1-a0);
      da_zero[2]=-da_gain[2]*a0;
      calibration_message_out("DA2 at 0V=",a0);
      calibration_message_out("DA2 at 5V=",a1);

      r_da(2,0);
      sleep_ms(5000);
      a0=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      r_da(2,5.0);
      sleep_ms(5000);
      a1=ad_gain[0]*(r_ad_calibr()-ad_zero[0]);
      calibration_message_out("DA2 cal 0V=",a0);
      calibration_message_out("DA2 cal 5V=",a1);
      calibration_phase++;
      if (calibration_manual==1) calibration_phase=10;
      printf("\n");
    break;

    case 5:
     calibration_b=0;
     calibration_phase=0;
     calibrated=1;
     //save_params();
     calibration_message_out("Completted ", 5);
     printf("\n");
    break; 
    case 10:
     sleep_ms(10);
    break;
    case 11:
      calibration_b=0;
      calibration_phase=0;
    break;
    default:
     calibration_b=0;
     calibration_phase=0;
     calibrated=1;
     //save_params();
    break;
  }
  sleep_ms(10);
}