#include <stdlib.h>
#include <switch.h>
#include <math.h>
#include "map.h"
#include "save.h"
#include "ui.h"
#include "data.h"

u32 mapPinIconCount = 0;
u32 mapPinLocCount = 0;
u32 map_pin_count = 0;
Point PlayerLocation;


//2d seems to work better than 3d
int compDistance (const void* elem1, const void* elem2) 
{
    Point f = *((Point*)elem1);
    Point s = *((Point*)elem2);
    double f_distance = sqrt(pow(f.x-PlayerLocation.x,2)+pow(f.z-PlayerLocation.z,2));
    double s_distance = sqrt(pow(s.x-PlayerLocation.x,2)+pow(s.z-PlayerLocation.z,2));
    if (f_distance > s_distance) return  1;
    if (f_distance < s_distance) return -1;
    return 0;
}

/*void addMapPin(u32 icon, Point location) {
    u32 pin_hash = 0x9383490e;
    u32 pin_offset = _searchHash(pin_hash) - 4;
    u32 pin_maxlength = ((GAMEDATASAV.length - pin_offset) / 8);
    u32 pin_total = 0;
    for ( u32 i = 0; i < pin_maxlength; i++) {
        u32 pin_base = pin_offset + (8*i);
        u32 pin_header = readU32FromAddr(pin_base);
        u32 pin_value = readU32FromAddr(pin_base + 4);
        
        //printMessage2("%d: hash %x offset %x base %x header %x value: %x", i, pin_hash, pin_offset, pin_base, pin_header, pin_value);
        if (pin_header != pin_hash) { break; }
        
        if (pin_value == 0xffffffff) {
            //printMessage2("%d: hash %x offset %x base %x header %x value: %x", i, pin_hash, pin_offset, pin_base, pin_header, pin_value);
			writeU32ToAddr(pin_base + 4, icon);
            pin_counter++;
			break;
		}

        if (++pin_total >= 100) { break; }
    }


    u32 pinloc_hash = 0xea9def3f;
    u32 pinloc_offset = _searchHash(pinloc_hash) - 4;
    u32 pinloc_maxlength = ((GAMEDATASAV.length - pinloc_offset) / 8);
    u32 pinloc_total = 0;
    for ( u32 i = 0; i < pinloc_maxlength; i++) {
        if (i%3 != 0) { continue; }
        u32 pinloc_base = pinloc_offset + (8*i);
        u32 pinloc_header = readU32FromAddr(pinloc_base);
        
        //printMessage2("hash 0x%x offset 0x%x base 0x%x header 0x%x x: %f y: %f z:%f\n", pinloc_hash, pinloc_offset, pinloc_base, pinloc_header, pinloc_value_x, pinloc_value_y, pinloc_value_z);
        if (pinloc_header != pinloc_hash) { break; }

        float pinloc_value = readF32FromAddr(pinloc_base + 4);

        if (pinloc_value == -100000) {
            //printMessage2("hash 0x%x offset 0x%x base 0x%x header 0x%x x: %f y: %f z:%f\n", pinloc_hash, pinloc_offset, pinloc_base, pinloc_header, pinloc_value_x, pinloc_value_y, pinloc_value_z);
			writeF32ToAddr(pinloc_base + 4, location.x);
            writeF32ToAddr(pinloc_base + 12, location.y);
            writeF32ToAddr(pinloc_base + 20, location.z);
            pinloc_counter++;
			break;
		}

        if (++pinloc_total >= 100) { break; }
    }
}*/

void iterateMapPinIcons(func_t_0 iterFunc, u32 icon) {
    mapPinIconCount = 0;
    u32 pin_hash = 0x9383490e;
    u32 pin_offset = _searchHash(pin_hash) - 4;
    u32 pin_maxlength = ((getSaveSize() - pin_offset) / 8);
    for ( u32 i = 0; i < pin_maxlength; i++) {
        u32 pin_base = pin_offset + (8*i);
        u32 pin_header = readU32FromAddr(pin_base);
        u32 pin_value = readU32FromAddr(pin_base + 4);
        
        if (pin_header != pin_hash) { break; }
        
        if (!iterFunc(pin_base+4, pin_value, icon)) {
			break;
		}
    }
}

