#include "FixedPoints.h"
#include "FixedPointsCommon.h"

#include "fuelmap.h"

int O2_SENSOR = A0;
int RPM_SENSOR = A1;
int THROTTLE_POS_SENSOR = A2;
int COOL_TEMP_SENSOR = A3;
int SPEED_SENSOR = A4;
int MANIFOLD_PRESSURE_SENSOR = A5;
int NB_O2_SENSOR = A6;

int INJECTOR_PULSE = 12;
int ISSUE_DETECTED_LED = 11;
int SYSTEM_STATUS_LED = 9;

UQ8x8 current_afr;
UQ8x8 current_speed;
UQ8x8 current_map;
UQ8x8 current_cool_temp;
UQ8x8 current_rpm;
UQ8x8 current_throttle_pos;
UQ8x8 target_afr;

int aggro_mode = LOW;
int cruise_mode = HIGH;

enum EngineStateType{
  ACC_ON = 0,
  ENGINE_ON = 1
};

enum SystemStateType{
  OFF = 0,
  OPERATIONAL = 1,
  WARNING = 2,
  CRITICAL = 3
};

SystemStateType system_state;
EngineStateType engine_state;

int update_cycle_cntr = 0;
int loop_cycle = 10; //ms

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.println("*****************************************************************");
  Serial.println("*                         FIERO ECU                             *");
  Serial.println("*    Created By Justin Fitzpatrick - [email protected]   *");
  Serial.println("*                   VERSION 0.0.1-230203                        *");
  Serial.println("*****************************************************************");

  pinMode(2, INPUT);
  pinMode(3, INPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  read_sensors();
  determine_state();

  read_mode_switch();


  if (update_cycle_cntr >= 200){
    Serial.println("Target AFR:");
    Serial.println(target_afr.getInteger());
    Serial.println(target_afr.getFraction());
    Serial.println("Aggro mode: ");
    Serial.println(aggro_mode);
    Serial.println("Cruise mode: ");
    Serial.println(cruise_mode);
    Serial.println("MAP:");
    Serial.println(current_rpm.getInteger());
    update_cycle_cntr = 0;
  }

  if (analogRead(A0) > 512){
    digitalWrite(INJECTOR_PULSE, HIGH);
  } else {
    digitalWrite(INJECTOR_PULSE, LOW);
  }

  if (update_cycle_cntr <= 100){
    digitalWrite(SYSTEM_STATUS_LED, HIGH);
  } else {
    digitalWrite(SYSTEM_STATUS_LED, LOW);
  }

  update_cycle_cntr++;
  delay(loop_cycle);
}

void read_mode_switch(){
  aggro_mode = digitalRead(3);
  cruise_mode = digitalRead(2);
}

void read_sensors(){

}

void determine_state(){
  current_afr = calculate_current_afr(analogRead(O2_SENSOR));
  current_rpm = calculate_current_rpm(analogRead(RPM_SENSOR));
  current_map = calculate_current_load(analogRead(MANIFOLD_PRESSURE_SENSOR));

  if (current_rpm < 100){
    engine_state = ACC_ON;
    system_state = OFF;
  }

  target_afr = determine_target_afr();

  //Serial.println("*****************************************************************");
  //Serial.println("*         CURRENT AFR:                                          *");
  //Serial.println(current_afr.getInteger());
  //Serial.println(current_afr.getFraction());
  //Serial.println("*         CURRENT RPM:                                          *");
  //Serial.println(current_rpm);

  write_data_to_ECU(target_afr, current_afr, current_rpm);
}

void write_data_to_ECU(UQ8x8 tafr, UQ8x8 cafr, UQ8x8 crpm){
  //analogWrite(0, (float)tafr);
}

UQ8x8 calculate_current_afr(int afr_sensor_value){
  //Convert 0-1023 integer of 0V - 5V to current Air Fuel Ratio (Lamda) Value
  // 0V = 10.00 AFR
  // 4.99V = 19.98 AFR

  UQ8x8 NINE_POINT_NINE_EIGHT = 9.98;

  return afr_sensor_value * (NINE_POINT_NINE_EIGHT / 1023) + 10;
}

UQ8x8 calculate_current_rpm(int rpm_sensor_value){
  // Convert 0-1023 integer value of 0V-5V to current RPM of engine
  return rpm_sensor_value *  (6500 / 1023);
}

UQ8x8 calculate_current_load(int map_sensor_value){
  return map_sensor_value * (100 / 1023);
}

UQ8x8 determine_target_afr(){
  // Use all available relevant data to determine what our target AFR should be
  UQ8x8 target;

  int rpm_pos = 0;

  if (current_rpm >= 0 && current_rpm < 500){
    rpm_pos = 0;
  } else if (current_rpm >= 500 && current_rpm < 1000) {
    rpm_pos = 1;
  } else if (current_rpm >= 1000 && current_rpm < 1500) {
    rpm_pos = 2;
  } else if (current_rpm >= 1500 && current_rpm < 2000) {
    rpm_pos = 3;
  } else if (current_rpm >= 2000 && current_rpm < 2500) {
    rpm_pos = 4;
  } else if (current_rpm >= 2500 && current_rpm < 3000) {
    rpm_pos = 5;
  } else if (current_rpm >= 3000 && current_rpm < 3500) {
    rpm_pos = 6;
  } else if (current_rpm >= 3500 && current_rpm < 4000) {
    rpm_pos = 7;
  } else if (current_rpm >= 4000 && current_rpm < 4500) {
    rpm_pos = 8;
  } else if (current_rpm >= 4500 && current_rpm < 5000) {
    rpm_pos = 9;
  } else if (current_rpm >= 5000 && current_rpm < 5500) {
    rpm_pos = 10;
  } else if (current_rpm >= 5500) {
    rpm_pos = 11;
  }

  int load_pos = 0;

  if (current_map <=10) {
    load_pos = 0;
  } else if (current_map > 10 && current_map <= 20){
    load_pos = 1;
  } else if (current_map > 20 && current_map <= 30){
    load_pos = 2;
  } else if (current_map > 30 && current_map <= 40){
    load_pos = 3;
  } else if (current_map > 40 && current_map <= 50){
    load_pos = 4;
  } else if (current_map > 50 && current_map <= 60){
    load_pos = 5;
  } else if (current_map > 60 && current_map <= 70){
    load_pos = 6;
  } else if (current_map > 70 && current_map <= 80){
    load_pos = 7;
  } else if (current_map > 80 && current_map <= 90){
    load_pos = 8;
  } else if (current_map > 90){
    load_pos = 9;
  }

  if (aggro_mode){
    target = aggro_fuel_map[rpm_pos][load_pos];
  } else if (cruise_mode){
    target = cruise_fuel_map[rpm_pos][load_pos];
  } else {
    target = cruise_fuel_map[rpm_pos][load_pos];
  }

  return target;
  
}