import os
import math
from opendm import log
from opendm import location
from pyproj import CRS

class GeoFile:
    def __init__(self, geo_path):
        self.geo_path = geo_path
        self.entries = {}
        self.srs = None

        with open(self.geo_path, 'r') as f:
            contents = f.read().strip()

        lines = list(map(str.strip, contents.split('\n')))
        if lines:
            self.raw_srs = lines[0]  # SRS
            self.srs = location.parse_srs_header(self.raw_srs)
            longlat = CRS.from_epsg("4326")

            for line in lines[1:]:
                if line != "" and line[0] != "#":
                    parts = line.split()
                    if len(parts) >= 3:
                        i = 3
                        filename = parts[0]
                        x, y = [float(p) for p in parts[1:3]]
                        z = float(parts[3]) if len(parts) >= 4 else None

                        # Always convert coordinates to WGS84
                        if z is not None:
                            x, y, z = location.transform3(self.srs, longlat, x, y, z)
                        else:
                            x, y = location.transform2(self.srs, longlat, x, y)

                        yaw = pitch = roll = None

                        if len(parts) >= 7:
                            yaw, pitch, roll = [float(p) for p in parts[4:7]]
                            if math.isnan(yaw) or math.isnan(pitch) or math.isnan(roll):
                                yaw = pitch = roll = None
                            i = 7

                        horizontal_accuracy = vertical_accuracy = None
                        if len(parts) >= 9:
                            horizontal_accuracy,vertical_accuracy = [float(p) for p in parts[7:9]]
                            i = 9

                        extras = " ".join(parts[i:])
                        self.entries[filename] = GeoEntry(filename, x, y, z,
                                                        yaw, pitch, roll,
                                                        horizontal_accuracy, vertical_accuracy, 
                                                        extras)
                    else:
                        log.ODM_WARNING("Malformed geo line: %s" % line)
    
    def get_entry(self, filename):
        return self.entries.get(filename)


class GeoEntry:
    def __init__(self, filename, x, y, z, yaw=None, pitch=None, roll=None, horizontal_accuracy=None, vertical_accuracy=None, extras=None):
        self.filename = filename
        self.x = x
        self.y = y
        self.z = z
        self.yaw = yaw
        self.pitch = pitch
        self.roll = roll
        self.horizontal_accuracy = horizontal_accuracy
        self.vertical_accuracy = vertical_accuracy
        self.extras = extras

    def __str__(self):
        return "{} ({} {} {}) ({} {} {}) ({} {}) {}".format(self.filename, 
                                             self.x, self.y, self.z,
                                             self.yaw, self.pitch, self.roll,
                                             self.horizontal_accuracy, self.vertical_accuracy,
                                             self.extras).rstrip()
    
    def position_string(self):
        return "{} {} {}".format(self.x, self.y, self.z)