//from left to right. pins at bottom. chips on top
//1 GND (black)
//2 Data
//3 Clock
//4 Reset
//5 +5V (red)
//6 Right BTN
//7 Middle BTN
//8 Left BTN

//Arduino Pro Mini 328P 5V 16MHz
//hold power button pressed during flashing


//pinout: https://martin-prochnow.de/projects/thinkpad_keyboard

//see also https://github.com/feklee/usb-trackpoint/blob/master/code/code.ino

//#define DEBUG

#include "Trackpoint.h"

//Default:
/*Trackpoint trackpoint(8, // CLK
                      9, // DATA
                      12); // RESET
*/

/*funktioniert
 * Trackpoint trackpoint(2, // CLK (rosa)
                      3, // DATA (gelb)
                      4); // RESET (gruen)
*/
Trackpoint trackpoint(3, // CLK (rosa, TP3)
                      4, // DATA (gelb, TP2)
                      2); // RESET (gruen, TP4)

#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"
bool radioOk=false; //true, if sending was successfull. can be false, even if data was send and received

RF24 radio(9,10); //ce, cs
//SCK D13
//Miso D12
//Mosi D11
// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };  //0xF0F0F0F0xxLL, xx is 1-byte address
#define NRF24CHANNEL 75

struct nrfdata {
  uint8_t steer; //between 0 and 255,  127 is stop. will be scaled to -1000 to 1000
  uint8_t speed; //between 0 and 255,  127 is stop. will be scaled to -1000 to 1000
  uint8_t commands; //bit 0 set = motor enable
  uint8_t checksum;
};

long last_sendNRF=0;
#define NRFSEND_DELAY 20 //ms

#define PIN_TOUCH 5
long last_touch=0;
#define TOUCH_TIMEOUT 100

//command variables
boolean motorenabled=false;

#define PIN_LED A1

#define PIN_BUTTON 6

#define PIN_POWERON 7

#define TRACKPOINT_MAX 70 //value for maximum stick movement
float speedscale=0.0;
float steerscale=0.0;

int16_t last_xin=0;
int16_t last_yin=0;
int16_t xin_smooth=0;
int16_t yin_smooth=0;
int16_t maxacc=0;
int16_t maxacc_brake=0;
int16_t maxaccsteer=0;
int16_t maxaccsteer_brake=0;

#define SETUP_NONE 0
#define SETUP_WAIT 1 //waiting for input
#define SETUP_DONE 2 //waiting after input (do not move motors)
uint8_t setupmode=SETUP_NONE;
long setupmode_waitstarttime=0; //starttime of SETUP_WAIT mode
#define SETUP_WAIT_TIMEOUT 10000 //maximum time to wait for input before canceling
#define SETUP_HOLD_POWEROFF 2000 //if button held down after x ms in setup mode, power off
#define SETUP_DONE_TIME 1000 //time to keep motors disabled after exiting setup
#define SETUP_MOVE_THRESHOLD 275 //500*TRACKPOINT_MAX/127
uint8_t speedmode=1; //0 (slow), 1(medium), 2(fast)
#define SETUP_SPEEDMODE_MAX 2

uint16_t led_ton=0; //never. time in ms for on time
uint16_t led_toff=65535; //always
long led_nextswitch=0;

#define TIME_INACTIVITY_POWEROFF 120000
long time_lastactivity=0;
#define ACTIVITYMOVEMENT 5//Stick movement for activity recognition

boolean touching=false;

int voltage=4000;
#define VOLTAGE_WARN 3400
/*
 * 3681=3.725V
*/

void setup() {
  //Mouse.begin();
  
  pinMode(PIN_TOUCH, INPUT_PULLUP);
  pinMode(PIN_LED, OUTPUT);
  pinMode(PIN_BUTTON, INPUT_PULLUP);
  pinMode(PIN_POWERON, OUTPUT);
  digitalWrite(PIN_LED, LOW);

  digitalWrite(PIN_POWERON, HIGH); //keep unit powered on


  Serial.begin(115200);
  printf_begin();
  Serial.println("Booting");
  
  
  
  radio.begin();
  //Serial.print("CRC Length=");
  //Serial.println(radio.getCRCLength());

  radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS
  //radio.setDataRate( RF24_1MBPS ); 

  radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive)
  
  radio.setRetries(15,15); // optionally, increase the delay between retries & # of retries
  radio.setPayloadSize(8); // optionally, reduce the payload size.  seems to improve reliability
  
  //radio.openWritingPipe(pipes[0]); //write on pipe 0
  //radio.openReadingPipe(1,pipes[1]); //read on pipe 1

  radio.openWritingPipe(pipes[1]); //write on pipe 1
  radio.openReadingPipe(1,pipes[0]); //read on pipe 0

  radio.printDetails();

  radio.startListening();

  #ifdef DEBUG
  Serial.println("Radio initialized");
  #endif

  trackpoint.reset();
  trackpoint.setRemoteMode();
  trackpoint.setSensitivityFactor(0xc0); // more sensitive than by default

  #ifdef DEBUG
  Serial.println("Trackpoint initialized");
  #endif

  voltage=readVcc();
  Serial.print("Voltage=");
  Serial.println( voltage, DEC ); 

  setup_updateSpeedmode(); //set speeds
  
}


