#ifndef  __PID__H
#define  __PID__H

#pragma code
#ifdef DEBUG_PID
#define SEND_PID_STRING(STRING) send_string((STRING))
#define SEND_PID_UCHAR_HEXA(CHR) send_unsigned_char_as_hexa((CHR))
#define SEND_PID_INT(INT) send_int_as_hexa((INT))
#define SEND_PID_LONG_HEXA(LNG) send_long_as_hexa((LNG))
#define SEND_PID_BYTE(CHR) send_byte((CHR))
static char string_state[]="State: ";
static char string_control[]="CTRL_Term: ";
static char string_error_term[]="Err Term: ";
static unsigned short counter = 0;
static char new_line[]="\r\n";
static char string_int_term[]="Int Term: ";
static char string_pos_dif[]="Pos Dif: ";
static char string_des_vel[]="Des. Veloc:"; ?    static char string_des_ask[]="Ask. Veloc:";
static char string_ask_acc[]="Ask. Accel:";
static char string_des_pos[]="Des. Pos:";
static char string_int_term_of[]="Int.Term. OverFlow!";
static char string_control_overflow[]="Cont.Term Overflow!";
static char string_control_term[]="Ctrl. Term:";
static char string_pwm[]="PWM: ";
#else
#define SEND_PID_STRING(STRING) 1
#define SEND_PID_UCHAR_HEXA(CHR) 1
#define SEND_PID_INT(INT) 1
#define SEND_PID_LONG_HEXA(LNG) 1
#define SEND_PID_BYTE(CHR) 1
#endif

#pragma udata
unsigned char traj_finished;

// Buffer to copy the current mesured position
signed long mesured_position_buf[2];

#pragma code
/********************************************************/
/*          Timer stuff                                 */
/********************************************************/

void init_timer (void)
{
   timerH[0]=timerH[1]=0;
   time_ptr = (unsigned char *)&time;
   INTCONbits.TMR0IE = 1;
   // Inlined, we need to change TMR0H before TMR0L (see datasheet)
   TMR0H = 0;
   TMR0L = 0;
   T0CON = 0x07;// 16 bit timer, 1:256 prescaler, not started
INTCON2bits.TMR0IP =1; //TMR0 High Priority
}

void start_timer (void)
{
  T0CONbits.TMR0ON = 1;
}

void stop_timer (void)
{
  T0CONbits.TMR0ON = 0;
  TMR0H = 0;
  TMR0L = 0;
  timerH[0]=timerH[1]=0;
}

//********************************************************
//          Sampling stuff                               *
//********************************************************

void get_time (void)
{
  // TODO do we need 32bits precision ?
   // In that case we need to enable the tmr0 overflow interrupt,
   // and increment a 16bits-wide counter in the interrupt routine.
   //

  *time_ptr = TMR0L;
  *(time_ptr + 1) = TMR0H;
  *(time_ptr + 2) = timerH[0];
  *(time_ptr + 3) = timerH[1];
 
}

void get_sample (void)
{
  // copy the mesured position to a buffer to calculate pid
  mesured_position_buf[0] = mesured_position[0];
  mesured_position_buf[1] = mesured_position[1];
}

void get_derivative_sample (void)
{

  old_derivative[0] = new_derivative[0];
  old_derivative[0] = new_derivative[0];

  new_derivative[0] = error_term[0];
  new_derivative[1] = error_term[1];

  derivative_term[0] = new_derivative[0] - old_derivative[0];
  derivative_term[1] = new_derivative[1] - old_derivative[1];

}

//*********************************************************
//*         Pid stuff                                     *
//*********************************************************
void desired_position_accel (void)
{
long temps = (time-time_offset[index]);
    desired_velocity[index] = asked_accel[index]*(temps >> 16);

    if(desired_velocity[index] >= (asked_velocity[index] >> 16)) {
       state[index] = STATE_CNST;
       desired_position[index] =  asked_velocity[index] * (temps >> 17);
       stop_distance[index] = desired_position[index];
       time_to_stop[index] = (time-time_offset[index]);
    }
    else
  desired_position[index] = desired_velocity[index] * (temps >> 1);
}

