// ZenDrive Simulator firmware
// Copyright (c) 2021 - Valentin Boulanger
// Version : 1.2.1
//--------------------------------------------------------------------

#include "ZenDrive.h"

// Pins declaration
const int SPEEDS_PIN = A0;
const int HANDBRAKE_PIN = A1;
const int CLUTCH_PIN = A2;
const int BRAKE_PIN = A3;
const int ACCELERATOR_PIN = A4;
const int DIRECTION_PIN = A5;
const int LEFT_BLINKER_PIN = 2;
const int RIGHT_BLINKER_PIN = 3;
const int WARNING_PIN = 4;
const int CRUISE_PIN = 5;
const int CRUISE_UP_PIN = 6;
const int CRUISE_DOWN_PIN = 7;
const int STARTER_PIN = 8;
const int HORN_PIN = 9;
const int LIGHTS_ON_PIN = 10;
const int ROAD_LIGHTS_PIN = 16;
const int HEAD_LIGHTS_PIN = 14;
const int FOG_LIGHTS_PIN = 15;

// Enabling modules
const bool USE_GEARSHIFT = true;
const bool USE_PEDALS = true;
const bool USE_WHEEL = true;

void setup() { 
  // Initialize sensors
  pinMode(SPEEDS_PIN, INPUT);
  pinMode(HANDBRAKE_PIN, INPUT_PULLUP);
  pinMode(CLUTCH_PIN, INPUT_PULLUP);
  pinMode(BRAKE_PIN, INPUT_PULLUP);
  pinMode(ACCELERATOR_PIN, INPUT_PULLUP);
  pinMode(DIRECTION_PIN, INPUT_PULLUP);
  pinMode(LEFT_BLINKER_PIN, INPUT_PULLUP);
  pinMode(RIGHT_BLINKER_PIN, INPUT_PULLUP);
  pinMode(WARNING_PIN, INPUT_PULLUP);
  pinMode(CRUISE_PIN, INPUT_PULLUP);
  pinMode(CRUISE_UP_PIN, INPUT_PULLUP);
  pinMode(CRUISE_DOWN_PIN, INPUT_PULLUP);
  pinMode(STARTER_PIN, INPUT_PULLUP);
  pinMode(HORN_PIN, INPUT_PULLUP);
  pinMode(LIGHTS_ON_PIN, INPUT_PULLUP);
  pinMode(ROAD_LIGHTS_PIN, INPUT_PULLUP);
  pinMode(HEAD_LIGHTS_PIN, INPUT_PULLUP);
  pinMode(FOG_LIGHTS_PIN, INPUT_PULLUP);

  // Set current states
  /* Gearshift module */
  if(USE_GEARSHIFT){
    int value = analogRead(SPEEDS_PIN);
    if(value < 200) ZenDrive.switchSpeed1();
    else if(value < 326) ZenDrive.switchSpeed2();
    else if(value < 421) ZenDrive.switchSpeed3();
    else if(value < 493) ZenDrive.switchSpeed4();
    else if(value < 551) ZenDrive.switchSpeed5();
    else if(value < 596) ZenDrive.switchSpeed6();
    else if(value < 634) ZenDrive.switchSpeedR();
    else ZenDrive.switchNeutral();
    //Read handbrake
    ZenDrive.setHandbrake(map(analogRead(HANDBRAKE_PIN), 0, 1023, 0, 255));
  }
  /* Pedals module */
  if(USE_PEDALS){
    // Read clutch
    ZenDrive.setClutch(map(analogRead(CLUTCH_PIN), 0, 1023, 0, 255));
    // Read brake
    ZenDrive.setBrake(map(analogRead(BRAKE_PIN), 0, 1023, 0, 255));
    // Read accelerator
    ZenDrive.setAccelerator(map(analogRead(ACCELERATOR_PIN), 0, 1023, 0, 255));
  }
  /* Steering wheel module */
  if(USE_WHEEL){
    // Blinkers
    if(digitalRead(LEFT_BLINKER_PIN)){
      ZenDrive.setBlinkerLeft(true);
      ZenDrive.setBlinkerRight(false);
    }
    else if(digitalRead(RIGHT_BLINKER_PIN)){
      ZenDrive.setBlinkerLeft(false);
      ZenDrive.setBlinkerRight(true);
    }
    else{
      ZenDrive.setBlinkerLeft(false);
      ZenDrive.setBlinkerRight(false);
    }
    // Right blinker
    ZenDrive.setBlinkerRight(digitalRead(RIGHT_BLINKER_PIN));
    // Warning
    ZenDrive.setWarning(digitalRead(WARNING_PIN));
    // Lights
    if (!digitalRead(LIGHTS_ON_PIN)) ZenDrive.switchLightsOn();
    else if(!digitalRead(ROAD_LIGHTS_PIN)) ZenDrive.switchRoadLights();
    else ZenDrive.switchLightsOff();
    // Head lights
    ZenDrive.setHeadLights(digitalRead(HEAD_LIGHTS_PIN));
    // Fog lights
    ZenDrive.setFogLights(digitalRead(FOG_LIGHTS_PIN));
    // Starter
    ZenDrive.setStarter(digitalRead(STARTER_PIN));
    // Read direction
    ZenDrive.setDirection(map(analogRead(DIRECTION_PIN), 0, 1023, -127, 127));
  } 
  // Initialize ZenDrive Library
  ZenDrive.begin();
}

