/* 
 * File:   velocimetro.c
 * Author: Christian
 *
 * Created on 23 de mayo de 2019, 0:52
 */

#include "main.h"

#ifdef _IC1_H
    #define COUNTS_PER_CAPEVNT 16.0     // cantidad de cuentas (del encoder) por evento de captura
    #define SPACE_PER_CAPEVNT 0.11681288379454361701827490592101 //Espacio en cm por evento de captura
#else
    #define COUNTS_PER_CAPEVNT 4.0 
    #define SPACE_PER_CAPEVNT (SPACECONST_cm*COUNTS_PER_CAPEVNT) //Espacio en cm por evento de captura
#endif  

#define COUNTS_PER_REV 2797.0                                // cantidad de cuentas (del encoder) por revolucion
#define REVS_PER_CAPTURE COUNTS_PER_CAPEVNT/COUNTS_PER_REV // cantidad de capturas en una revolucion

#define SPACECONST_cm  0.0073008052371589 // longitud de la seccion rueda que mide cada cuenta del codificador (en cm)
#define CAPTURE_SPACECONST_cm COUNTS_PER_CAPEVNT*SPACECONST_cm      // longitud de rueda por cada captura (en cm)
#define CAPTURE_SPACECONST_m (COUNTS_PER_CAPEVNT*SPACECONST_cm)/100 // Idem pero en metros

#define x8_PRESCALED_TMR_COUNT_VALUE_s (Tcy_s)//*8 // Valor de cada cuenta del PR del T3 en segundos


const double captureSpeedConst_cm_s = (CAPTURE_SPACECONST_cm) / (x8_PRESCALED_TMR_COUNT_VALUE_s); // Velocidad medida por cada cuenta/captura del IC en cm/s
const double captureSpeedConst_m_s = ((CAPTURE_SPACECONST_cm) / (x8_PRESCALED_TMR_COUNT_VALUE_s))/100; // Velocidad medida por cada cuenta/captura del IC en m/s
const double captureSpeedConst_rps = ((REVS_PER_CAPTURE) / (x8_PRESCALED_TMR_COUNT_VALUE_s)); // Velocidad medida por cada cuenta/captura del IC en rps
const double captureSpeedConst_rpm = ((REVS_PER_CAPTURE) / (x8_PRESCALED_TMR_COUNT_VALUE_s))*60; // Velocidad medida por cada cuenta/captura del IC en rpm

extern volatile uint32_t IC1_Current[4];  // Ultimos valores del PR capturados por
extern volatile uint32_t IC2_Current[4]; // IC1 e IC2 
extern volatile uint32_t IC1_Previous;  // (Actuales e inmdiatamente previos)
extern volatile uint32_t IC2_Previous;

volatile bool IC1_Direction; //-Direccion del giro 0=positivo 1=negativo
extern volatile bool IC1_isFresh;//-Determina si los valores capturados son nuevos
volatile bool IC2_Direction;
extern volatile bool IC2_isFresh;

uint32_t ATime_2; // Delta de T (diferencia de tiempo)
uint32_t ATime_1; // ...

//double speedRight = 0.0; // Velocidad de la rueda derecha
//double speedLeftt = 0.0; // Idem rueda izquierda


double cms_to_raw(double cm)
{
    return cm/captureSpeedConst_cm_s;
}
double ms_to_raw(double m)
{
    return m/captureSpeedConst_m_s;
}
double rps_to_raw(double rps)
{
    return rps/captureSpeedConst_rps;
}
double rpm_to_raw(double rpm)
{
    return rpm/captureSpeedConst_rpm;
}

double counts_to_cms(double captures)
{
    return captures * SPACE_PER_CAPEVNT;
}
double raw_to_cms(double raw)
{
    return raw * captureSpeedConst_cm_s;
}


