#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

/* Pin definitions */
#define JOYSTICK_1_X_PIN A3
#define JOYSTICK_1_Y_PIN A2
#define JOYSTICK_1_BUTTON_PIN 5

#define JOYSTICK_2_X_PIN A1
#define JOYSTICK_2_Y_PIN A0
#define JOYSTICK_2_BUTTON_PIN 6

#define JOYSTICK_THRESHOLD 50

#define MODE_TOGGLE_BUTTON_PIN 7
#define CALIBRATE_BUTTON_PIN A5

#define BLUE_LED_PIN 3
#define RED_LED_PIN 4
#define GREEN_LED_PIN A4

#define RADIO_CE_PIN 9
#define RADIO_CSN_PIN 10

/* Timeout between each calibration constant */
#define CALIBRATION_TIME 1000

/* How long to hold the "Mode" button down for automatic mode to be engaged */
#define ENGAGE_AUTO_MODE_DOWN_TIME 1000

/* Enums used for better readability of values / array indexes */
enum Side {
    LOWER,
    MIDDLE,
    UPPER
};
enum Mode {
    WATERMODE,
    FIREMODE,
    AUTOMODE
};
enum OnOff {
    OFF,
    ON
};

/*
    Class to handle the joysticks. If they were perfectly calibrated from the start, these would not have
    been neccesearly. However seeing how much trouble we had with our readings, and how the
    lower/middle/upper value seemed to differ, this was needed.
*/
class Joystick {
    private:
        int pin, threshold, lowerOut, upperOut, middleOut;
        int bounds[3]; //lower, middle and upper value
    public:
        Joystick(int p, int t) {
            pin = p;
            threshold = t;

            bounds[0] = 512;
            bounds[1] = 0;
            bounds[2] = 1023; //boundaries in ideal conditions (before calibration)
            lowerOut = 0; //standard out values, in this case 0, 511, 1023. No negative values due to bitshift
            upperOut = 1023;
            middleOut = (upperOut + lowerOut) / 2;

            pinMode(pin, INPUT);
        }
        int get() {
            int value = analogRead(pin); //value from the joystick
            int valueWithOffset = value - bounds[0]; //value "middle" as zero point
            if(abs(valueWithOffset) < threshold) return middleOut; //if the value is within the threshold (zero area)
            /* mapping functions to make sure each 'side' has 512 values regardless of the actual case */
            if(valueWithOffset < 0) value = map(value, bounds[1], bounds[0], lowerOut, middleOut);
            else if(valueWithOffset > 0) value = map(value, bounds[0], bounds[2], middleOut, upperOut);
            else value = middleOut;

            /* constrain the value within the bounds */
            return constrain(value, lowerOut, upperOut);
        }
        void calibrate(int s) {
            /* calibrate a given side (LOWER, MIDDLE, UPPER). Setting the bounds. */
            bounds[s] = analogRead(pin);
        }
};

/*
    Toggle class to handle a standard toggle. can toggle between the numbers of states set
    and be set to a state of choosing
*/

class Toggle {
    protected:
        byte state; //current state
        byte numStates; //number of states available
        bool togglable; //if the instance can be toggled
    public:
        Toggle(byte s) {
            numStates = s;
            state = 0;
            togglable = 1;
        }
        /* method to set the state. virutal so that child classes can overwrite  */
        virtual void setState(byte s) {
            state = s;
        }
        void toggle() {
            if(!togglable) return; //toggle only if its allowed
            state++; //inclease state
            state = state % numStates; //if state surpasses the alloved number of states, start over
            togglable = 0; //do not allow to toggle again before 'reset' is called
        }
        void reset() {
            togglable = 1; //allow toggle
        }
        byte getState() {
            return state; //get current state
        }
};

/*
    LedToggle extends Toggle to add a led feature. Each state has a led on a given pin number and this led
    is set to high when that state is active
*/
class LedToggle : public Toggle {
    private:
        //static array for simplicity. maximum of ten leds available
        int leds[10];
    public:
        LedToggle(byte pins[], byte states) : Toggle(states) {
            //set correct pinMode to each led
            for(int i = 0; i < states; i++) {
                leds[i] = pins[i];
                pinMode(leds[i], OUTPUT);
            }
        }
        /*
            overwrite parentclass setState. this does the same, but also changes sets the led of the
            current state to high and the others to low
        */
        virtual void setState(byte s) {
            state = s;
            for(int i = 0; i < numStates; i++) {
                if(s == i) digitalWrite(leds[i], 1);
                else digitalWrite(leds[i], 0);
            }
        }
};

/* Function to encode an array to a 64 bit bitsequence */
uint64_t intarrToUint64(int data[], int keep[], int dataSize) {
    uint64_t compression = 0; //initialize variable
    for(int i = 0; i < dataSize; i++) {
        /*
            For each loop left shift the variable by x bits assigned in 'keep'. Then read in and
            store the data in that area of the variable.
        */
        compression = (compression << keep[i]) | data[i];
    }
    return compression;
}
/* Function to encode a 64 bit variable into a byte array */
void uint64ToByteArr(uint64_t data, byte arr[], int byteSize) {
    //loop over the number of bytes wanted
    for(int i = 0; i < byteSize; i++) {
        arr[byteSize - i - 1] = data & 0xFF; //read in the least significant byte
        data >>= 8; //right shift the data with a byte
    }
}