void loop() {
  // Set refresh to false
  ZenDrive.gearshiftNeedRefresh = false;
  ZenDrive.pedalsNeedRefresh = false;
  ZenDrive.wheelNeedRefresh = false;
  
  /* Gearshift module */
  if(USE_GEARSHIFT){
    //Read speed states on the analog shared pin
    int value = analogRead(SPEEDS_PIN);
    if(value < 200) ZenDrive.switchSpeed1();
    else if(value < 326) ZenDrive.switchSpeed2();
    else if(value < 421) ZenDrive.switchSpeed3();
    else if(value < 493) ZenDrive.switchSpeed4();
    else if(value < 551) ZenDrive.switchSpeed5();
    else if(value < 596) ZenDrive.switchSpeed6();
    else if(value < 634) ZenDrive.switchSpeedR();
    else ZenDrive.switchNeutral();
    //Read handbrake
    ZenDrive.setHandbrake(map(analogRead(HANDBRAKE_PIN), 0, 1023, 0, 255));
    // Send gearshift states if there are changes
    if(ZenDrive.gearshiftNeedRefresh) ZenDrive.sendGearshiftStates();
  }
  /* Pedals module */
  if(USE_PEDALS){
    // Read clutch
    ZenDrive.setClutch(map(analogRead(CLUTCH_PIN), 0, 1023, 0, 255));
    // Read brake
    ZenDrive.setBrake(map(analogRead(BRAKE_PIN), 0, 1023, 0, 255));
    // Read accelerator
    ZenDrive.setAccelerator(map(analogRead(ACCELERATOR_PIN), 0, 1023, 0, 255));
    // Send pedals states if they are changes
    if(ZenDrive.pedalsNeedRefresh) ZenDrive.sendPedalsStates();
  }
  /* Steering wheel module */  
  if(USE_WHEEL){
    // Blinkers
    if(!digitalRead(LEFT_BLINKER_PIN)){
      ZenDrive.setBlinkerLeft(true);
      ZenDrive.setBlinkerRight(false);
    }
    else if(!digitalRead(RIGHT_BLINKER_PIN)){
      ZenDrive.setBlinkerLeft(false);
      ZenDrive.setBlinkerRight(true);
    }
    else{
      ZenDrive.setBlinkerLeft(false);
      ZenDrive.setBlinkerRight(false);
    }
    // Warning
    ZenDrive.setWarning(digitalRead(WARNING_PIN));
    // Lights
    if (!digitalRead(LIGHTS_ON_PIN)) ZenDrive.switchLightsOn();
    else if(!digitalRead(ROAD_LIGHTS_PIN)) ZenDrive.switchRoadLights();
    else ZenDrive.switchLightsOff();
    // Head lights
    ZenDrive.setHeadLights(digitalRead(HEAD_LIGHTS_PIN));
    // Fog lights
    ZenDrive.setFogLights(digitalRead(FOG_LIGHTS_PIN));
    // Starter
    ZenDrive.setStarter(digitalRead(STARTER_PIN));
    // Horn
    ZenDrive.setHorn(!digitalRead(HORN_PIN));
    // Cruise on
    if(!digitalRead(CRUISE_PIN)) ZenDrive.activeCruise();
    // Cruise up
    if(!digitalRead(CRUISE_UP_PIN)) ZenDrive.increaseCruise();
    // Cruise down
    if(!digitalRead(CRUISE_DOWN_PIN)) ZenDrive.decreaseCruise();  
    // Read direction
    ZenDrive.setDirection(map(analogRead(DIRECTION_PIN), 0, 1023, -127, 127));
    // Send steering wheel states if they are changes
    if(ZenDrive.wheelNeedRefresh) ZenDrive.sendWheelStates();
  }
  delay(50);
}