#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#define MAXSTEPS 1000000
extern uint8_t roundOut;
static uint32_t stepCount;

struct matrix {
    uint16_t width;
    uint16_t height;
    float **data;
};

void haltAndCatchFire(const char* message){
    printf("%s\n", message);
    exit(EXIT_FAILURE);
}


void matrixIsQuadratical(struct matrix *m){
    if ((uint16_t)m->width != (uint16_t)m->height){
        printf("Array size is not quadratical!\n");
        exit(EXIT_FAILURE);
    }
}

void matrixNew(struct matrix *dest, uint16_t size_x, uint16_t size_y){
    /* Set size constants */
    
    dest->width = size_x;
    dest->height = size_y;
    
    dest->data = malloc(size_y * sizeof(intptr_t));
    
    for (int i = 0; i < size_y; i++){
        dest->data[i] = malloc(size_x * sizeof(float));
        
        for (int j = 0; j < size_x; j++) dest->data[i][j] = 0;
    }
    
    if (dest == NULL || dest->data == NULL){
        haltAndCatchFire("Failed to init matrix!");
    }
}

void maxtrixFree(struct matrix *m){
    for (int i = 0; i < m->height; i++){
        free(m->data[i]);
    }
    free(m->data);
    free(m);
}

void matrixNewFromUser(struct matrix *dest, uint16_t size_x, uint16_t size_y){
    matrixNew(dest, size_x, size_y);
    
    printf("Please enter a %dx%d array:\n  [\n", dest->width, dest->height);
    
    float value = 0;
    for (int y = 0; y < size_y; y++){
        printf("     ");
        
        for (int x = 0; x < size_x; x++){
            if (fscanf(stdin, "%f", &value) != 1) haltAndCatchFire("Error while reading input!");
            
            dest->data[y][x] = value;
        }
    }
    printf("  ]\n");
}

void matrixPrint(struct matrix *m, const char* name){
    printf("%s(%d, %d): \n  [\n", name, m->width, m->height);
    
    for (int y = 0; y < m->height; y++){
        printf("     ");
        for (int x = 0; x < m->width; x++){
            if (roundOut) printf("%d ", (int)m->data[y][x]);
            else printf("%f ", m->data[y][x]);
        }
        printf("\n");
    }
    printf("  ]\n\n");
}

void matrixIdentity(struct matrix *m){
    matrixIsQuadratical(m);

    for (int y = 0; y < m->height; y++){
        for (int x = 0; x < m->width; x++){
            /* If x == y, then put a 1, else a 0 */
            m->data[y][x] = x == y ? 1 : 0;
        }
    }
}

void matrixFill(struct matrix *m, float value){
    for (int y = 0; y < m->height; y++){
        for (int x = 0; x < m->width; x++){
            m->data[y][x] = value;
        }
    }
}


float matrixDet(struct matrix *m){
    matrixIsQuadratical(m);
    
    /*
    float* workMatrix = malloc((size_x + 1) * sizeof(float));
    for (int i = 1; i <= size_x; i++) workMatrix[i] = i % 2 ? matrix[1][i] : -matrix[1][i];
    */
    
    float result = 0;
    for (int column = 0; column < 3; column++){
        float val = (m->data[1][((column + 1) % 3)] * m->data[2][((column + 2 ) % 3)]) - (m->data[2][((column + 1) % 3)] * m->data[1][((column + 2 ) % 3)]);

        result += (column != 1 ? m->data[0][column] : -m->data[0][column]) * (column != 1 ? val : -val);
    }
    
    return result;
}
/*

float matrixDet(const float *matrix, uint16_t start_x, uint16_t start_y, int16_t size){
    float val;
    for (int y = start_y; y < start_y + size; y++){
        for (int x = start_x; x < start_x + size; x++){
            
            
            if (stepCount > MAXSTEPS){
                printf("Exceeded maximum calculation iteration safeguard of > %d\n", MAXSTEPS);
                exit(EXIT_FAILURE);
            }
            stepCount++;
        }
    }
    
    return val;
}*/


void matrixRound(struct matrix *m){
    for (int y = 0; y < m->height; y++){
        for (int x = 0; x < m->width; x++){
            m->data[y][x] = roundf(m->data[y][x]);
        }
    }
}

void matrixAdd(struct matrix *a, struct matrix *b, struct matrix *dest){
    if (a->width != b->width || a->height != b->height){
        printf("Matricies are not equal in size!");
        exit(EXIT_FAILURE);
    } 
    
    //matrixFree(dest);
    matrixNew(dest, a->width, a->height);
    
    for (int y = 0; y < a->height; y++){
        for (int x = 0; x < a->width; x++){
            dest->data[y][x] = a->data[y][x] + b->data[y][x];
        }
    }
}

void matrixMultiply(struct matrix *a, struct matrix *b, struct matrix *dest){
    if (a->width != b->height) haltAndCatchFire("Matricies are not compatible for multiplication!");
    
    matrixNew(dest, b->width, a->height);
    
    for (int y = 0; y < a->height; y++){
        for (int x = 0; x < b->width; x++){
            for (int i = 0; i < a->width; i++){
                dest->data[y][x] += (a->data[y][i] * b->data[i][x]);
            }
        }
    }
}

void matrixMultiplyByNumber(struct matrix *m, float value){
    for (int y = 0; y < m->height; y++){
        for (int x = 0; x < m->width; x++){
            m->data[y][x] = m->data[y][x] * value;
        }
    }
}

void matrixPow(float *matrix, float value){
    
}

void matrixTranspose(float *matrix){
    
}