#include <util/delay.h>
#include <avr/io.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 3
#define CS_PIN 2
#define RED 4
#define GREEN 5

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 R_TRK_FWD 18
#define L_TRK_FWD 17
#define R_TRK_BKW 19
#define L_TRK_BKW 16
#define BKT_FWD   15
#define BKT_BKW   14


int main() {
    init();
    initVariant();
    SPI.begin();
    Serial.begin(9600);

    pinMode(GREEN, OUTPUT);
    pinMode(RED, OUTPUT);

    SoftPWMBegin();

    SoftPWMSet(R_TRK_FWD, 0); // bucket
    SoftPWMSet(L_TRK_FWD, 0); // bucket
    SoftPWMSet(R_TRK_BKW, 0); // left track backward
    SoftPWMSet(L_TRK_BKW, 0); // left track forward
    SoftPWMSet(BKT_FWD  , 0); // right track backward
    SoftPWMSet(BKT_BKW  , 0); // right track forward

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

    bool breaking = false;
    int cycles = 0;

    while (!breaking) {

        bool timeout = false;

        while (!radio.available()) {
            delay(5);
            if (cycles++ > 10) {
              //  digitalWrite(RED, HIGH);
                _delay_ms(1);
               // digitalWrite(RED, LOW);
                _delay_ms(1);
                timeout = true;
                cycles = 0;
                break;
            }
        }

        if (timeout) {
            SoftPWMSet(14, 0); // bucket
            SoftPWMSet(15, 0); // bucket
            SoftPWMSet(16, 0); // left track backward
            SoftPWMSet(17, 0); // left track forward
            SoftPWMSet(19, 0); // right track backward
            SoftPWMSet(18, 0); // right track forward
        } else {
            cycles = 0;
            StickValues stick_values{};
            if (radio.read(&stick_values, sizeof(int16_t)*4)) {
//                digitalWrite(GREEN, HIGH);
//                _delay_ms(5);
//                digitalWrite(GREEN, LOW);
//                _delay_ms(5);
                //Serial.println((int16_t)stick_values.stick_1_y);
            }

            radio.flush_rx();

            if (stick_values.stick_2_y > 10) {
                digitalWrite(GREEN, HIGH);
                digitalWrite(RED, LOW);
            } else if (stick_values.stick_2_y < -10) {
                digitalWrite(RED, HIGH);
                digitalWrite(GREEN, LOW);
            } else {
                digitalWrite(RED, LOW);
                digitalWrite(GREEN, LOW);
            }


            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);
            }

            if (stick_values.stick_2_y > 5) {
                SoftPWMSet(BKT_FWD, 255);
                SoftPWMSet(BKT_BKW, 0);
                //SoftPWMSet(BKT_FWD, max(0, min(255, abs(stick_values.stick_2_y))));
                //SoftPWMSet(R_TRK_BKW, max(0, min(255, abs(stick_values.stick_2_y))));
                // SoftPWMSet(BKT_BKW, 0);
            } else if (stick_values.stick_2_y < -5) {
                SoftPWMSet(BKT_BKW, 255);
                SoftPWMSet(BKT_FWD, 0);
                //SoftPWMSet(BKT_BKW, max(0, min(255, abs(stick_values.stick_2_y))));
                //SoftPWMSet(R_TRK_FWD, max(0, min(255, abs(stick_values.stick_2_y))));
                //SoftPWMSet(BKT_, 1);
            } else {
                SoftPWMSet(BKT_BKW, 0);
                SoftPWMSet(BKT_FWD, 0);
            }

            left_track = left_track * 3;
            right_track = right_track * 3;

            if (left_track > 0) {
                SoftPWMSet(L_TRK_FWD, max(0, min(255, left_track)));
                SoftPWMSet(L_TRK_BKW, 0);
            } else if (left_track < 0) {
                SoftPWMSet(L_TRK_BKW, max(0, min(255, abs(left_track))));
                SoftPWMSet(L_TRK_FWD, 0);
            } else if (left_track == 0) {
                SoftPWMSet(L_TRK_BKW, 0);
                SoftPWMSet(L_TRK_FWD, 0);
            }

            if (right_track > 0) {
                SoftPWMSet(R_TRK_FWD, max(0, min(255, right_track)));
                SoftPWMSet(R_TRK_BKW, 0);
            } else if (right_track < 0) {
                SoftPWMSet(R_TRK_BKW, max(0, min(255, abs(right_track))));
                SoftPWMSet(R_TRK_FWD, 0);
            } else if (right_track == 0) {
                SoftPWMSet(R_TRK_FWD, 0);
                SoftPWMSet(R_TRK_BKW, 0);
            }




            if (serialEventRun) serialEventRun();

        }
    }
}










//SoftPWMSet(14, 255); // bucket
//SoftPWMSet(15, 10); // bucket
//SoftPWMSet(16, 255); // left track backward
//SoftPWMSet(17, 255); // left track forward
//SoftPWMSet(19, 255); // right track backward
//SoftPWMSet(18, 255); // right track forward

//            SoftPWMSet(17, stick_values.stick_1_y);
//            SoftPWMSet(18, stick_values.stick_1_x);
//            SoftPWMSet(19, stick_values.stick_2_y);
//            SoftPWMSet(20, stick_values.stick_2_x);
//            SoftPWMSet(21, stick_values.stick_3_a);
//            SoftPWMSet(22, stick_values.stick_3_b);