//
// Display RC Channekl Inputs
//

#include <SoftwareSerial.h>
//#include "SabertoothSimplified.h"

// Sabertooth S1 Motor Driver
#define S1_PIN 5         // Sabertooth S1

// Flysky FS-i6X 10 Channel RC Transmitter and FS-iA6B Receiver
#define INPUT_CH1_PIN 2             // Throttle
#define INPUT_CH2_PIN 3             // Steering
#define INPUT_CH3_PIN 37            // Paul - please update this pin
#define INPUT_CH4_PIN 38            // Paul - please update this pin
#define INPUT_CH5_PIN A4            // Flysky Switch 5 (engine start/stop)
#define INPUT_CH6_PIN A8            // Flysky Switch 6 (clutch control)
#define INPUT_CH7_PIN A6            // Actuator
#define INPUT_CH8_PIN A7            // Flysky Var8 knob (chute rotation)

// Create the Sabertooth object ST
//SoftwareSerial SWSerial(NOT_A_PIN, S1_PIN);
//SabertoothSimplified ST(SWSerial);

//
// Global variable declarations and initialization
//

//
// Setup function
//

void setup() {
  Serial.begin(9600);
  //SWSerial.begin(9600);

  // FS-iA6B Receiver pin assignments
  pinMode(INPUT_CH1_PIN, INPUT);
  pinMode(INPUT_CH2_PIN, INPUT);
  pinMode(INPUT_CH3_PIN, INPUT);
  pinMode(INPUT_CH4_PIN, INPUT);
  pinMode(INPUT_CH5_PIN, INPUT);
  pinMode(INPUT_CH6_PIN, INPUT);
  pinMode(INPUT_CH7_PIN, INPUT);
  pinMode(INPUT_CH8_PIN, INPUT);

  delay(500);

  Serial.println("Setup complete - System Ready.");
}

//
// Main Loop function
//

void loop() {
  int ch1Value = pulseIn(INPUT_CH1_PIN, HIGH, 25000);
  int ch2Value = pulseIn(INPUT_CH2_PIN, HIGH, 25000);
  int ch3Value = pulseIn(INPUT_CH3_PIN, HIGH, 25000);
  int ch4Value = pulseIn(INPUT_CH4_PIN, HIGH, 25000);
  int ch5Value = pulseIn(INPUT_CH5_PIN, HIGH, 25000);
  int ch6Value = pulseIn(INPUT_CH6_PIN, HIGH, 25000);
  int ch7Value = pulseIn(INPUT_CH7_PIN, HIGH, 25000);
  int ch8Value = pulseIn(INPUT_CH8_PIN, HIGH, 25000);

  int inputA0 = analogRead(A0); 
  int inputA1 = analogRead(A1); 

  ch3Value = map(inputA0, 0, 1023, 2000, 1000);    // scale to RC receiver range 1000..2000
  ch4Value = map(inputA1, 0, 1023, 1000, 2000);    // scale to RC receiver range 1000..2000

  Serial.print("Ch1: "); Serial.print(ch1Value);
  Serial.print("   Ch2: "); Serial.print(ch2Value);
  Serial.print("   Ch3: "); Serial.print(ch3Value);
  Serial.print("   Ch4: "); Serial.print(ch4Value);
  Serial.print("   Ch5: "); Serial.print(ch5Value);
  Serial.print("   Ch6: "); Serial.print(ch6Value);
  Serial.print("   Ch7: "); Serial.print(ch7Value);
  Serial.print("   Ch8: "); Serial.print(ch8Value);
  Serial.println();

  delay(500);
}