const uint64_t pipe = 0xF0F0F0F0AALL; //radio pipe for communication

RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN);

/* Initializing the joysticks on their respective pins */
Joystick j1x(JOYSTICK_1_X_PIN, JOYSTICK_THRESHOLD);
Joystick j1y(JOYSTICK_1_Y_PIN, JOYSTICK_THRESHOLD);
Joystick j2x(JOYSTICK_2_X_PIN, JOYSTICK_THRESHOLD);
Joystick j2y(JOYSTICK_2_Y_PIN, JOYSTICK_THRESHOLD);

/* Initialize the two toggles, one with leds and 3 states, one with 2 states (on/off) */
LedToggle modeToggle( (byte[]){BLUE_LED_PIN, RED_LED_PIN, GREEN_LED_PIN}, 3);
Toggle guideFlameToggle(2);

int data[8]; //temporary array of integers as theyre read in
int bitsToKeep[] = {10, 10, 1, 10, 10, 1, 2, 1}; //how many bits of each int needed
long timeDown; //how long a button is pressed down

void setup() {
    /* Initializing variable */
    Serial.begin(9600);
    radio.begin();
    radio.setPayloadSize(6);
    radio.openWritingPipe(pipe);

    /* Setting startup modes */
    modeToggle.setState(WATERMODE);
    guideFlameToggle.setState(OFF);
}

void loop() {

    /* Toggle functions to switch between the three modes, and turnining the pilot flame on and off */

    //if the mode button is held down
    if(digitalRead(MODE_TOGGLE_BUTTON_PIN)) {
        if(timeDown == 0) timeDown = millis(); //start timer for how long it has been down
        //if the button has been held down for the given ammount of time, engage automatic mode
        else if(millis() - timeDown > ENGAGE_AUTO_MODE_DOWN_TIME) modeToggle.setState(AUTOMODE);
    } else if(timeDown != 0) {
        //if the button was recently released
        if(millis() - timeDown < ENGAGE_AUTO_MODE_DOWN_TIME) { //if automatic mode has not been started
            if(modeToggle.getState() == WATERMODE) modeToggle.setState(FIREMODE); //switch to firemode
            else {
                //switch to watermode and ensure the pilot flame is turned off
                guideFlameToggle.setState(0);
                modeToggle.setState(WATERMODE);
            }
        }
        //reset the timer
        timeDown = 0;
    }
    else modeToggle.reset(); //reset the mode toggle so it may be toggled again

    //turn on pilot flame if fire mode is engaged and the joystick is pressed down. reset elsewise
    if(!digitalRead(JOYSTICK_1_BUTTON_PIN) && modeToggle.getState() == FIREMODE) guideFlameToggle.toggle();
    else guideFlameToggle.reset();

    //temporary storage for all data to be sent
    data[0] = j1x.get();
    data[1] = j1y.get();
    data[2] = guideFlameToggle.getState();
    data[3] = j2x.get();
    data[4] = j2y.get();
    data[5] = !digitalRead(JOYSTICK_2_BUTTON_PIN);
    data[6] = modeToggle.getState();
    data[7] = 1; //last bit always 1 to ensure no all 0 data packets are sent, as the radio likes to ignore them

    //temporary storage in a 64 bit variable
    uint64_t dataToSend = intarrToUint64(data, bitsToKeep, 8);

    //encode the 64 bit variable to an array of 6 bytes
    byte arr[6];
    uint64ToByteArr(dataToSend, arr, 6);

    /* total compression is now from 32 byte to 6 byte */

    radio.write(arr, 6); //write it to the radio pipe


    // if calibration is wanted
    if(digitalRead(CALIBRATE_BUTTON_PIN)) {
        //save the current mode
        int modeBeforeCalibration = modeToggle.getState();

        //delay
        delay(CALIBRATION_TIME);
        /*
            calibrate all sides in first x then y direction. the first calibration (middle) is set to both x and y
        */
        for(int i = 0; i < 3; i++) {
            digitalWrite(BLUE_LED_PIN, 1);
            digitalWrite(GREEN_LED_PIN, 1);
            delay(CALIBRATION_TIME);
            j1x.calibrate(i);
            j2x.calibrate(i);
            if(i == 0) {
                j1y.calibrate(i);
                j2y.calibrate(i);
            }
            digitalWrite(BLUE_LED_PIN, 0);
            digitalWrite(GREEN_LED_PIN, 0);
            delay(CALIBRATION_TIME);
        }
        for(int i = 1; i < 3; i++) {
            digitalWrite(BLUE_LED_PIN, 1);
            digitalWrite(GREEN_LED_PIN, 1);
            delay(CALIBRATION_TIME);
            j1y.calibrate(i);
            j2y.calibrate(i);
            digitalWrite(BLUE_LED_PIN, 0);
            digitalWrite(GREEN_LED_PIN, 0);
            if(i != 2) delay(CALIBRATION_TIME);
        }
        // revert to the state before calibration
        modeToggle.setState(modeBeforeCalibration);
    }
}