#include <string>
#include <cstdio>
#include <iostream>

using namespace std;

#include "mbed.h"
#include "SHA256.h"


//Photointerrupter input pins
#define I1pin D3
#define I2pin D6
#define I3pin D5

//Incremental encoder input pins
#define CHApin   D12
#define CHBpin   D11

//Motor Drive output pins   //Mask in output byte
#define L1Lpin D1           //0x01
#define L1Hpin A3           //0x02
#define L2Lpin D0           //0x04
#define L2Hpin A6          //0x08
#define L3Lpin D10           //0x10
#define L3Hpin D2          //0x20

#define PWMpin D9

//Motor current sense
#define MCSPpin   A1
#define MCSNpin   A0

//Test outputs
#define TP0pin D4
#define TP1pin D13
#define TP2pin A2

#define PWM_PERIOD 2000
#define HASH_INTERVAL 100000

//Mapping from sequential drive states to motor phase outputs
/*
State   L1  L2  L3
0       H   -   L
1       -   H   L
2       L   H   -
3       L   -   H
4       -   L   H
5       H   L   -
6       -   -   -
7       -   -   -
*/
//Drive state to output table
const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};

//Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
//const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed

//Phase lead to make motor spin
const int8_t lead = 2;  //2 for forwards, -2 for backwards

int8_t orState = 0;    //Rotot offset at motor state 0
int8_t rotorState;
int8_t rotorStateOld = 0;

uint8_t maxVelocity = 20;

long long revolutions = 0;
long long unsigned globalKey = 0;
Mutex globalKey_mutex;

Thread motorCtrlT(osPriorityNormal, 1024);

//Status LED
DigitalOut led1(LED1);

//Photointerrupter inputs
InterruptIn I1(I1pin);
InterruptIn I2(I2pin);
InterruptIn I3(I3pin);

//Motor Drive outputs
DigitalOut L1L(L1Lpin);
DigitalOut L1H(L1Hpin);
DigitalOut L2L(L2Lpin);
DigitalOut L2H(L2Hpin);
DigitalOut L3L(L3Lpin);
DigitalOut L3H(L3Hpin);

DigitalOut TP1(TP1pin);
PwmOut MotorPWM(PWMpin);



typedef struct {
    string message;
} mail_t;

Mail<mail_t, 16> mail_box;

RawSerial pc(SERIAL_TX, SERIAL_RX);
Queue<void, 8> inCharQ;
void serialISR(){
    uint8_t newChar = pc.getc();
    inCharQ.put((void*)(size_t)newChar);
}


//Set a given drive state
void motorOut(int8_t driveState){

    //Lookup the output byte from the drive state.
    int8_t driveOut = driveTable[driveState & 0x07];

    //Turn off first
    if (~driveOut & 0x01) L1L = 0;
    if (~driveOut & 0x02) L1H = 1;
    if (~driveOut & 0x04) L2L = 0;
    if (~driveOut & 0x08) L2H = 1;
    if (~driveOut & 0x10) L3L = 0;
    if (~driveOut & 0x20) L3H = 1;

    //Then turn on
    if (driveOut & 0x01) L1L = 1;
    if (driveOut & 0x02) L1H = 0;
    if (driveOut & 0x04) L2L = 1;
    if (driveOut & 0x08) L2H = 0;
    if (driveOut & 0x10) L3L = 1;
    if (driveOut & 0x20) L3H = 0;
}

//Convert photointerrupter inputs to a rotor state
inline int8_t readRotorState(){
    return stateMap[I1 + 2*I2 + 4*I3];
}

//Basic synchronisation routine
int8_t motorHome() {
    //Put the motor in drive state 0 and wait for it to stabilise
    motorOut(0);
    ThisThread::sleep_for(2000);

    //Get the rotor state
    return readRotorState();
}

void sendMessage(string message) {
    mail_t *mail = mail_box.alloc();
    mail->message = message;
    mail_box.put(mail);
}

void motorInterruptHandler() {
    rotorState = readRotorState();
    int8_t direction = rotorState - rotorStateOld;
    motorOut((rotorState-orState+lead+6)%6); //+6 to make sure the remainder is positive
    if (rotorState == 5 && rotorStateOld == 0) {
        revolutions++;
    }
    else if (rotorState == 0 && rotorStateOld == 5) {
        revolutions--;
    }
    //if (intState == 4 && intStateOld == 3) TP1 = !TP1;
    rotorStateOld = rotorState;
    //sendMessage(to_string(revolutions));
    // if (rotorState % 6 == 0) {
    //     sendMessage(to_string(revolutions));
    // }
}