/*
void sendButtonState(byte state) {
  static const char hidStates[] = {MOUSE_LEFT, MOUSE_RIGHT};

  for (byte i = 0; i < sizeof(hidStates); i++) {
    byte hidState = hidStates[i];
    if (state & (1 << i)) {
      Mouse.press(hidState);
    } else if (Mouse.isPressed(hidState)) {
      Mouse.release(hidState);
    }
  }
}
*/

// Reads TrackPoint data and sends data to computer.
void loop() {
  

  if (millis()-last_sendNRF >= NRFSEND_DELAY)
  {
    voltage=readVcc(); //read own voltage
    
    last_sendNRF=millis();
    trackpoint.readData(); //discard last value. otherwise values are scales way too high
    Trackpoint::DataReport d = trackpoint.readData(); //d.x and d.y between 128 to 255=-1000 to 0, and 0 to 127=0 to +1000
  
    #ifdef DEBUG
    Serial.print("DataReport: ");
    Serial.print(d.x);
    Serial.print(", ");
    Serial.println(d.y);
    #endif
    
  
    nrfdata senddata;
    
    //senddata.steer=map(constrain((uint8_t)(d.x+127),127-TRACKPOINT_MAX,127+TRACKPOINT_MAX) , 127-TRACKPOINT_MAX,127+TRACKPOINT_MAX, 127+(127*steerscale), 127-(127*steerscale) ); //steer
    //senddata.speed=map(constrain((uint8_t)(d.y+127),127-TRACKPOINT_MAX,127+TRACKPOINT_MAX) , 127-TRACKPOINT_MAX,127+TRACKPOINT_MAX, 127-(127*speedscale), 127+(127*speedscale) ); //speed


    //map x and y to -1000 to 1000
    int16_t xin;
    if (d.x>=0 && d.x<=127){ //positive range
      xin=map(constrain((int16_t)d.x,0,TRACKPOINT_MAX) , 0,TRACKPOINT_MAX, 0, 1000 );
    }else{ //negative range  128(=-1000) to 255(0)
      xin=map(constrain((int16_t)d.x,127+TRACKPOINT_MAX,255) , 127+TRACKPOINT_MAX,255, -1000, 0 );
    }
    int16_t yin;
    if (d.y>=0 && d.y<=127){ //positive range
      yin=map(constrain((float)d.y,0,TRACKPOINT_MAX) , 0,TRACKPOINT_MAX, 0, 1000 );
    }else{ //negative range  128(=-1000) to 255(0)
      yin=map(constrain((float)d.y,127+TRACKPOINT_MAX,255) , 127+TRACKPOINT_MAX,255, -1000, 0 );
    }

    last_xin=xin; //save position values for other stuff than control
    last_yin=yin;

    if (abs(xin)>ACTIVITYMOVEMENT){
      time_lastactivity=millis(); //reset activity timeout
    }else if (abs(yin)>ACTIVITYMOVEMENT){
      time_lastactivity=millis(); //reset activity timeout
    }

    /*
    float r=sqrt((xin*xin) + (yin*yin));
    float phi=atan2(yin,xin); // arc tangent of y/x. 0 is right

    Serial.print(xin,0);
    Serial.print(", ");
    Serial.print(yin,0);
    Serial.print(":  ");
    Serial.print(r,0);
    Serial.print(", ");
    Serial.println(phi,4);*/

    
    //xin_smooth=smoothfilter*xin_smooth + (1-smoothfilter)*xin;
    //yin_smooth=smoothfilter*yin_smooth + (1-smoothfilter)*yin;
    if (maxaccsteer>0){
      // ### X ###
      int16_t _xaccel=xin_smooth-xin;
      if ((xin_smooth>0 && xin<-2) || (xin_smooth<0 && xin>2) ){ //if actively braking
        if (_xaccel<-maxaccsteer_brake){ //limit deceleration
          _xaccel=-maxaccsteer_brake;
        }else if (_xaccel>maxaccsteer_brake){
          _xaccel=maxaccsteer_brake;
        }
      }else{ //not braking
        if (_xaccel<-maxaccsteer){ //limit acceleration
          _xaccel=-maxaccsteer;
        }else if (_xaccel>maxaccsteer){
          _xaccel=maxaccsteer;
        }
      }
      xin_smooth-=_xaccel; //update value

    }else{ //no acc limit
      xin_smooth=xin; //update immediately
    }
    if (maxacc>0){
      // ### Y ###
      int16_t _yaccel=yin_smooth-yin;
      if ((yin_smooth>0 && yin<-2) || (yin_smooth<0 && yin>2) ){ //if actively braking
        if (_yaccel<-maxacc_brake){ //limit deceleration
          _yaccel=-maxacc_brake;
        }else if (_yaccel>maxacc_brake){
          _yaccel=maxacc_brake;
        }
      }else{ //not braking
        if (_yaccel<-maxacc){ //limit acceleration
          _yaccel=-maxacc;
        }else if (_yaccel>maxacc){
          _yaccel=maxacc;
        }
      }
      yin_smooth-=_yaccel; //update value
    }else{ //no acc limit
      yin_smooth=yin;
    }

    senddata.steer=map(xin_smooth, -1000,1000, 127+(128*steerscale), 127-(127*steerscale) ); //steer
    senddata.speed=map(yin_smooth, -1000,1000, 127-(127*speedscale), 127+(128*speedscale) ); //speed
    
    senddata.commands=0; //reset
    
    if (!radioOk || setupmode!=SETUP_NONE){ //if last transmission failed or in setup mode
      //senddata.steer=127; //stop
      //senddata.speed=127;
      senddata.commands|= 0 << 0; //motorenabled send false
      xin_smooth=0; //reset smooth value
      yin_smooth=0;
    }else{
      senddata.commands|= motorenabled << 0; //motorenabled is bit 0
    }
    #ifdef DEBUG
    Serial.print(senddata.steer);
    Serial.print(", ");
    Serial.println(senddata.speed);
    #endif
    

    senddata.checksum=(uint8_t)((senddata.steer+3)*(senddata.speed+13));
    sendRF(senddata);
    
    #ifdef DEBUG
    Serial.println( readVcc(), DEC ); 
    #endif
  }
  

  if(!digitalRead(PIN_TOUCH)){ //check touch
    last_touch=millis();
  }

  
  if(millis()-last_touch <= TOUCH_TIMEOUT){ //is touched
    if (!touching && setupmode!=SETUP_DONE) { //was false, is touching again (and not during setup_done wait)
      Serial.println("touching was false");
      if (last_xin==0 && last_yin==0) { //stick at center position
        touching=true; //enable only if stick is at center again
        motorenabled=true;
        Serial.println("touching reactivated");
      }
    }else{
      motorenabled=true;
    }
  }else{
    touching=false;
    motorenabled=false;
  }

  if (!digitalRead(PIN_BUTTON) && setupmode==SETUP_NONE){ //Button pressed, and not in setup mode
    setupmode=SETUP_WAIT;
    setupmode_waitstarttime=millis();
    Serial.println("Entering Setup");
    //digitalWrite(PIN_POWERON, LOW); //Power off
  }

  //inactivity poweroff
  if (millis()>time_lastactivity+TIME_INACTIVITY_POWEROFF){
    Serial.println("Inactivity Poweroff");
    delay(100);
    digitalWrite(PIN_POWERON, LOW); //Power off
  }

  switch(setupmode){
    case SETUP_WAIT:
      if (millis()>setupmode_waitstarttime+SETUP_WAIT_TIMEOUT){ //waittime over
        setupmode=SETUP_NONE; //exit setup
      }else if(last_yin > SETUP_MOVE_THRESHOLD){ //y moved up
        Serial.print("Moved Up");
        if (speedmode<SETUP_SPEEDMODE_MAX){ //if not already at maximum
          speedmode+=1;
        }
        setup_updateSpeedmode();
        setupmode=SETUP_DONE; //exit setupmode
        setupmode_waitstarttime=millis(); //use this value for done timer
      }else if(last_yin < -SETUP_MOVE_THRESHOLD){ //y moved down
        Serial.print("Moved Down");
        if (speedmode>0){ //if not already at minimum
          speedmode-=1;
        }
        setup_updateSpeedmode();
        setupmode=SETUP_DONE; //exit setupmode
        setupmode_waitstarttime=millis();//use this value for done timer
      }else if(last_xin < -SETUP_MOVE_THRESHOLD){ //y moved left
        Serial.print("Moved Left");
        maxacc=20; //the higher the snappier
        maxacc_brake=30;
        maxaccsteer=30;
        maxaccsteer_brake=70;
        setupmode=SETUP_DONE; //exit setupmode
        setupmode_waitstarttime=millis();//use this value for done timer
      }else if(last_xin > SETUP_MOVE_THRESHOLD){ //y moved right
        Serial.print("Moved Right");
        maxacc=0;
        maxacc_brake=0;
        maxaccsteer=0;
        maxaccsteer_brake=0;
        setupmode=SETUP_DONE; //exit setupmode
        setupmode_waitstarttime=millis();//use this value for done timer
      }else if (millis()>setupmode_waitstarttime+SETUP_HOLD_POWEROFF && !digitalRead(PIN_BUTTON)){  //if button held down after SETUP_HOLD_POWEROFF
        Serial.println("Manual Power Off");
        delay(100);
        digitalWrite(PIN_POWERON, LOW); //Power off
      }

      
      
      if (!touching){ //remote got put away (not touch)
        Serial.println("Poweroff");
        delay(100);
        digitalWrite(PIN_POWERON, LOW); //Power off
      }
    break;
    case SETUP_DONE:
      touching=false;
      motorenabled=false;
      Serial.println("touching set false");
      if (millis()>setupmode_waitstarttime+SETUP_DONE_TIME){
        setupmode=SETUP_NONE; //return to control mode, allows enabling motors
      }
    break;
  }


  //LED Blink Codes
  switch(setupmode){
    case SETUP_NONE:
      if (radioOk){
        if (touching){ //=touching
          led_ton=500; //always on
          led_toff=0;
        }else{
          led_ton=1000; //blink slowly regulary
          led_toff=1000;
        }
      }else{ //not connected
        if (touching){ //=touching
          led_ton=5; //short flash
          led_toff=200;
        }else{
          led_ton=0; //off
          led_toff=500;
        }
      }
      if (voltage<=VOLTAGE_WARN){
        led_ton=25; //flash on fast
        led_toff=75;
      }
      
    break;
    case SETUP_WAIT:
      led_ton=200; //blink fast
      led_toff=200;
    break;
    case SETUP_DONE:
      led_ton=20; //blink fast
      led_toff=20;
    break;

  }
  if (millis()>=led_nextswitch){ //Set LED State by timings
    if (digitalRead(PIN_LED)){ //led was on
      if (led_toff>0){
        digitalWrite(PIN_LED, LOW);  //led off
      }
      led_nextswitch=millis()+led_toff;
    }else{
      if (led_ton>0){
        digitalWrite(PIN_LED, HIGH);  //led on
      }
      led_nextswitch=millis()+led_ton;
    }
  }
  

  

  
}


