#include <avr/io.h>
#include <util/delay.h>
#include <stdio.h>
#include <stdarg.h>
#include <util/setbaud.h>
#include <Arduino.h>
#include <lib.h>
#include "SoftPWM.h"

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

#define CE_PIN 7
#define CS_PIN 8

class Radio {

public:
    RF24 instance;

};

class Motor {

public:
    int power;
};

const uint64_t dozer_pipe = 0xF0F0F0F0D2LL;
const uint64_t remote_pipe = 0xF0F0F0F0E1LL;

#pragma pack(8)
struct StickValues {
    int16_t stick_1_y;
    int16_t stick_1_x;
    int16_t stick_2_y;
    int16_t stick_2_x;
};

#define	PI		    3.141592654
#define	DEG(rad)	(rad*180.0/PI)

int main() {

    init();
    initVariant();
    SPI.begin();
    Serial.begin(9600);

    RF24 radio(CE_PIN, CS_PIN);
    radio.begin();
    radio.setChannel(100);
    radio.setPALevel (RF24_PA_MAX);
    radio.setDataRate(RF24_1MBPS);
    radio.openWritingPipe(remote_pipe);
    radio.openReadingPipe(1, dozer_pipe);
    radio.startListening();

    pinMode(9, INPUT_PULLUP);
    delay(3000);

    int16_t ref_stick_1_y = analogRead(A4); // RY
    int16_t ref_stick_1_x = analogRead(A5); // RX
    int16_t ref_stick_2_y = analogRead(A6); // LY
    int16_t ref_stick_2_x = analogRead(A7); // LX

    while (true) {

        int val = digitalRead(9);  // read input value
        Serial.println(val);
        if (val == 0) {         // check if the input is HIGH (button released)
            ref_stick_1_y = analogRead(A4); // RY
            ref_stick_1_x = analogRead(A5); // RX
            ref_stick_2_y = analogRead(A6); // LY
            ref_stick_2_x = analogRead(A7); // LX
        } else {

        }

        // Parse into a struct
        StickValues stick_values;
        stick_values.stick_1_y = analogRead(A5); // RY
        stick_values.stick_1_x = analogRead(A4); // RX
        stick_values.stick_2_y = analogRead(A6); // LY
        stick_values.stick_2_x = analogRead(A7); // LX

        int16_t delta_1_y = stick_values.stick_1_y - ref_stick_1_y;
        int16_t delta_1_x = ref_stick_1_x - stick_values.stick_1_x;
        int16_t delta_2_y = stick_values.stick_2_y - ref_stick_2_y;
        int16_t delta_2_x = stick_values.stick_2_x - ref_stick_2_x;

        stick_values.stick_1_y = delta_1_y / 2;
        stick_values.stick_1_x = delta_1_x / 2;
        stick_values.stick_2_y = delta_2_y / 2;
        stick_values.stick_2_x = delta_2_x / 2;

        // Send to dozer
        radio.stopListening();
        delay(10);
        if (radio.write(&stick_values, sizeof(int16_t)*4))
        {
            float mag = sqrt(pow(stick_values.stick_1_x, 2) + pow(stick_values.stick_1_y, 2));
            float ang = DEG(atan2((float)stick_values.stick_1_x, (float)stick_values.stick_1_y)) + 180;

            int right_track = 0;
            int left_track = 0;

            if (ang >= 0 && ang < 90 && mag > 1) { // Bottom to Left Quad. RT:F
                if (ang < 0 + FULL_SPEED_ZONE)
                    right_track = (int)(-100.0 * (mag/MAG_SCALER_H));
                else if (ang > 90 - FULL_SPEED_ZONE)
                    right_track = (int)(100.0 * (mag/MAG_SCALER_H));
                else // split it (and mult) so it's ranged between -100, 100 then mag it
                    right_track = ((0 + 45) - ang) * MAGIC_45_2_100 * (mag/MAG_SCALER_H);

                left_track = (int)(-100.0 * (mag/MAG_SCALER_H));
            }
            if (ang >= 90 && ang < 180 && mag > 1) { // Top to Left Quad. RT:F, LT:S
                right_track = (int)(100.0 * (mag/MAG_SCALER_H));

                if (ang < 90 + FULL_SPEED_ZONE)
                    left_track = (int)(-100.0 * (mag/MAG_SCALER_H));
                else if (ang > 180 - FULL_SPEED_ZONE)
                    left_track = (int)(100.0 * (mag/MAG_SCALER_H));
                else
                    left_track = ((90 + 45) - ang) * MAGIC_45_2_100 * (mag/MAG_SCALER_H);
            }
            if (ang >= 180 && ang < 270 && mag > 1) { // Top to Right Quad
                if (ang < 180 + FULL_SPEED_ZONE)
                    right_track = (int)(100.0 * (mag/MAG_SCALER_H));
                else if (ang > 270 - FULL_SPEED_ZONE)
                    right_track = (int)(-100.0 * (mag/MAG_SCALER_H));
                else
                    right_track = ((180 + 45) - ang) * MAGIC_45_2_100 * (mag/MAG_SCALER_H);

                left_track = (int)(100.0 * (mag/MAG_SCALER_H));
            }
            if (ang >= 270 && ang <= 360 && mag > 1) { // Bottom to Right Quad
                right_track = (int)(100.0 * (mag/MAG_SCALER_H)) * -1;

                if (ang < 270 + FULL_SPEED_ZONE)
                    left_track = (int)(100.0 * (mag/MAG_SCALER_H));
                else if (ang > 360 - FULL_SPEED_ZONE)
                    left_track = (int)(100.0 * (mag/MAG_SCALER_H)) * -1;
                else
                    left_track = ((270 + 45) - ang) * MAGIC_45_2_100 * (mag/MAG_SCALER_H);
            }


            //Serial.println(mag);
            Serial.print(ang);
            Serial.print("  :  ");
            Serial.print(mag);
            Serial.print("  :  [");
            Serial.print(left_track);
            Serial.print(", ");
            Serial.print(right_track);
            Serial.print(", ");
            Serial.print(stick_values.stick_2_y);
            Serial.println("]");

            //Serial.println("");
        }
        radio.startListening();
        if (serialEventRun) serialEventRun();
    }
}

// Dot really ever have to read
//if (radio.available()) {
//    while (radio.available()) {
//        radio.read(&got_time, sizeof(unsigned long));
//    }
//}