#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Mon Nov 22 01:14:48 2021

@author: andrea
"""
import numpy as np
from scipy.optimize import least_squares

def model(x, c1, c2):
    return (x[0] - c1)** 2 + (x[1] - c2)** 2

c1 = np.array([-1,1,0])
c2 = np.array([0,0,1])
d = np.array([1,1,2])

def fun(x):
    return model(x, c1, c2) - d**2
    
def jac(x):
    J = np.empty((c1.size, x.size))
    J[:, 0] = -4*(x[0] - c1)*model(x, c1, c2)
    J[:, 1] = -4*(x[1] - c2)*model(x, c1, c2)
    print("jac is ", jac)
    return J

x0 = np.array([2.1, -3.9])

res = least_squares(fun, x0)

print(res)