//double get_speed_raw_R(void)
//{
//    if(IC1_isFresh){
//        IC1_isFresh = false;
//    }else{
//        IC1_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_1 = ((IC1_Current<IC1_Previous)?IC1_Current+(UINT32_MAX-IC1_Previous):IC1_Current-IC1_Previous);
//    
//    if(IC1_Direction) return (-1.0 / (double)ATime_1);
//    else return (1.0 / (double)ATime_1);
//}
//double get_speed_raw_L(void)
//{
//    if(IC2_isFresh){
//        IC2_isFresh = false;
//    }else{
//        IC2_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_2 = ((IC2_Current<IC2_Previous)?IC2_Current+(UINT32_MAX-IC2_Previous):IC2_Current-IC2_Previous);
//    
//    if(IC2_Direction) return (-1.0 / (double)ATime_2);
//    else return (1.0 / (double)ATime_2);
//}
//double get_speed_raw_M(void)
//{
//    uint32_t ic1_p = IC1_Previous;
//    uint32_t ic1_c = IC1_Current;
//    uint32_t ic2_p = IC2_Previous;
//    uint32_t ic2_c = IC2_Current;
//    
//    
//    if(IC1_isFresh){        
//        IC1_isFresh = false;
//    }else{
//        IC1_Current = _Pwm_POSTMR;
//    }
//    if(IC2_isFresh){        
//        IC2_isFresh = false;
//    }else{
//        IC2_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_1 = ((ic1_c<ic1_p)?ic1_c+(UINT32_MAX-ic1_p):ic1_c-ic1_p);
//    ATime_2 = ((ic2_c<ic2_p)?ic2_c+(UINT32_MAX-ic2_p):ic2_c-ic2_p);
//    
//    if(IC2_Direction) return -2.0/(double)( ATime_1 + ATime_2 );
//    else return 2.0/(double)( ATime_1 + ATime_2 );    
//    
//}
//double get_abs_speed_raw_M(void)
//{
//    uint32_t ic1_p = IC1_Previous;
//    uint32_t ic1_c = IC1_Current;
//    uint32_t ic2_p = IC2_Previous;
//    uint32_t ic2_c = IC2_Current;
//    
////    double aux;
//    
//    if(IC1_isFresh){        
//        IC1_isFresh = false;
//    }else{
//        IC1_Current = _Pwm_POSTMR;
//    }
//    if(IC2_isFresh){        
//        IC2_isFresh = false;
//    }else{
//        IC2_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_1 = ((ic1_c<ic1_p)?ic1_c+(UINT32_MAX-ic1_p):ic1_c-ic1_p);
//    ATime_2 = ((ic2_c<ic2_p)?ic2_c+(UINT32_MAX-ic2_p):ic2_c-ic2_p);
//    
//    return 2.0/(double)( ATime_1 + ATime_2 );    
//}
//
//double get_speed_cms_R(void)
//{
//    if(IC1_isFresh){
//        IC1_isFresh = false;
//    }else{
//        IC1_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_1 = ((IC1_Current<IC1_Previous)?IC1_Current+(UINT32_MAX-IC1_Previous):IC1_Current-IC1_Previous);
//    
//    if(IC1_Direction) return (-captureSpeedConst_cm_s / (double)ATime_1);
//    else return (captureSpeedConst_cm_s / (double)ATime_1);
//}
//double get_speed_cms_L(void)
//{
//    if(IC2_isFresh){
//        IC2_isFresh = false;
//    }else{
//        IC2_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_2 = ((IC2_Current<IC2_Previous)?IC2_Current+(UINT32_MAX-IC2_Previous):IC2_Current-IC2_Previous);
//    
//    if(IC2_Direction) return (-captureSpeedConst_cm_s / (double)ATime_2);
//    else return (captureSpeedConst_cm_s / (double)ATime_2);
//}
double get_speed_cms_M(void)
{
    uint32_t ic1_p = IC1_Previous;
    uint32_t ic1_c[4];
    ic1_c[0] = IC1_Current[0];
    ic1_c[1] = IC1_Current[1];
    ic1_c[2] = IC1_Current[2];
    ic1_c[3] = IC1_Current[3];
    uint32_t ic2_p = IC2_Previous;
    uint32_t ic2_c[4];
    ic2_c[0] = IC2_Current[0];
    ic2_c[1] = IC2_Current[1];
    ic2_c[2] = IC2_Current[2];
    ic2_c[3] = IC2_Current[3];
    
    
    if(IC1_isFresh){        
        IC1_isFresh = false;
    }else{
        IC1_Current[0] = _Pwm_POSTMR;
        IC1_Current[1] = _Pwm_POSTMR;
        IC1_Current[2] = _Pwm_POSTMR;
        IC1_Current[3] = _Pwm_POSTMR;
    }
    if(IC2_isFresh){        
        IC2_isFresh = false;
    }else{
        IC2_Current[0] = _Pwm_POSTMR;
        IC2_Current[1] = _Pwm_POSTMR;
        IC2_Current[2] = _Pwm_POSTMR;
        IC2_Current[3] = _Pwm_POSTMR;
    }
    
        ATime_1 = ((ic1_c[0]<ic1_p)?ic1_c[0]+(UINT32_MAX-ic1_p):ic1_c[0]-ic1_p);
        ATime_2 = ((ic2_c[0]<ic2_p)?ic2_c[0]+(UINT32_MAX-ic2_p):ic2_c[0]-ic2_p);
    int i;
    for(i=1;i<4;i++){
        ATime_1 += ((ic1_c[i]<ic1_c[i-1])?ic1_c[i]+(UINT32_MAX-ic1_c[i-1]):ic1_c[i]-ic1_c[i-1]);
        ATime_2 += ((ic2_c[i]<ic2_c[i-1])?ic2_c[i]+(UINT32_MAX-ic2_c[i-1]):ic2_c[i]-ic2_c[i-1]);
    }
    
    if(IC2_Direction) return -(8.0*captureSpeedConst_cm_s)/(double)( ATime_1 + ATime_2 );
    else return (8.0*captureSpeedConst_cm_s)/(double)( ATime_1 + ATime_2 );    
    
}

