//https://github.com/rogerclarkmelbourne/Arduino_STM32   in arduino/hardware
//Board: Generic STM32F103C series
//Upload method: serial
//20k RAM 64k Flash

// RX ist A10, TX ist A9   (3v3 level)
//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
//set boot0 back to 0 to run program on powerup

//#define DEBUG

#define PIN_LED PC13

#define PIN_POTI1 PA7
#define PIN_POTI2 PA6

#define SENDPERIOD 20


uint16_t poti1=0;
uint16_t poti2=0;


long last_send=0;

int16_t out_steer=0;
int16_t out_speed=0;


volatile long t_raising1=0;
volatile long t_falling1=0;
volatile long t_raising2=0;
volatile long t_falling2=0;
volatile long t_raising3=0;
volatile long t_falling3=0;
volatile long t_raising4=0;
volatile long t_falling4=0;

long last_updated_ch1=0;
long last_updated_ch2=0;
long last_updated_ch3=0;
long last_updated_ch4=0;


long ppmlagmillis=0; //updated by ppmOK()
boolean flag_ppmupdated=false; //true if at least one ppm value (chx_in) updated


#define MAXPPMUPDATETIME 50 //max time it should take to update a ppm channel value (otherwise ppmOK() will return false)
uint16_t ch1_in;
uint16_t ch2_in;
uint16_t ch3_in;
uint16_t ch4_in;
//ch1 = steer (ail)
  //ch2 = speed (ele)
  //ch3 = speed multiplier (thr)

#define PIN_CH1 PB9
#define PIN_CH2 PB8
#define PIN_CH3 PB7
#define PIN_CH4 PB6

#define PPM_TIME_MIN 900
#define PPM_TIME_MAX 2100


void setup() {

  Serial.begin(115200); //Debug and Program. A9=TX1,  A10=RX1  (3v3 level)

  Serial2.begin(19200); //control.  B10=TX3, B11=RX3 (Serial2 is Usart 3)

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

  pinMode(PIN_POTI1, INPUT);
  pinMode(PIN_POTI2, INPUT);


  pinMode(PIN_CH1, INPUT);
  attachInterrupt(PIN_CH1, ppmchanged1, CHANGE); //see http://docs.leaflabs.com/static.leaflabs.com/pub/leaflabs/maple-docs/0.0.12/lang/api/attachinterrupt.html
  pinMode(PIN_CH2, INPUT);
  attachInterrupt(PIN_CH2, ppmchanged2, CHANGE);
  pinMode(PIN_CH3, INPUT);
  attachInterrupt(PIN_CH3, ppmchanged3, CHANGE);
  pinMode(PIN_CH4, INPUT);
  attachInterrupt(PIN_CH4, ppmchanged4, CHANGE);

}

void loop() {

  updateChannels(); //calculate chx_in times from ppm signals. 0 <= chx_in <= 1000
  boolean _ppmOK=ppmOK();

  //Potis
  /*poti1=analogRead(PIN_POTI1);
  #define POTIMIN 100
  #define POTIMAX 4000
  if (poti1<POTIMIN){
    poti1=POTIMIN;
  }else if(poti1>POTIMAX){
    poti1=POTIMAX;
  }
  poti1=(poti1-POTIMIN)*1000/(POTIMAX-POTIMIN);

  poti2=analogRead(PIN_POTI2);
  if (poti2<POTIMIN){
    poti2=POTIMIN;
  }else if(poti2>POTIMAX){
    poti2=POTIMAX;
  }
  poti2=(poti2-POTIMIN)*1000/(POTIMAX-POTIMIN);
  
  out_speed=poti1;
  out_steer=0;
  */


  if (_ppmOK){ //ppm check failed   
    //out_speed=(int16_t)( (ch2_in*2-1000)*(ch3_in/1000.0) );
    out_speed=(int16_t)( (ch2_in*2-1000));
    out_steer=ch1_in*2-1000;
  }else{
    //out_speed=(int16_t)( (ch2_in*2-1000)*(ch3_in/1000.0) );
    out_speed=0; //stop
    out_steer=0; //stop
  }

  

  if (flag_ppmupdated){
    flag_ppmupdated=false; //clear flag
    digitalWrite(PIN_LED,!digitalRead(PIN_LED)); //toggle led
  }


  if (millis()-last_send>SENDPERIOD){
    Serial2.write((uint8_t *) &out_steer, sizeof(out_steer)); 
    Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
    last_send=millis();


    #ifdef DEBUG
    Serial.print("steer=");
    Serial.print(out_steer);
    Serial.print(" speed=");
    Serial.print(out_speed);
    /*
    Serial.print(ch1_in);
    Serial.print(",");
    Serial.print(ch2_in);
    Serial.print(",");
    Serial.print(ch3_in);
    Serial.print(",");
    Serial.print(ch4_in);
    */
    Serial.print(", ppmOK=");
    Serial.print(_ppmOK);
    Serial.print(", ppmlag=");
    Serial.print(ppmlagmillis);
    #endif

    
    Serial.println();
  }

}