void sendRF(nrfdata senddata){
  #ifdef DEBUG
  Serial.println("Transmitting...");
  #endif
  
  radio.stopListening(); //stop listening to be able to transmit
  radioOk = radio.write( &senddata, sizeof(nrfdata) );
  if (radioOk){
      #ifdef DEBUG
      Serial.println("ok");
      #endif
        
  }else{
      #ifdef DEBUG
      Serial.println("failed");
      #endif
  }
  radio.startListening();
  
}

long readVcc() { 
  long result; // Read 1.1V reference against AVcc 
  ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1); 
  delay(2); // Wait for Vref to settle 
  ADCSRA |= _BV(ADSC); // Convert 
  while (bit_is_set(ADCSRA,ADSC)); 
  result = ADCL; 
  result |= ADCH<<8; 
  result = 1126400L / result; // Back-calculate AVcc in mV 
  return result; 
}

void setup_updateSpeedmode(){
  switch(speedmode){
    case 0: //slow
      speedscale=0.15;
      steerscale=0.2;
    break;
    case 1: //medium
      speedscale=0.4;
      steerscale=0.45;
    break;
    case 2: //fast
      speedscale=1.0;
      steerscale=0.8;
    break;
    default:
      speedscale=0.1;
      steerscale=0.1;
    break;
  }
}