uint32_t MOT_GetPosDif(uint32_t pos1, uint32_t pos2, uint32_t *gt)
{
    if( pos1 > pos2)
    {
        *gt = 0;
        return (pos1 - pos2);
    }
    else 
    {
        *gt = 1;
        return (pos2 - pos1);
    }
}


void MOT_PositionLoopDouble(void)
{
    uint32_t position1, position2, posdif, gt;
    uint32_t pwm1, pwm2;
    
    position1 = motor1_data_double->position;
    position2 = motor2_data_double->position;
    
    posdif = MOT_GetPosDif(position1, position2, &gt);
    
    if (posdif > glob_mot_data.max_pos_diff)
    {
        //retry
        glob_mot_data.pos_diff_count++;
        if (glob_mot_data.pos_diff_count > glob_mot_data.max_pos_diff_count)
        {
            //maximum retries - unable to adjust speed
            glob_mot_data.pos_diff_count = 0;
            //send stop command
            MOT_SendData(motor1_data_double, CAN_COM_STOP, 0);
            MOT_SendData(motor2_data_double, CAN_COM_STOP, 0);
            
            sys_status |= MOT_POS_DIFF;
        }
    }
    
    pwm1 = motor1_data_double->speed;   
    pwm2 = motor2_data_double->speed;
    
    //no position difference - maximum speed   
    if (posdif < glob_mot_data.max_pos_diff)
    {
        MOT_SendData(motor1_data_double, CAN_COM_SSET, glob_mot_data.pwm_max);
        MOT_SendData(motor2_data_double, CAN_COM_SSET, glob_mot_data.pwm_max);
    }
    else if (gt == 0)  //position1 > position2
    {
        //adjust pwm
        pwm1 -= glob_mot_data.pwm_delta;
        pwm2 += glob_mot_data.pwm_delta;
        
        //check min pwm limit
        if (pwm1 < glob_mot_data.pwm_min)
            pwm1 = glob_mot_data.pwm_min;
        //check max pwm limit    
        if (pwm2 > glob_mot_data.pwm_max) 
            pwm2 = glob_mot_data.pwm_max;  
    }
    else if (gt == 1)  //position2 > position1
    {
        //adjust pwm
        pwm1 += glob_mot_data.pwm_delta;
        pwm2 -= glob_mot_data.pwm_delta;
        
        //check max pwm limit
        if (pwm1 > glob_mot_data.pwm_max)  
            pwm1 = glob_mot_data.pwm_max;
        //check min pwm limit    
        if (pwm2 < glob_mot_data.pwm_min) 
            pwm2 = glob_mot_data.pwm_min; 
    }
     
    //update speed
     MOT_SendData(motor1_data_double, CAN_COM_SSET, pwm1);
     MOT_SendData(motor2_data_double, CAN_COM_SSET, pwm2);
  
}

void MOT_SortPositions(void)
{
    int i,j=0;
    
    for (i = 0; i < g_dbl_motors; i++)
    {
        if (j==0)
        {
            mot_pos_diff[j].pos =  motor_data_double[i]->position; 
            mot_pos_diff[j].mot = motor_data_double[i];
            //mot_pos_diff[j].up_down = SPEED_UP;
        }
        else
        {
            if (motor_data_double[i]->position > mot_pos_diff[j-1].pos)
                mot_pos_diff[j].pos =  motor_data_double[i]->position; 
            else
            {
                mot_pos_diff[j].pos = mot_pos_diff[j-1].pos;
                mot_pos_diff[j-1].pos = motor_data_double[i]->position;
            }
             mot_pos_diff[j].mot = motor_data_double[i];     
        }       
        j++;
    }
}

