#define Iny_1_in 4
#define Iny_2_in 5
#define Iny_3_in 6
#define Iny_4_in 7

#define Iny_1 8
#define Iny_2 9
#define Iny_3 10
#define Iny_4 11

#define Sensor_CKP 12
#define Led 13
 


double Const_RPM = 20.02; 
int RPM = 0;
int Contador = 0;
int Contador_muestras = 0;

float Const_iny_1 = 1/1044;
float Const_iny_2 = 2.795;
float Const_con_1 = 200/19;
float Tiempo_iny_pre = 0;
float Voltaje_MAF = 0;
float Voltaje_TPS = 0;

bool Estado_i1 = false;
bool Estado_iny_1_ultimo = true;
bool Estado_i2 = false;
bool Estado_iny_2_ultimo = true;
bool Estado_i3 = false;
bool Estado_iny_3_ultimo = true;
bool Estado_i4 = false;
bool Estado_iny_4_ultimo = true;

bool Sensor_CKP_ultimo = true;
int Contador_CKP = 0;

float Sensor_TPS = 0;
float Sensor_MAF = 0;

int Vel_motor = 0;

float Tiempo_iny_1 = 0;
int Tiempo_iny_2 = 0;
int Tiempo_iny_3 = 0;
int Tiempo_iny_4 = 0;

int Contador_iny_1 = 0;
int Contador_iny_2 = 0;
int Contador_iny_3 = 0;
int Contador_iny_4 = 0;


int RPM_pru = 500;

void setup() {
  pinMode(Iny_1_in, INPUT_PULLUP);
  pinMode(Iny_2_in, INPUT);
  pinMode(Iny_3_in, INPUT);
  pinMode(Iny_4_in, INPUT);

  pinMode(Iny_1, OUTPUT);
  pinMode(Iny_2, OUTPUT);
  pinMode(Iny_3, OUTPUT);
  pinMode(Iny_4, OUTPUT);
  
  pinMode(Led, OUTPUT);
  analogReference(INTERNAL);
  //pinMode(A0, INPUT);
  //pinMode(A1, INPUT);
  
  pinMode(13, OUTPUT);
  //pinMode(12, OUTPUT);
  pinMode(Sensor_CKP, INPUT);
  cli();

  TCCR1A = 0;
  TCCR1B = 0;

  TCCR1B |= B00000101;
  TIMSK1 |= B00000010;
  OCR1A = 125;

  TCCR2A = 0;
  TCCR2B = 0;
  
  TCCR2B |= B00000010;
  TIMSK2 |= B00000100;
  OCR2B = 200;
  
  sei();
  Serial.begin(9600);
}

void loop() {
  
  
  
/*  ADMUX |= B00000001;
  ADMUX |= B00000000;
  ADCSRA |= B11000000;

  while(bit_is_set(ADCSRA, ADSC));
    float Sensor_MAF = ADCL | (ADCH << 8);
  ADCL = 0;
  ADMUX |= B00000000;
  ADMUX |= B00000000;
  ADCSRA |= B11000000;

  while(bit_is_set(ADCSRA, ADSC));
    float Sensor_TPS = ADCL | (ADCH << 8);
  ADCL = 0;
  */

  
  //Sensor_TPS = map(analogRead(A0),0,1024,20,200);
  //Sensor_MAF = map(analogRead(A1),0,1024,0,1024);
  

  Sensor_MAF = analogRead(A1);
  Sensor_TPS = analogRead(A0);
  
  Voltaje_MAF = (Sensor_MAF*5)/1024;
  Voltaje_TPS = (Sensor_TPS*5)/1024;
  //Serial.println(Sensor_MAF);
  //Serial.println(Voltaje_MAF);
  //Serial.println(RPM);
  //Serial.println(Voltaje_MAF*4.7);
  //Serial.println(Voltaje_TPS*4.7);
  //Tiempo_iny_pre = ((RPM*pow(Voltaje_MAF, Const_iny_2))/1044)+1;
  Tiempo_iny_pre = ((RPM_pru*pow(Voltaje_MAF, Const_iny_2))/1044)+1;
  Tiempo_iny_1=10*(Tiempo_iny_pre+10);
  Serial.println(Tiempo_iny_1);
  
  //Tiempo_iny_1=Sensor_MAF;
  Tiempo_iny_2=Sensor_MAF;
  Tiempo_iny_3=Sensor_TPS;
  Tiempo_iny_4=Sensor_TPS;
}


ISR(TIMER2_COMPB_vect){
  TCNT2 = 0;
  
  //Estado_i2 = !Estado_i2;
  Contador_muestras++;
  
  if(digitalRead(Iny_1_in)==true || Estado_iny_1_ultimo==false){
    Contador_iny_1++;
    Estado_iny_1_ultimo=false;
    if(Contador_iny_1<=Tiempo_iny_1){
      digitalWrite(Iny_1, HIGH);
    }else{
      digitalWrite(Iny_1, LOW);
      Contador_iny_1=0;
      Estado_iny_1_ultimo=true;
    }
  }

  if(digitalRead(Iny_2_in)==true || Estado_iny_2_ultimo==false){
    Contador_iny_2++;
    Estado_iny_2_ultimo=false;
    if(Contador_iny_2<=Tiempo_iny_2){
      digitalWrite(Iny_2, HIGH);
    }else{
      digitalWrite(Iny_2, LOW);
      Contador_iny_2=0;
      Estado_iny_2_ultimo=true;
    }
  }

  if(digitalRead(Iny_3_in)==true || Estado_iny_3_ultimo==false){
    Contador_iny_3++;
    Estado_iny_3_ultimo=false;
    if(Contador_iny_3<=Tiempo_iny_3){
      digitalWrite(Iny_3, HIGH);
    }else{
      digitalWrite(Iny_3, LOW);
      Contador_iny_3=0;
      Estado_iny_3_ultimo=true;
    }
  }

  if(digitalRead(Iny_4_in)==true || Estado_iny_4_ultimo==false){
    Contador_iny_4++;
    Estado_iny_4_ultimo=false;
    if(Contador_iny_4<=Tiempo_iny_4){
      digitalWrite(Iny_4, HIGH);
    }else{
      digitalWrite(Iny_4, LOW);
      Contador_iny_4=0;
      Estado_iny_4_ultimo=true;
    }
  }
  
  if(digitalRead(Sensor_CKP)!=Sensor_CKP_ultimo && digitalRead(Sensor_CKP)==true){
    Contador_CKP++;
    Sensor_CKP_ultimo = true;
  }else if (digitalRead(Sensor_CKP)==false){
    Sensor_CKP_ultimo = false;
  }
  
  if(Contador_muestras==1000){
    RPM = Contador_CKP*Const_RPM;
    Contador_muestras = 0;
    Contador_CKP = 0;
  }

  if(RPM>=1000){
    digitalWrite(Led,HIGH);
  }else{
    digitalWrite(Led,LOW);
  }
}

ISR(TIMER1_COMPA_vect){
  TCNT1 = 0;
  
  
  
}