void iterateMapPinLocs(func_t_1 iterFunc, Point location) {
    mapPinLocCount = 0;
    u32 pinloc_hash = 0xea9def3f;
    u32 pinloc_offset = _searchHash(pinloc_hash) - 4;
    u32 pinloc_maxlength = ((getSaveSize() - pinloc_offset) / 24);
    for ( u32 i = 0; i < pinloc_maxlength; i++) {
        u32 pinloc_base = pinloc_offset + (24*i);
        u32 pinloc_header = readU32FromAddr(pinloc_base);
        float pinloc_value_x = readF32FromAddr(pinloc_base + 4);
        float pinloc_value_y = readF32FromAddr(pinloc_base + 12);
        float pinloc_value_z = readF32FromAddr(pinloc_base + 20);
        
        if (pinloc_header != pinloc_hash) { break; }

        if (!iterFunc(pinloc_base+4, pinloc_value_x, pinloc_value_y, pinloc_value_z, location)) {
			break;
		}
    }
}

void sortKoroksByDistance() 
{
    /*Point korokLocations[900];
    for (int i = 0 ;i < 900; i++) {
        korokLocations[i] = KOROKS[i];
    }*/
    
    qsort(KOROKS, 900, sizeof(Point), compDistance);

    u32 added_pins = 0;
    u32 max_pins_to_add = 100 - map_pin_count;
    for (int i = 0; i < 900 && added_pins < max_pins_to_add; i++) {
        if (isKorokCompletedOrMarked(KOROKS[i])) { continue; }

        if (!addPinToMap(MAP_PIN_LEAF, KOROKS[i])) { break; }
        added_pins++;
        /*printMessage("%d (0x%x): %f, %f ,%f (dist: %f)\n", added_pins, korokLocations[i].hash, 
            korokLocations[i].x, korokLocations[i].y, korokLocations[i].z,
            sqrt(pow(korokLocations[i].x-PlayerLocation.x,2)+pow(korokLocations[i].y-PlayerLocation.y,2)+pow(korokLocations[i].z-PlayerLocation.z,2)));*/
    }
    printMessage("%d pins added to map.", added_pins);
}

bool isKorokCompletedOrMarked(Point korok) {
    //check if korok is marked on map already
    if (isLocationInPinLoc(korok)) { return true; }
    //check if korok is marked completed in game save
    return (readU32FromHash(korok.hash) == 1);
}

void eraseCompletedKorokPins() {

}

bool addPinToMap(u32 pin_icon, Point location) {
    iterateMapPinIcons(&_addMapPinIcon, pin_icon);
    iterateMapPinLocs(&_addMapPinLoc, location);
    if (mapPinIconCount != mapPinLocCount) {
        printMessage("Mismatch: Added %d map icons, but %d map locations.", mapPinIconCount, mapPinLocCount);
        return false;
    }
    map_pin_count = map_pin_count + mapPinIconCount;
    return (mapPinIconCount == 1);
}

bool _addMapPinIcon(u32 offset, u32 value, u32 icon) {
    if (value == MAP_PIN_EMPTY) {
        writeU32ToAddr(offset, icon);
        mapPinIconCount++;
        return false;
    }
    return true;
}

bool _addMapPinLoc(u32 offset, float value_x, float value_y, float value_z, Point location) {
    if (value_x == -100000 && value_y == 0 && value_z == 0) {
        writeF32ToAddr(offset, location.x);
        writeF32ToAddr(offset+8, location.y);
        writeF32ToAddr(offset+16, location.z);
        mapPinLocCount++;
        return false;
    }
    return true;
}

bool isLocationInPinLoc(Point location) {
    iterateMapPinLocs(&_isLocationInPinLoc, location);
    return mapPinLocCount == 1;
}