void desired_position_cnst (void)
{
long temps = (time-time_offset[index]);
    desired_position[index] = stop_distance[index] +
                              asked_velocity[index]*((temps - time_to_stop[index]) >> 16);
    if (desired_position[index] >= asked_position[index] - stop_distance[index] ) {
      time_to_stop[index] = temps;
      stop_distance[index] = desired_position[index];
      state[index] = STATE_DECEL;
    }
}

void desired_position_decel (void)
{
long temps = (time-time_offset[index]) - time_to_stop[index];
    desired_position[index] = stop_distance[index] + asked_velocity[index]*(temps >> 16)- asked_accel[index] * (temps >> 16) * (temps>> 1);
    if (desired_position[index] >= asked_position[index]) {
      desired_position[index] = asked_position[index];
      state[index] = STATE_STOP;
    }
}

void calculate_desired_position (void)
{
  switch (state[index]) {
    case STATE_ACCEL:
      desired_position_accel();
      break;
    case STATE_CNST:
      desired_position_cnst();
      break;
    case STATE_DECEL:
      desired_position_decel();   
      break;
    default:
      return;
  }
}

void calculate_error_term (void)
{
    position_difference = desired_position[index] - mesured_position_buf[index];
    error_term[index] = ( int ) position_difference ;
    // if position_difference not on 16 bits, satursate error_term...
    // TODO check that
    if (error_term[index] != position_difference)
      error_term[index] = (position_difference < 0) ? -0x7fff : 0x7fff;
   
integration_term_buffer = integration_term[index] + error_term[index];

integration_term[index] =  (short long)(integration_term_buffer);

if(integration_term[index] != integration_term_buffer){
  integration_term[index] = (integration_term_buffer < 0) ? -0x7fffff:0x7fffff ;
  //DEBUG
  //SEND_PID_STRING(string_int_term_of);
}

    // TODO The integration limit is not yet done
}

void do_pid (void)
{
   if (state[index] == STATE_STOP) {
      if (desired_position[index] >= mesured_position[index]) {
  control_terms[index]=0;
         return;
      }
    }
    control_term_buffer = coef_prop*error_term[index] + coef_deriv*(derivative_term[index]) + coef_integ*( (int) (integration_term[index] >> 8) );
control_terms[index] = (int)(control_term_buffer);

if(control_terms[index] != control_term_buffer){
  control_terms[index] = (control_term_buffer < 0)? -0x7fff : 0x7fff;
  //DEBUG
  //SEND_PID_STRING(string_control_overflow);
}
} 

//*************************************************************
//           Update pwms     www.dangdangdz.com                  *
//*************************************************************
// Called only once

void update_pwms(void)
{
  // TODO check signs
  if (control_terms[0]<0) {
    CCPR1L = ((-control_terms[0])>>7); // TODO is it only 7 ? signed -> unsigned...
    pwm_signs.sign0 = 0;
  }
  else {
    CCPR1L = (control_terms[0]>>7);
    pwm_signs.sign0 = 1;
  }

  if (control_terms[1]<0) {
    ECCPR1L = ((-control_terms[1])>>7); // TODO is it only 7 ? signed -> unsigned...
    pwm_signs.sign1 = 0;
  }
  else {
    ECCPR1L = (control_terms[1]>>7);
    pwm_signs.sign1 = 1;
  } 
}  
 

