// For: https://forum.arduino.cc/t/how-to-control-a-servomotor-with-mpu6050-without-arduino-servo-library/1044928/4

/*
*
*	Author : Zedric
*
*/

uint16_t a {};
uint16_t b {};
const uint16_t TIME {39999};
const uint16_t MIN {};
const uint16_t MAX {1023};
const uint16_t MIN_2 {1805};  // Spektrum @100% travel = 1102 to 1898 | @150% = 903 to 2097
const uint16_t MAX_2 {4193};  //
uint16_t chan[2] {0, 0};
volatile uint8_t i {};

uint16_t remap(long x, long in_min, long in_max, long out_min, long out_max);

ISR(TIMER1_OVF_vect)
{
  ADCSRA |= (1 << ADSC);
}

ISR(ADC_vect)
{
  chan[i] = ADC;
  i ^= (1 << 0);
  ADMUX ^= (1 << MUX0);
}

void setup()
{
  DDRB |= (1 << PORTB5) | (1 << PORTB2) | (1 << PORTB1);
  ADMUX |= (1 << REFS0);
  ADCSRA |= (1 << ADEN) | (1 << ADSC) | (1 << ADIE) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
  DIDR0 |= (1 << ADC1D) | (1 << ADC0D);
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;
  ICR1 = TIME;
  OCR1A = MIN_2;
  OCR1B = MIN_2;
  TIMSK1 = (1 << TOIE1);
  TCCR1A |= (1 << COM1A1) | (1 << COM1B1) | (1 << WGM11);
  TCCR1B |= (1 << WGM13) | (1 << WGM12) | (1 << CS11);  //Mode (14) Fast PWM & Clk/8
  sei();
  Serial.begin(115200);
}

void loop()
{
  if (a != chan[0] || b != chan[1])
  {
    a = chan[0];
    b = chan[1];
    OCR1A = remap(chan[0], MIN, MAX, MIN_2, MAX_2);
    OCR1B = remap(chan[1], MIN, MAX, MIN_2, MAX_2);
    Serial.print(F("Channel 1 = "));
    Serial.print(OCR1A);
    Serial.print(" ");
    Serial.print(F("Channel 2 = "));
    Serial.println(OCR1B);
  }
}

uint16_t remap(long x, long in_min, long in_max, long out_min, long out_max)
{
  return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
}