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

#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;

const int INTERVAL = 15;

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);

  // Initialize ZenDrive Library
  ZenDrive.begin();

  // Send current states
  /* 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));
  ZenDrive.sendGearshiftStates();

  /* Pedals module */
  // 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));
  ZenDrive.sendPedalsStates();

  /* Steering wheel module */
  // Left blinker
  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));
  ZenDrive.sendWheelStates();
}

void loop() {
  // Set refresh to false
  ZenDrive.gearshiftNeedRefresh = false;
  ZenDrive.pedalsNeedRefresh = false;
  ZenDrive.wheelNeedRefresh = false;
  
  /* Gearshift module */
  //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 */
  // 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 */  
  // Left blinker
  ZenDrive.setBlinkerLeft(digitalRead(LEFT_BLINKER_PIN));
  // 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));
  // 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);
}