bool _isLocationInPinLoc(u32 offset, float value_x, float value_y, float value_z, Point location) {
    if (value_x == -100000 && value_y == 0 && value_z == 0) {
        return false;
    }

    if (value_x == location.x && value_y == location.y && value_z == location.z) {
        mapPinLocCount++;
        return false;
    }
    return true;
}

void loadPlayerLocation() {
    PlayerLocation = (Point){.hash = 0, 
    .x = readF32FromHash(HASHES.PLAYER_POSITION, 0),
    .y = readF32FromHash(HASHES.PLAYER_POSITION, 8),
    .z = readF32FromHash(HASHES.PLAYER_POSITION, 16)};
    printMessage("player loc is x: %f, y: %f, z: %f.", PlayerLocation.x, PlayerLocation.y, PlayerLocation.z);
}

void countMapPins() {
    map_pin_count = 0;
    iterateMapPinIcons(&_loadMapPinIcons, 0);
    iterateMapPinLocs(&_loadMapPinLocs, (Point){});
    if (mapPinIconCount != mapPinLocCount) {
        printMessage("Mismatch: Counted %d map icons, but have %d map locations.", mapPinIconCount, mapPinLocCount);
    }
    printMessage("number of map pins: %d.", map_pin_count);
}

bool _loadMapPinIcons(u32 offset, u32 value, u32 icon) {
    if (value == MAP_PIN_EMPTY) {
        return false;
    }
    if (value <= 0x23 && value >= 0x1b) {
        mapPinIconCount++;
        map_pin_count++;
    } else {
        printMessage("Invalid map pin icon 0x%x @ offset 0x%x", value, offset);
    }
    return true;
}

bool _loadMapPinLocs(u32 offset, float value_x, float value_y, float value_z, Point location) {
    if (value_x == -100000 && value_y == 0 && value_z == 0) {
        return false;
    }
    mapPinLocCount++;
    return true;
}

void eraseMapPins() {
    iterateMapPinIcons(&_eraseMapPinIcons, 0);
    iterateMapPinLocs(&_eraseMapPinLocs, (Point){.hash=0});
    map_pin_count = 0;
    printMessage("%d pin icons erased, %d pin locations erased.", mapPinIconCount, mapPinLocCount);
}

bool _eraseMapPinIcons(u32 offset, u32 value, u32 icon) {
    if (value != MAP_PIN_EMPTY) {
        //if we have a hash set, we're looking for a specific icon to erase. if this isn't it, return true to iterator to continue
        if (icon != 0 && icon != value) { return true; }
        writeU32ToAddr(offset, MAP_PIN_EMPTY);
        mapPinIconCount++;
    }
    return true;
}

bool _eraseMapPinLocs(u32 offset, float value_x, float value_y, float value_z, Point location) {
    if (value_x != -100000 && value_y != 0 && value_z != 0) {
        //if we have a hash set, we're looking for a specific location to erase. if this isn't it, return true to iterator to continue
        if (location.hash != 0 && (location.x != value_x || location.y != value_y || location.z != value_z)) { return true; }
        writeF32ToAddr(offset, -100000);
        writeF32ToAddr(offset+8, 0);
        writeF32ToAddr(offset+16, 0);
        mapPinLocCount++;
    }
    return true;
}

void convertInvalidMapPinIcons() {
    iterateMapPinIcons(&_convertInvalidMapPinIcons, 0x1b);
    map_pin_count = map_pin_count + mapPinIconCount;
    printMessage("%d invalid map pin icons converted.", mapPinIconCount);
    countMapPins();
}

bool _convertInvalidMapPinIcons(u32 offset, u32 value, u32 icon) {
    if (value == MAP_PIN_EMPTY) {
        return false;
    }
    if (value > 0x23 || value < 0x1b) {
        writeU32ToAddr(offset, icon);
        mapPinIconCount++;
    }
    return true;
}