void messageThread(void) {
    while(true) {
        osEvent evt = mail_box.get();
        if (evt.status == osEventMail) {
            mail_t *mail = (mail_t*)evt.value.p;
            pc.printf("%s\n\r", mail->message.c_str());
            mail_box.free(mail);
        }
    }
}

void commandThread(void) {
    pc.attach(&serialISR);
    string command;

    while(1) {
        osEvent newEvent = inCharQ.get();
        char newChar = (char)(size_t)newEvent.value.p;
        char char_array[command.size() + 1];
        char message[64];
        strlcpy(char_array, command.c_str(), command.size() + 1);
        if (newChar == '\r') {
            if (command.size() > 0) {
                // Setting Key for Bitcoin Mining
                if (command[0] == 'K') {
                    uint64_t key;
                    sscanf(char_array, "%*c%16llx", &key);
                    globalKey_mutex.lock();
                    globalKey = key;
                    globalKey_mutex.unlock();
                    snprintf(message, sizeof(message), "Updating hash key to %16llu", key);
                    sendMessage(message);
                }
                // Setting Duty Cycle
                else if (command[0] == 'P') {
                    int key;
                    sscanf(char_array, "%*c%d", &key);
                    if (key < 0 || key > 100) {
                        sendMessage("Invalid pulse width, please specify a value between 0 and 100");
                    } else {
                        MotorPWM.pulsewidth_us(0.01 * key * PWM_PERIOD);
                        snprintf(message, sizeof(message), "Updating pulse width to %d", key);
                        sendMessage(message);
                    }
                }
                // Setting maximum velocity
                else if (command[0] == 'V') {
                    uint8_t velocity;
                    sscanf(char_array, "%*c%8s", &velocity);
                    snprintf(message, sizeof(message), "Setting max velocity to %d", velocity);
                    sendMessage(message);

                }
                else {
                    sendMessage("Invalid command, please try");
                }
            }

            command = "";
        } else {
            command.push_back(newChar);
            sendMessage("Entering command: " + command);
        }
    }

}

void bitcoinThreat(void) {
    uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,
                          0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,
                          0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,
                          0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,
                          0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,
                          0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,
                          0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
                          0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
    uint64_t* key = (uint64_t*)&sequence[48];
    uint64_t* nonce = (uint64_t*)&sequence[56];
    uint8_t hash[32];

    Timer t;
    SHA256 sha256;

    t.start();
    while(true) {
        if (*nonce % HASH_INTERVAL == 0) {
            t.stop();
            float time_elapsed = t.read();
            float hash_speed = HASH_INTERVAL / time_elapsed;
            sendMessage("Hash speed: " + to_string(hash_speed) + " H/s");
            t.reset();
            t.start();
        }
        globalKey_mutex.lock();
        *key = globalKey;
        globalKey_mutex.unlock();
        sha256.computeHash(hash, sequence, 64);
        if (hash[0] == 0 && hash[1] == 0) {
            sendMessage("Found hash with nonce: " + to_string(*nonce));
        }
        *nonce += 1;
    }
}

void motorCtrlTick() {
    motorCtrlT.flags_set(0x1);
}

void motorCtrlFn() {
    Ticker motorCtrlTicker;
    motorCtrlTicker.attach_us(&motorCtrlTick, 100000);

    long long prevRevs;
    uint8_t count;
    Timer t;

    t.start();

    while(1) {
        motorCtrlT.signal_wait(0x1);
        count++;
        if (count == 10) {
            float time_elapsed = t.read();
            t.stop();
            count = 0;
            long velocity = revolutions - prevRevs / time_elapsed;
            sendMessage("Velocity: " + to_string(velocity) + " rev/s");
            prevRevs = revolutions;
            t.reset();
            t.start();
        }
    }
}

//Main
int main() {
    MotorPWM.period_us(PWM_PERIOD);
    MotorPWM.pulsewidth_us(0.5*PWM_PERIOD);
    motorInterruptHandler();

    pc.printf("Hello\n\r");

    //Run the motor synchronisation
    orState = motorHome();
    pc.printf("Rotor origin: %x\n\r",orState);
    //orState is subtracted from future rotor state inputs to align rotor and motor states

    I1.rise(&motorInterruptHandler);
    I2.rise(&motorInterruptHandler);
    I3.rise(&motorInterruptHandler);
    I1.fall(&motorInterruptHandler);
    I2.fall(&motorInterruptHandler);
    I3.fall(&motorInterruptHandler);

    Thread t1(osPriorityLow);
    t1.start(bitcoinThreat);

    Thread t2;
    t2.start(messageThread);

    Thread t3;
    t3.start(commandThread);

    motorCtrlT.start(motorCtrlFn);

    while (1) {
        ThisThread::sleep_for(1000);
    }
}