//double get_speed_ms_R(void)
//{
//    if(IC1_isFresh){
//        IC1_isFresh = false;
//    }else{
//        IC1_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_1 = ((IC1_Current<IC1_Previous)?IC1_Current+(UINT32_MAX-IC1_Previous):IC1_Current-IC1_Previous);
//    
//    if(IC1_Direction) return (-captureSpeedConst_m_s / (double)ATime_1);
//    else return (captureSpeedConst_m_s / (double)ATime_1);
//}
//double get_speed_ms_L(void)
//{
//    if(IC2_isFresh){
//        IC2_isFresh = false;
//    }else{
//        IC2_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_2 = ((IC2_Current<IC2_Previous)?IC2_Current+(UINT32_MAX-IC2_Previous):IC2_Current-IC2_Previous);
//    
//    if(IC2_Direction) return (-captureSpeedConst_m_s / (double)ATime_2);
//    else return (captureSpeedConst_m_s / (double)ATime_2);
//}
//
//double get_speed_rpm_R(void)
//{
//    if(IC1_isFresh){
//        IC1_isFresh = false;
//    }else{
//        IC1_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_1 = ((IC1_Current<IC1_Previous)?IC1_Current+(UINT32_MAX-IC1_Previous):IC1_Current-IC1_Previous);
//    
//    if(IC1_Direction) return (-captureSpeedConst_rpm / (double)ATime_1);
//    else return (captureSpeedConst_rpm / (double)ATime_1);
//}
//double get_speed_rpm_L(void)
//{
//    if(IC2_isFresh){
//        IC2_isFresh = false;
//    }else{
//        IC2_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_2 = ((IC2_Current<IC2_Previous)?IC2_Current+(UINT32_MAX-IC2_Previous):IC2_Current-IC2_Previous);
//    
//    if(IC2_Direction) return (-captureSpeedConst_rpm / (double)ATime_2);
//    else return (captureSpeedConst_rpm / (double)ATime_2);
//}
//
//double get_speed_rps_R(void)
//{
//    if(IC1_isFresh){
//        IC1_isFresh = false;        
//    }else{
//        IC1_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_1 = ((IC1_Current<IC1_Previous)?IC1_Current+(UINT32_MAX-IC1_Previous):IC1_Current-IC1_Previous);
//    
//    if(IC1_Direction) return (-captureSpeedConst_rps / (double)ATime_1);
//    else return (captureSpeedConst_rps / (double)ATime_1);
//}
//double get_speed_rps_L(void)
//{
//    if(IC2_isFresh){
//        IC2_isFresh = false;
//    }else{
//        IC2_Current = _Pwm_POSTMR;
//    }
//    
//    ATime_2 = ((IC2_Current<IC2_Previous)?IC2_Current+(UINT32_MAX-IC2_Previous):IC2_Current-IC2_Previous);
//    
//    if(IC2_Direction) return (-captureSpeedConst_rps / (double)ATime_2);
//    else return (captureSpeedConst_rps / (double)ATime_2);
//}        