void MOT_ProccesPositions(void)
{
    int i;
    uint32_t mid_idx = g_dbl_motors/2;
    
    for (i = 0; i < g_dbl_motors; i++)
    {
        if ( (abs(mot_pos_diff[i].pos-mot_pos_diff[mid_idx].pos)) > glob_mot_data.max_pos_diff)
        {
            mot_pos_diff[i].pwm -= glob_mot_data.pwm_delta;
            if (mot_pos_diff[i].pwm < glob_mot_data.pwm_min)
                mot_pos_diff[i].pwm = glob_mot_data.pwm_min;
        }
        else
        {
            mot_pos_diff[i].pwm += glob_mot_data.pwm_delta;
            if (mot_pos_diff[i].pwm > glob_mot_data.pwm_max)
                mot_pos_diff[i].pwm = glob_mot_data.pwm_max;
        }
    }  
}

void MOT_ProcessPositions(void)
{
    int i;
    uint32_t mid_idx = g_dbl_motors/2;
    
    for (i = 0; i < g_dbl_motors; i++)
    {
        if (i != mid_idx)
        {
            if ( (abs(mot_pos_diff[i].pos-mot_pos_diff[mid_idx].pos)) > glob_mot_data.max_pos_diff)
            {
                if (mot_pos_diff[i].pos > mot_pos_diff[mid_idx].pos)
                {
                    mot_pos_diff[i].retries++;
                    mot_pos_diff[i].pwm -= glob_mot_data.pwm_delta;
                    if (mot_pos_diff[i].pwm < glob_mot_data.pwm_min)
                        mot_pos_diff[i].pwm = glob_mot_data.pwm_min;
                }
                else
                {
                    mot_pos_diff[i].pwm += glob_mot_data.pwm_delta;
                    if (mot_pos_diff[i].pwm > glob_mot_data.pwm_max)
                        mot_pos_diff[i].pwm = glob_mot_data.pwm_max;
                }
            }
            else
            {
                mot_pos_diff[i].retries = 0;
            }
        }
    }  
}

uint32_t MOT_GetMedianPosition(void)
{
     int i;
     uint32_t position=0;
     
     for (i = 0; i < g_linked_motors; i++)
     {
         position += linked_motor_data[i]->position;
     }
     position /= g_linked_motors;
     
     return position;
}

uint32_t ProcessMedianPosition(void)
{
    int i;
    uint32_t pos_diff=0;
    
    uint32_t median_position = MOT_GetMedianPosition();
    
    for (i = 0; i < g_linked_motors; i++)
    {
        if ( abs(linked_motor_data[i]->position - median_position) > glob_mot_data.max_pos_diff)
        {
            pos_diff = 1;
            
            if (linked_motor_data[i]->position > median_position)  //motor runs faster
            {
                //adjust speed -> speed down
                linked_motor_data[i]->pwm -= glob_mot_data.pwm_delta;
                //check limit
                if (linked_motor_data[i]->pwm < glob_mot_data.pwm_min)
                    linked_motor_data[i]->pwm = glob_mot_data.pwm_min;
            }
            else   //motor runs slower
            {
                //adjust speed -> speed up
                linked_motor_data[i]->pwm += glob_mot_data.pwm_delta;
                //check limit
                if (linked_motor_data[i]->pwm > glob_mot_data.pwm_max)
                    linked_motor_data[i]->pwm = glob_mot_data.pwm_max;
            }
        }
    }
    return pos_diff;
}

void MOT_PositionLoopMulti(void)
{
    int i;
    if (ProcessMedianPosition())
    {
        //retry
        glob_mot_data.pos_diff_count++;
        //maximum retries - unable to adjust speed
        if (glob_mot_data.pos_diff_count > glob_mot_data.max_pos_diff_count)
        {
            glob_mot_data.pos_diff_count = 0;
             //send stop command
            for (i = 0; i < g_linked_motors; i++)
                MOT_SendData(linked_motor_data[i], CAN_COM_STOP, 0);
        }
        else
        {
            //update speed
            for (i = 0; i < g_linked_motors; i++)
                MOT_SendData(linked_motor_data[i], CAN_COM_SSET, linked_motor_data[i]->pwm);
        }
    }
    else
    {
       glob_mot_data.pos_diff_count = 0;
       //set maximum speed
       for (i = 0; i < g_linked_motors; i++)
           MOT_SendData(linked_motor_data[i], CAN_COM_SSET, glob_mot_data.pwm_max);
    }   
}