#include <debug.h>
#include <makros.h>
#include <RCReceive.h>
#include <Servo.h>

/*
 Kreuzmischer. Kanal 1 ist Geschwindigkeit, Kanal 2 ist Steuerung.
 Der Empfänger wird vom Pin 2 und 3 gelesen, 
 */

// Hardwareanbindung für Arduino Hardware
// Empfängerkanäle
const byte PIN_RC_THR = 2; // das ist INT 0 aber Pin 2!!!!
const byte PIN_RC_STE = 3; // das ist INT 1 aber Pin 3!!!!

// Modus Soft oder Hard
const byte PIN_MODE = 4;
const byte PIN_REVERS_1 = 7;
const byte PIN_REVERS_2 = 8;

// Ausgänge
const byte SERVO_1 = 9; // PWM Kanal 
const byte SERVO_2 = 10; // PWM Kanal
const byte LED = 13; // LED auf dem Board

RCReceive escReceiver;
RCReceive servoReceiver;

Servo a1, a2;

void setup() {
  // Kanäle auf Ausgang, und dann deaktivieren
  pinMode(SERVO_1, OUTPUT);
  pinMode(SERVO_2, OUTPUT);

  pinMode(LED, OUTPUT);
  digitalWrite(LED, LOW);

  // Eingang für RC
  pinMode(PIN_RC_THR, INPUT_PULLUP); 
  pinMode(PIN_RC_STE, INPUT_PULLUP);     

  escReceiver.attachInt(PIN_RC_THR);
  servoReceiver.attachInt(PIN_RC_STE);

  // Eingang Modus
  pinMode(PIN_MODE, INPUT_PULLUP); 
  pinMode(PIN_REVERS_1, INPUT_PULLUP); 
  pinMode(PIN_REVERS_2, INPUT_PULLUP); 

  // Servos definieren
  a1.attach(SERVO_1);
  a2.attach(SERVO_2);

  a1.write(90);
  a2.write(90);    

#ifndef __AVR_ATtinyX4__ 
#ifdef debug
  Serial.begin(57600); 
  Serial.flush();
  Serial.println("RC V-mixer");
  delay(100);
#endif
#endif
}

void loop() {

  dbgOut("E:");
  dbgOut(escReciver.getLastRCValue());
  dbgOut(",S:");
  dbgOutLn(servoReciver.getLastRCValue());

  if (escReceiver.hasError() || servoReceiver.hasError()) {
    a1.write(90);
    a2.write(90);    
  }
  else if (escReceiver.hasNP() && servoReceiver.hasNP()) {
    doWork();
  }
  else {
    int value = escReceiver.getValue();
    dbgOut("E:");
    dbgOut(value);
    value = servoReceiver.getValue();
    dbgOut("S:");
    dbgOut(value);
    dbgOutLn(", no NP");
  }

#ifdef debug
  delay(100);
#endif
}

/*
Modus Soft berechnung, Werte werden jeweils halbiert und dann addiert.
 */
void doWork() {
  // Werte holen
  int escValue = escReceiver.getValue();
  int servoValue = servoReceiver.getValue();
  dbgOut("E:");
  dbgOut(escValue);
  dbgOut(",S:");
  dbgOutLn(servoValue);

  // Nullpunkte festlegen
  escValue = escValue - escReceiver.getNP();
  servoValue = servoValue - servoReceiver.getNP();

  // Hier wird zwischen hard und soft unterschieden.
  if (digitalRead(PIN_MODE) == 0) {
    // jetzt Wertebereich verkleinern
    escValue = escValue / 2;
    servoValue = servoValue / 2;
  }  

  // die eigentliche Berechnung
  int servo1 = escValue + servoValue;
  int servo2 = escValue - servoValue;

  // Und damit das nicht aus dem Ruder läuft, etwas begrenzen
  servo1 = constrain(servo1, -128, 128);
  servo2 = constrain(servo2, -128, 128);

  if (digitalRead(PIN_REVERS_1) == 0) {
    // Servokanal 1 umkehren
    servo1 = servo1 * -1;
  }

  if (digitalRead(PIN_REVERS_2) == 0) {
    // Servokanal 1 umkehren
    servo2 = servo2 * -1;
  }

  // Nullpunkte wieder hinzu
  servo1 = servo1 + 128;
  servo2 = servo2 + 128;

  // Auf den Servo umsetzen
  servo1 = map(servo1,0, 255,0,180);
  servo2 = map(servo2,0, 255,0,180);

  // und einstellen
  a1.write(servo1);
  a2.write(servo2);
}