{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "import sys\n",
    "from functools import singledispatch\n",
    "import warnings\n",
    "\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import math\n",
    "import pyproj\n",
    "\n",
    "\n",
    "# Path to local plio if wanted\n",
    "sys.path.insert(0, \"/path/to/plio\")\n",
    "\n",
    "from plio.examples import get_path\n",
    "from plio.io.io_bae import read_gpf, read_ipf\n",
    "import plio.io.io_controlnetwork as cn\n",
    "import plio.io.isis_serial_number as sn"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Reads a .atf file and outputs all of the \n",
    "# .ipf, .gpf, .sup, .prj, and path to locate the \n",
    "# .apf file (should be the same as all others) \n",
    "def read_atf(atf_file):\n",
    "    with open(atf_file) as f:\n",
    "\n",
    "        files = []\n",
    "        ipf = []\n",
    "        sup = []\n",
    "        files_dict = []\n",
    "        \n",
    "        # Grabs every PRJ, GPF, SUP, and IPF image from the ATF file\n",
    "        for line in f:\n",
    "            if line[-4:-1] == 'prj' or line[-4:-1] == 'gpf' or line[-4:-1] == 'sup' or line[-4:-1] == 'ipf' or line[-4:-1] == 'atf':\n",
    "                files.append(line)\n",
    "        \n",
    "        files = np.array(files)\n",
    "        \n",
    "        # Creates appropriate arrays for certain files in the right format\n",
    "        for file in files:\n",
    "            file = file.strip()\n",
    "            file = file.split(' ')\n",
    "\n",
    "            # Grabs all the IPF files\n",
    "            if file[1].endswith('.ipf'):\n",
    "                ipf.append(file[1])\n",
    "\n",
    "            # Grabs all the SUP files\n",
    "            if file[1].endswith('.sup'):\n",
    "                sup.append(file[1])\n",
    "\n",
    "            files_dict.append(file)\n",
    "\n",
    "        # Creates a dict out of file lists for GPF, PRJ, IPF, and ATF\n",
    "        files_dict = (dict(files_dict))\n",
    "        \n",
    "        # Sets the value of IMAGE_IPF to all IPF images\n",
    "        files_dict['IMAGE_IPF'] = ipf\n",
    "        \n",
    "        # Sets the value of IMAGE_SUP to all SUP images\n",
    "        files_dict['IMAGE_SUP'] = sup\n",
    "        \n",
    "        # Sets the value of PATH to the path of the ATF file\n",
    "        files_dict['PATH'] = os.path.dirname(os.path.abspath(atf_file))\n",
    "        \n",
    "        return files_dict\n",
    "\n",
    "# converts columns l. and s. to isis\n",
    "def line_sample_size(record, path):\n",
    "    with open(os.path.join(path, record['ipf_file'] + '.sup')) as f:\n",
    "        for i, line in enumerate(f):\n",
    "            if i == 2:\n",
    "                img_index = line.split('\\\\')\n",
    "                img_index = img_index[-1].strip()\n",
    "                img_index = img_index.split('.')[0]\n",
    "                \n",
    "            if i == 3:\n",
    "                line_size = line.split(' ')\n",
    "                line_size = line_size[-1].strip()\n",
    "                assert int(line_size) > 0, \"Line number {} from {} is a negative number: Invalid Data\".format(line_size, record['ipf_file'])\n",
    "                \n",
    "            if i == 4:\n",
    "                sample_size = line.split(' ')\n",
    "                sample_size = sample_size[-1].strip()\n",
    "                assert int(sample_size) > 0, \"Sample number {} from {} is a negative number: Invalid Data\".format(sample_size, record['ipf_file'])\n",
    "                break\n",
    "                \n",
    "        \n",
    "        line_size = int(line_size)/2.0 + record['l.'] + 1\n",
    "        sample_size = int(sample_size)/2.0 + record['s.'] + 1\n",
    "        return sample_size, line_size, img_index\n",
    "    \n",
    "# converts known to ISIS keywords\n",
    "def known(record):\n",
    "    if record['known'] == 0:\n",
    "        return 'Free'\n",
    "    \n",
    "    elif record['known'] == 1 or record['known'] == 2 or record['known'] == 3:\n",
    "        return 'Constrained'\n",
    "    \n",
    "# converts +/- 180 system to 0 - 360 system\n",
    "def to_360(num):\n",
    "    return num % 360\n",
    "\n",
    "# ocentric to ographic latitudes\n",
    "def oc2og(dlat, dMajorRadius, dMinorRadius):\n",
    "    try:    \n",
    "        dlat = math.radians(dlat)\n",
    "        dlat = math.atan(((dMajorRadius / dMinorRadius)**2) * (math.tan(dlat)))\n",
    "        dlat = math.degrees(dlat)\n",
    "    except:\n",
    "        print (\"Error in oc2og conversion\")\n",
    "    return dlat\n",
    "\n",
    "# ographic to ocentric latitudes\n",
    "def og2oc(dlat, dMajorRadius, dMinorRadius):\n",
    "    try:\n",
    "        dlat = math.radians(dlat)\n",
    "        dlat = math.atan((math.tan(dlat) / ((dMajorRadius / dMinorRadius)**2)))\n",
    "        dlat = math.degrees(dlat)\n",
    "    except:\n",
    "        print (\"Error in og2oc conversion\")\n",
    "    return dlat\n",
    "\n",
    "# gets eRadius and pRadius from a .prj file\n",
    "def get_axis(file):\n",
    "    with open(file) as f:\n",
    "        from collections import defaultdict\n",
    "\n",
    "        files = defaultdict(list)\n",
    "        \n",
    "        for line in f:\n",
    "            \n",
    "            ext = line.strip().split(' ')\n",
    "            files[ext[0]].append(ext[-1])\n",
    "            \n",
    "        eRadius = float(files['A_EARTH'][0])\n",
    "        pRadius = eRadius * (1 - float(files['E_EARTH'][0]))\n",
    "        \n",
    "        return eRadius, pRadius\n",
    "    \n",
    "# function to convert lat_Y_North to ISIS_lat\n",
    "def lat_ISIS_coord(record, semi_major, semi_minor):\n",
    "    ocentric_coord = og2oc(record['lat_Y_North'], semi_major, semi_minor)\n",
    "    coord_360 = to_360(ocentric_coord)\n",
    "    return coord_360\n",
    "\n",
    "# function to convert long_X_East to ISIS_lon\n",
    "def lon_ISIS_coord(record, semi_major, semi_minor):\n",
    "    ocentric_coord = og2oc(record['long_X_East'], semi_major, semi_minor)\n",
    "    coord_360 = to_360(ocentric_coord)\n",
    "    return coord_360\n",
    "\n",
    "def body_fix(record, semi_major, semi_minor, inverse=False):\n",
    "    \"\"\"\n",
    "    Parameters\n",
    "    ----------\n",
    "    record : ndarray\n",
    "             (n,3) where columns are x, y, height or lon, lat, alt\n",
    "    \"\"\"\n",
    "    \n",
    "    ecef = pyproj.Proj(proj='geocent', a=semi_major, b=semi_minor)\n",
    "    lla = pyproj.Proj(proj='latlon', a=semi_major, b=semi_minor)\n",
    "    \n",
    "    if inverse:\n",
    "        lon, lat, height = pyproj.transform(ecef, lla, record[0], record[1], record[2])\n",
    "        return lon, lat, height\n",
    "    else:\n",
    "        y, x, z = pyproj.transform(lla, ecef, record[0], record[1], record[2])\n",
    "        return y, x, z\n",
    "\n",
    "def ignore_toggle(record):\n",
    "    if record['stat'] == 0:\n",
    "        return True\n",
    "    else:\n",
    "        return False\n",
    "\n",
    "# TODO: Does isis cnet need a convariance matrix for sigmas? Even with a static matrix of 1,1,1,1 \n",
    "def compute_sigma_covariance_matrix(lat, lon, rad, latsigma, lonsigma, radsigma, semimajor_axis):\n",
    "    \n",
    "    \"\"\"\n",
    "    Given geospatial coordinates, desired accuracy sigmas, and an equitorial radius, compute a 2x3\n",
    "    sigma covariange matrix.\n",
    "    Parameters\n",
    "    ----------\n",
    "    lat : float\n",
    "          A point's latitude in degrees\n",
    "    lon : float\n",
    "          A point's longitude in degrees\n",
    "    rad : float\n",
    "          The radius (z-value) of the point in meters\n",
    "    latsigma : float\n",
    "               The desired latitude accuracy in meters (Default 10.0)\n",
    "    lonsigma : float\n",
    "               The desired longitude accuracy in meters (Default 10.0)\n",
    "    radsigma : float\n",
    "               The desired radius accuracy in meters (Defualt: 15.0)\n",
    "    semimajor_axis : float\n",
    "                     The semi-major or equitorial radius in meters (Default: 1737400.0 - Moon)\n",
    "    Returns\n",
    "    -------\n",
    "    rectcov : ndarray\n",
    "              (2,3) covariance matrix\n",
    "    \"\"\"\n",
    "    \n",
    "    lat = math.radians(lat)\n",
    "    lon = math.radians(lon)\n",
    "    \n",
    "    # SetSphericalSigmasDistance\n",
    "    scaled_lat_sigma = latsigma / semimajor_axis\n",
    "\n",
    "    # This is specific to each lon.\n",
    "    scaled_lon_sigma = lonsigma * math.cos(lat) / semimajor_axis\n",
    "    \n",
    "    # SetSphericalSigmas\n",
    "    cov = np.eye(3,3)\n",
    "    cov[0,0] = scaled_lat_sigma ** 2\n",
    "    cov[1,1] = scaled_lon_sigma ** 2\n",
    "    cov[2,2] = radsigma ** 2\n",
    "    \n",
    "    # Approximate the Jacobian\n",
    "    j = np.zeros((3,3))\n",
    "    cosphi = math.cos(lat)\n",
    "    sinphi = math.sin(lat)\n",
    "    coslambda = math.cos(lon)\n",
    "    sinlambda = math.sin(lon)\n",
    "    rcosphi = rad * cosphi\n",
    "    rsinphi = rad * sinphi\n",
    "    j[0,0] = -rsinphi * coslambda\n",
    "    j[0,1] = -rcosphi * sinlambda\n",
    "    j[0,2] = cosphi * coslambda\n",
    "    j[1,0] = -rsinphi * sinlambda\n",
    "    j[1,1] = rcosphi * coslambda\n",
    "    j[1,2] = cosphi * sinlambda\n",
    "    j[2,0] = rcosphi\n",
    "    j[2,1] = 0.\n",
    "    j[2,2] = sinphi\n",
    "    mat = j.dot(cov)\n",
    "    mat = mat.dot(j.T)\n",
    "    rectcov = np.zeros((2,3))\n",
    "    rectcov[0,0] = mat[0,0]\n",
    "    rectcov[0,1] = mat[0,1]\n",
    "    rectcov[0,2] = mat[0,2]\n",
    "    rectcov[1,0] = mat[1,1]\n",
    "    rectcov[1,1] = mat[1,2]\n",
    "    rectcov[1,2] = mat[2,2]\n",
    "    \n",
    "    return rectcov\n",
    "#     return np.array([[1.0, 1.0, 1.0], [1.0, 1.0, 1.0]])\n",
    "\n",
    "\n",
    "def compute_cov_matrix(record, semimajor_axis):\n",
    "    cov_matrix = compute_sigma_covariance_matrix(record['lat_Y_North'], record['long_X_East'], record['ht'], record['sig0'], record['sig1'], record['sig2'], semimajor_axis)\n",
    "    return cov_matrix.ravel().tolist()\n",
    "\n",
    "# applys transformations to columns\n",
    "def apply_transformations(atf_dict, df):\n",
    "    prj_file = os.path.join(atf_dict['PATH'], atf_dict['PROJECT'].split('\\\\')[-1])\n",
    "    \n",
    "    eRadius, pRadius = get_axis(prj_file)\n",
    "    \n",
    "    lla = np.array([[df['long_X_East']], [df['lat_Y_North']], [df['ht']]])\n",
    "    \n",
    "    ecef = body_fix(lla, semi_major = eRadius, semi_minor = pRadius, inverse=False)\n",
    "    \n",
    "    df['s.'], df['l.'], df['image_index'] = (zip(*df.apply(line_sample_size, path = atf_dict['PATH'], axis=1)))\n",
    "    df['known'] = df.apply(known, axis=1)\n",
    "    df['long_X_East'] = ecef[0][0]\n",
    "    df['lat_Y_North'] = ecef[1][0]\n",
    "    df['ht'] = ecef[2][0]    \n",
    "    df['aprioriCovar'] = df.apply(compute_cov_matrix, semimajor_axis = eRadius, axis=1)\n",
    "    df['ignore'] = df.apply(ignore_toggle, axis=1)\n",
    "        \n",
    "def socet2isis(prj_file):\n",
    "    # Read in and setup the atf dict of information\n",
    "    atf_dict = read_atf(prj_file)\n",
    "    \n",
    "    # Get the gpf and ipf files using atf dict\n",
    "    gpf_file = os.path.join(atf_dict['PATH'], atf_dict['GP_FILE']);\n",
    "    ipf_list = [os.path.join(atf_dict['PATH'], i) for i in atf_dict['IMAGE_IPF']]\n",
    "    \n",
    "    # Read in the gpf file and ipf file(s) into seperate dataframes\n",
    "    gpf_df = read_gpf(gpf_file)\n",
    "    ipf_df = read_ipf(ipf_list)\n",
    "\n",
    "    # Check for differences between point ids using each dataframes\n",
    "    # point ids as a reference\n",
    "    gpf_pt_idx = pd.Index(pd.unique(gpf_df['point_id']))\n",
    "    ipf_pt_idx = pd.Index(pd.unique(ipf_df['pt_id']))\n",
    "\n",
    "    point_diff = ipf_pt_idx.difference(gpf_pt_idx)\n",
    "\n",
    "    if len(point_diff) != 0:\n",
    "        warnings.warn(\"The following points found in ipf files missing from gpf file: \\n\\n{}. \\\n",
    "                      \\n\\nContinuing, but these points will be missing from the control network\".format(list(point_diff)))\n",
    "        \n",
    "    # Merge the two dataframes on their point id columns\n",
    "    socet_df = ipf_df.merge(gpf_df, left_on='pt_id', right_on='point_id')\n",
    "    \n",
    "    # Apply the transformations\n",
    "    apply_transformations(atf_dict, socet_df)\n",
    "    \n",
    "    # Define column remap for socet dataframe\n",
    "    column_remap = {'l.': 'y', 's.': 'x',\n",
    "                    'res_l': 'lineResidual', 'res_s': 'sampleResidual', 'known': 'Type',\n",
    "                    'lat_Y_North': 'AprioriY', 'long_X_East': 'AprioriX', 'ht': 'AprioriZ',\n",
    "                    'sig0': 'AprioriLatitudeSigma', 'sig1': 'AprioriLongitudeSigma', 'sig2': 'AprioriRadiusSigma',\n",
    "                    'sig_l': 'linesigma', 'sig_s': 'samplesigma'}\n",
    "    \n",
    "    # Rename the columns using the column remap above\n",
    "    socet_df.rename(columns = column_remap, inplace=True)\n",
    "    \n",
    "    # Return the socet dataframe to be converted to a control net\n",
    "    return socet_df\n",
    "\n",
    "# creates a dict of serial numbers with the cub being the key\n",
    "def serial_numbers(images, path, extension):\n",
    "    serial_dict = dict()\n",
    "    \n",
    "    for image in images:\n",
    "        snum = sn.generate_serial_number(os.path.join(path, image + extension))\n",
    "        snum = snum.replace('Mars_Reconnaissance_Orbiter', 'MRO')\n",
    "        serial_dict[image] = snum\n",
    "    return serial_dict\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# path = '/home/tthatcher/Desktop/Projects/plio_imgs/quest_imgs/'\n",
    "# extension = '.lev1.cub'\n",
    "# atf_file = ('/home/tthatcher/Desktop/Projects/plio_imgs/quest_imgs/CTX_Athabasca_Middle_step0.atf')\n",
    "\n",
    "# Setup stuffs for the cub information namely the path and extension\n",
    "path = '/path/where/cub/files/are/'\n",
    "\n",
    "# Extension of your cub files\n",
    "extension = '.8bit.cub'\n",
    "\n",
    "# Path to atf file\n",
    "atf_file = ('/path/to/atf/file')\n",
    "\n",
    "socet_df = socet2isis(atf_file)\n",
    "\n",
    "images = pd.unique(socet_df['ipf_file'])\n",
    "\n",
    "serial_dict = serial_numbers(images, path, extension)\n",
    "\n",
    "# creates the control network\n",
    "cn.to_isis('/path/you/want/the/cnet/to/be/in/cn.net', socet_df, serial_dict)\n",
    "\n",
    "# cn.to_isis('/home/tthatcher/Desktop/Projects/plio_imgs/quest_imgs/cn.net', socet_df, serial_dict)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}