void updateChannels(){
  long funcmillis=millis();
  //Calculate Pulse Width. (pulseIn values typically between 1000 and 2000). Map and constrain between 0 and 1000
  long new_ch1_in=(t_falling1-t_raising1);
  if (new_ch1_in>=PPM_TIME_MIN && new_ch1_in<=PPM_TIME_MAX){ //time valid and has changed
    ch1_in=constrain(map(new_ch1_in,1000,2000, 0,1000), 0,1000);
    last_updated_ch1=funcmillis;
    flag_ppmupdated=true;
  }
  
  long new_ch2_in=(t_falling2-t_raising2);
  if (new_ch2_in>=PPM_TIME_MIN && new_ch2_in<=PPM_TIME_MAX){ //time valid and has changed
    ch2_in=constrain(map(new_ch2_in,1000,2000, 0,1000), 0,1000);
    last_updated_ch2=funcmillis;
    flag_ppmupdated=true;
  }
  
  long new_ch3_in=(t_falling3-t_raising3);
  if (new_ch3_in>=PPM_TIME_MIN && new_ch3_in<=PPM_TIME_MAX){ //time valid and has changed
    ch3_in=constrain(map(new_ch3_in,1000,2000, 0,1000), 0,1000);
    last_updated_ch3=funcmillis;
    flag_ppmupdated=true;
  }
  
  long new_ch4_in=(t_falling4-t_raising4);
  if (new_ch4_in>=PPM_TIME_MIN && new_ch4_in<=PPM_TIME_MAX){ //time valid and has changed
    ch4_in=constrain(map(new_ch4_in,1000,2000, 0,1000), 0,1000);
    last_updated_ch4=funcmillis;
    flag_ppmupdated=true;
  }

}

boolean ppmOK(){
  long m=millis();
  boolean returnvalue=true;
  ppmlagmillis=0;
  long v;
  v=m-last_updated_ch1;
  if (v>ppmlagmillis){ ppmlagmillis=v; }
  if (v>MAXPPMUPDATETIME){
    returnvalue=false;
  }
  v=m-last_updated_ch2;
  if (v>ppmlagmillis){ ppmlagmillis=v; }
  if (v>MAXPPMUPDATETIME){
    returnvalue=false;
  }
  v=m-last_updated_ch3;
  if (v>ppmlagmillis){ ppmlagmillis=v; }
  if (v>MAXPPMUPDATETIME){
    returnvalue=false;
  }
  v=m-last_updated_ch4;
  if (v>ppmlagmillis){ ppmlagmillis=v; }
  if (v>MAXPPMUPDATETIME){
    returnvalue=false;
  }
  return returnvalue;
}

void ppmchanged1(){
  if (digitalRead(PIN_CH1)){
    t_raising1=micros();
  }else{
    t_falling1=micros();
  }
}

void ppmchanged2(){
  if (digitalRead(PIN_CH2)){
    t_raising2=micros();
  }else{
    t_falling2=micros();
  }
}

void ppmchanged3(){
  if (digitalRead(PIN_CH3)){
    t_raising3=micros();
  }else{
    t_falling3=micros();
  }
}

void ppmchanged4(){
  if (digitalRead(PIN_CH4)){
    t_raising4=micros();
  }else{
    t_falling4=micros();
  }
}