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

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

/*
 dynamische Servowegbegrenzung. Kanal 1 ist ESC, Kanal 2 ist Steuerservo.
 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!!!!

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

// Stärke der Fahreinflusses auf die Steuerung
const byte control = 2;

// ------------------------------------------------------------
// AB HIER BITTE NIX MEHR ÄNDERN
// ------------------------------------------------------------
const byte ctrlM1 = control - 1;

// Laufvariable des Ringspeichers
RCReceive escReceiver;
RCReceive servoReceiver;

Servo a1;

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

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

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

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

  // Servos definieren
  a1.attach(SERVO_1);

#ifndef __AVR_ATtinyX4__ 
#ifdef debug
  Serial.begin(57600); 
  Serial.flush();
  Serial.println("dynaServo");
  delay(100);
#endif
#endif
}

void loop() {

  // Aktuellen RC-Wert Gas lesen
  escReceiver.poll();

  // und gleich auch für's Steuerservo
  servoReceiver.poll();

  // Nullpunktsbestimmung ?
  if (escReceiver.hasNP() || servoReceiver.hasNP()) {
    doWork();
  } else {
    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();;

  // Nullpunkte festlegen
  escValue = escValue - escReceiver.getNP();
  servoValue = servoValue - servoReceiver.getNP();
  dbgOut("E:");
  dbgOut(escValue);
  dbgOut(",S:");
  dbgOut(servoValue);

  // die eigentliche Berechnung
  int servo1 = servoValue ;

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

  float divisor  = 1 + (ctrlM1 * (abs(escValue) / 128.0));
  dbgOut(",D:");
  dbgOut(divisor);

  servo1 = servo1 / divisor;  
  dbgOut(",C:");
  dbgOut(servo1);

  // Nullpunkte wieder hinzu
  servo1 = servo1 + 128;

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

  dbgOut(",SV:");
  dbgOutLn(servo1);
  // und einstellen
  a1.write(servo1);
}