void pid_main_loop (void)
{
doMainLoop:
  while (!(start_motor[0]||start_motor[1]))
    Nop();
  status = STATUS_BUSY;
  sample_counter = 0;
  start_timer();

doSample:
  for (index = 0; index < 2; ++index) {
    if (start_motor[index]) {
      state[index] = STATE_ACCEL;
      get_time();
      time_offset[index] = time;
      signalRegister_LSB[index].traj_complete = 0;
      signalRegister_LSB[index].motor_off = 0;
    }
    start_motor[index] = 0;
  }
  get_time();
  sample_time=time+8; // next sample time. prescaler = 256 -> 8*256 = 2048
  get_sample();
  ++sample_counter;

  for (index = 0; index < 2; ++index) {
    calculate_desired_position();
    calculate_error_term();
  }

  if (sample_counter == derivative_sample) {
    sample_counter = 0;
    get_derivative_sample();
  }

  for (index = 0; index < 2; ++index) {
    do_pid();
  } 
  //update pwms : try to synchronise as much as possible
  update_pwms(); 
 
  traj_finished = 1;
  for (index = 0; index < 2; ++index) {
    if (state[index] == STATE_STOP && control_terms[index] == 0) {
      signalRegister_LSB[index].traj_complete = 1;
      signalRegister_LSB[index].motor_off = 1;
    }
    else traj_finished = 0;
  }

//DEBUG
  if (counter == 1) {
  counter = 0;
  //SEND_PID_STRING(string_control);
  //SEND_PID_INT(control_terms[0]);SEND_PID_BYTE('\t');
  //SEND_PID_INT(control_terms[1]);SEND_PID_STRING(new_line);
 
  SEND_PID_STRING(string_des_pos);
  SEND_PID_LONG_HEXA(desired_position[0]);SEND_PID_BYTE('\t');
  SEND_PID_LONG_HEXA(desired_position[1]);SEND_PID_STRING(new_line);

  SEND_PID_STRING(string_des_vel);
  SEND_PID_LONG_HEXA(desired_velocity[0]);SEND_PID_BYTE('\t');
  SEND_PID_LONG_HEXA(desired_velocity[1]);SEND_PID_STRING(new_line);

  SEND_PID_STRING(string_pwm);
  SEND_PID_UCHAR_HEXA(PWM0_DUTY_CYCLE);SEND_PID_BYTE('\t');
  SEND_PID_UCHAR_HEXA(PWM1_DUTY_CYCLE);SEND_PID_STRING(new_line);
 
  //SEND_PID_STRING(string_des_ask);
  //SEND_PID_LONG_HEXA((asked_velocity[0] >> 16));SEND_PID_BYTE('\t');
  //SEND_PID_LONG_HEXA((asked_velocity[1] >> 16));SEND_PID_STRING(new_line);

  //SEND_PID_STRING(string_int_term);
  //SEND_PID_LONG_HEXA(integration_term[0]);SEND_PID_BYTE('\t');
  //SEND_PID_LONG_HEXA(integration_term[1]);SEND_PID_STRING(new_line);

  //SEND_PID_STRING(string_control_term);
  //SEND_PID_LONG_HEXA(control_terms[0]);SEND_PID_BYTE('\t');
  //SEND_PID_LONG_HEXA(control_terms[1]);SEND_PID_STRING(new_line);

  //SEND_PID_STRING(string_ask_acc);
  //SEND_PID_LONG_HEXA(asked_accel[0]>>16);SEND_PID_BYTE('\t');
  //SEND_PID_LONG_HEXA(asked_accel[1]>>16);SEND_PID_STRING(new_line);

   //SEND_PID_STRING(string_pos_dif);
  //SEND_PID_LONG_HEXA(position_difference);
  //SEND_PID_STRING(new_line);

  //SEND_PID_STRING(string_error_term);
  //SEND_PID_INT(error_term[0]);SEND_PID_BYTE('\t');
  //SEND_PID_INT(error_term[1]);SEND_PID_STRING(new_line);

  //SEND_PID_STRING(string_int_term);
  //SEND_PID_LONG_HEXA(integration_term[0]);SEND_PID_BYTE('\t');
  //SEND_PID_LONG_HEXA(integration_term[1]);SEND_PID_STRING(new_line);

  SEND_PID_STRING(string_state); 
  SEND_PID_UCHAR_HEXA(state[0]);SEND_PID_BYTE('\t');
  SEND_PID_UCHAR_HEXA(state[1]);SEND_PID_STRING(new_line);SEND_PID_STRING(new_line);

}
counter++;

  if (!traj_finished) {
    while (time < sample_time){
       get_time();
}

    goto doSample;

  }
 
  status = STATUS_MOTOR_OFF | STATUS_TRAJECT_END;
  stop_timer();
  goto doMainLoop;
}

#endif  /*__PID__H*/